diff --git a/robowflex_dart/include/robowflex_dart/gui.h b/robowflex_dart/include/robowflex_dart/gui.h index 686e899e8..07ace2ab5 100644 --- a/robowflex_dart/include/robowflex_dart/gui.h +++ b/robowflex_dart/include/robowflex_dart/gui.h @@ -91,8 +91,8 @@ namespace robowflex InteractiveCallback callback{}; ///< Callback function on motion. dart::dynamics::Frame *parent{dart::dynamics::Frame::World()}; ///< Parent frame. double size{0.2}; ///< Size of marker. - double thickness{2.}; ///< Thickness of marker arrows. - bool obstructable{false}; ///< Is this frame obstructable? + double thickness{2.}; ///< Thickness of marker arrows. + bool obstructable{false}; ///< Is this frame obstructable? bool linear[3]{true, true, true}; ///< Linear position controls enabled. bool rotation[3]{true, true, true}; ///< Rotation ring controls enabled. @@ -203,14 +203,14 @@ namespace robowflex const WorldPtr &getWorldConst() const; private: - WorldPtr world_; ///< World to visualize. - WindowWidgetPtr widget_; ///< IMGUI widget. - std::vector widgets_; ///< Other widgets; + WorldPtr world_; ///< World to visualize. + WindowWidgetPtr widget_; ///< IMGUI widget. + std::vector widgets_; ///< Other widgets; std::shared_ptr animation_{nullptr}; ///< Animation thread. - ::osg::ref_ptr node_; ///< OSG Node. - Viewer viewer_; ///< Viewer + ::osg::ref_ptr node_; ///< OSG Node. + Viewer viewer_; ///< Viewer }; /** \brief Abstract class for IMGUI Widget. @@ -531,21 +531,21 @@ namespace robowflex const double rotation_alpha_{0.6}; ///< Rotation bound alpha. const double rotation_width_{0.05}; ///< Rotation bound width. - const float max_position_{5.0f}; ///< Max position value. - const float drag_step_{0.01f}; ///< Slider drag amount. + const float max_position_{5.0f}; ///< Max position value. + const float drag_step_{0.01f}; ///< Slider drag amount. - float position_[3]; ///< GUI frame position. - float rotation_[3]; ///< GUI frame rotation. + float position_[3]; ///< GUI frame position. + float rotation_[3]; ///< GUI frame rotation. - float xp_[2]; ///< GUI X position bounds. - float yp_[2]; ///< GUI Y position bounds. - float zp_[2]; ///< GUI Z position bounds. + float xp_[2]; ///< GUI X position bounds. + float yp_[2]; ///< GUI Y position bounds. + float zp_[2]; ///< GUI Z position bounds. - float xr_[2]; ///< GUI X orientation bounds. - float yr_[2]; ///< GUI Y orientation bounds. - float zr_[2]; ///< GUI Z orientation bounds. + float xr_[2]; ///< GUI X orientation bounds. + float yr_[2]; ///< GUI Y orientation bounds. + float zr_[2]; ///< GUI Z orientation bounds. - float inner_radius{0.2}; ///< GUI Rotation bound inner radius. + float inner_radius{0.2}; ///< GUI Rotation bound inner radius. /** \} */ }; @@ -587,16 +587,16 @@ namespace robowflex const int max_iteration_{200}; ///< Max iteration value. const float drag_tolerance_{0.01f}; ///< Slider drag for tolerance. - bool track_tsr_{false}; ///< Track the TSR by solving IK. - bool use_gradient_{false}; ///< Use gradient solving instead of built-in. - bool need_solve_{false}; ///< A solve is requested. + bool track_tsr_{false}; ///< Track the TSR by solving IK. + bool use_gradient_{false}; ///< Use gradient solving instead of built-in. + bool need_solve_{false}; ///< A solve is requested. - float step_; ///< GUI gradient step size. - float limit_; ///< GUI gradient limit. - float damping_; ///< GUI SVD damping. - float tolerance_; ///< GUI solver tolerance - int maxIter_; ///< GUI maximum allowed iterations. - int item_{0}; ///< GUI solver. + float step_; ///< GUI gradient step size. + float limit_; ///< GUI gradient limit. + float damping_; ///< GUI SVD damping. + float tolerance_; ///< GUI solver tolerance + int maxIter_; ///< GUI maximum allowed iterations. + int item_{0}; ///< GUI solver. /** \} */ diff --git a/robowflex_dart/include/robowflex_dart/joints.h b/robowflex_dart/include/robowflex_dart/joints.h index f01be3bb5..deffc0f5f 100644 --- a/robowflex_dart/include/robowflex_dart/joints.h +++ b/robowflex_dart/include/robowflex_dart/joints.h @@ -202,20 +202,20 @@ namespace robowflex /** \} */ protected: - StateSpace *space_; ///< State space this joint is for. - ompl::RNG &rng_; ///< Random number generator. + StateSpace *space_; ///< State space this joint is for. + ompl::RNG &rng_; ///< Random number generator. std::vector indices_; ///< Indices this joint corresponds to. std::vector dofs_; ///< Controlled DoF names. - unsigned int skelIndex_; ///< Index of skeleton. - unsigned int jointIndex_; ///< Index of joint. + unsigned int skelIndex_; ///< Index of skeleton. + unsigned int jointIndex_; ///< Index of joint. - unsigned int startInSpace_; ///< Start index in space configuration. - unsigned int sizeInSpace_; ///< Size of joint in space configuration. + unsigned int startInSpace_; ///< Start index in space configuration. + unsigned int sizeInSpace_; ///< Size of joint in space configuration. - unsigned int startIndex_; ///< Start index in joint. - unsigned int numDof_; ///< Number of DoF this joint controls. + unsigned int startIndex_; ///< Start index in joint. + unsigned int numDof_; ///< Number of DoF this joint controls. }; /** \brief A real vector joint of \a n dimensions. diff --git a/robowflex_dart/include/robowflex_dart/planner.h b/robowflex_dart/include/robowflex_dart/planner.h index 0aea8d111..8286bceb0 100644 --- a/robowflex_dart/include/robowflex_dart/planner.h +++ b/robowflex_dart/include/robowflex_dart/planner.h @@ -69,9 +69,10 @@ namespace robowflex * \return Allocated planner. */ template - PlannerAllocator makePlanner(Args &&... args) + PlannerAllocator makePlanner(Args &&...args) { - return [&]() -> ompl::base::PlannerPtr { + return [&]() -> ompl::base::PlannerPtr + { if (builder) { auto p = std::make_shared(builder->info, std::forward(args)...); @@ -94,7 +95,7 @@ namespace robowflex WorldPtr world_; ///< DART world containing robot and scene. ompl::base::GoalPtr goal_; ///< Current motion planning goal. }; - } // namespace darts + } // namespace darts } // namespace robowflex #endif diff --git a/robowflex_dart/include/robowflex_dart/planning.h b/robowflex_dart/include/robowflex_dart/planning.h index 1faf7c249..268b82ca3 100644 --- a/robowflex_dart/include/robowflex_dart/planning.h +++ b/robowflex_dart/include/robowflex_dart/planning.h @@ -411,10 +411,10 @@ namespace robowflex ompl::geometric::SimpleSetupPtr ss{nullptr}; ///< Simple Setup. WorldPtr world; ///< World used for planning. - std::vector path_constraints; ///< Path Constraints. - TSRConstraintPtr constraint{nullptr}; ///< OMPL Constraint for Path Constraints. + std::vector path_constraints; ///< Path Constraints. + TSRConstraintPtr constraint{nullptr}; ///< OMPL Constraint for Path Constraints. - Eigen::VectorXd start; ///< Start configuration. + Eigen::VectorXd start; ///< Start configuration. /** \brief Hyperparameter options. */ diff --git a/robowflex_dart/include/robowflex_dart/robot.h b/robowflex_dart/include/robowflex_dart/robot.h index cca13ac4e..5d4222d9a 100644 --- a/robowflex_dart/include/robowflex_dart/robot.h +++ b/robowflex_dart/include/robowflex_dart/robot.h @@ -303,7 +303,7 @@ namespace robowflex ///< named states. std::map> group_indices_; ///< Indices of group joints. - robowflex::RobotPtr robot_{nullptr}; ///< Robowflex robot. + robowflex::RobotPtr robot_{nullptr}; ///< Robowflex robot. }; /** \brief Load a robot from a URDF and SRDF (i.e., a MoveIt enabled robot). diff --git a/robowflex_dart/include/robowflex_dart/space.h b/robowflex_dart/include/robowflex_dart/space.h index a2631f097..4b6055b67 100644 --- a/robowflex_dart/include/robowflex_dart/space.h +++ b/robowflex_dart/include/robowflex_dart/space.h @@ -254,21 +254,21 @@ namespace robowflex void addJoint(const std::string &group_name, const JointPtr &joint); void addJointToGroup(const std::string &group_name, const JointPtr &joint); - WorldPtr world_; ///< World to use for planning. + WorldPtr world_; ///< World to use for planning. std::set jointset_; ///< Set of joints used in planning. std::vector indices_; ///< Vector of indices for planning. - bool metric_{true}; ///< Is this space all Rn? + bool metric_{true}; ///< Is this space all Rn? - std::vector joints_; ///< Vector of all joints used in planning. + std::vector joints_; ///< Vector of all joints used in planning. - ompl::RNG rng_; ///< Random number generator. + ompl::RNG rng_; ///< Random number generator. std::map> group_joints_; ///< Joints belonging to a group. std::map group_dimension_; ///< Dimension of the group. }; - } // namespace darts + } // namespace darts } // namespace robowflex #endif diff --git a/robowflex_dart/include/robowflex_dart/structure.h b/robowflex_dart/include/robowflex_dart/structure.h index 67d8ea401..51cd89671 100644 --- a/robowflex_dart/include/robowflex_dart/structure.h +++ b/robowflex_dart/include/robowflex_dart/structure.h @@ -117,7 +117,6 @@ namespace robowflex */ void dumpGraphViz(std::ostream &out, bool standalone = true); - /** \} */ /** \name Getting and Setting Configurations diff --git a/robowflex_dart/include/robowflex_dart/tsr.h b/robowflex_dart/include/robowflex_dart/tsr.h index 59f996a7a..5f8727380 100644 --- a/robowflex_dart/include/robowflex_dart/tsr.h +++ b/robowflex_dart/include/robowflex_dart/tsr.h @@ -83,7 +83,7 @@ namespace robowflex std::string frame{magic::ROOT_FRAME}; ///< Name of base frame. } base; ///< Base frame. - RobotPose pose{RobotPose::Identity()}; ///< Pose of TSR. + RobotPose pose{RobotPose::Identity()}; ///< Pose of TSR. struct { @@ -689,8 +689,8 @@ namespace robowflex */ void computeBijection(); - WorldPtr world_; ///< Underlying world. - Specification spec_; ///< TSR specification. + WorldPtr world_; ///< Underlying world. + Specification spec_; ///< TSR specification. std::size_t skel_index_; ///< Index of controlled skeleton. std::vector indices_; ///< Controlled indices. @@ -983,20 +983,20 @@ namespace robowflex WorldPtr world_; ///< World to use. std::set skel_indices_; ///< All skeleton indices used by members of the set. - bool qr_{false}; ///< If true, use QR in gradient solve. Else, SVD. - bool damped_{true}; ///< If true, use damped SVD. - double step_{1.0}; ///< Step scaling in gradient. - double limit_{1.}; ///< Step size limit. - double damping_{1e-8}; ///< Damping factor. + bool qr_{false}; ///< If true, use QR in gradient solve. Else, SVD. + bool damped_{true}; ///< If true, use damped SVD. + double step_{1.0}; ///< Step scaling in gradient. + double limit_{1.}; ///< Step size limit. + double damping_{1e-8}; ///< Damping factor. double tolerance_{magic::DEFAULT_IK_TOLERANCE}; ///< Tolerance for solving. std::size_t maxIter_{50}; ///< Maximum iterations to use for solving. - std::vector tsrs_; ///< Set of TSRs - std::vector weights_; ///< Weights on TSRs - std::size_t dimension_{0}; ///< Total error dimension of set. + std::vector tsrs_; ///< Set of TSRs + std::vector weights_; ///< Weights on TSRs + std::size_t dimension_{0}; ///< Total error dimension of set. - Eigen::VectorXd upper_; ///< Upper bounds on world configuration. - Eigen::VectorXd lower_; ///< Lower bounds on world configuration. + Eigen::VectorXd upper_; ///< Upper bounds on world configuration. + Eigen::VectorXd lower_; ///< Lower bounds on world configuration. }; /** \cond IGNORE */ @@ -1058,7 +1058,7 @@ namespace robowflex StateSpacePtr space_; ///< Robot state space. TSRSetPtr tsr_; ///< Set of TSR constraints. }; - } // namespace darts + } // namespace darts } // namespace robowflex #endif diff --git a/robowflex_dart/include/robowflex_dart/world.h b/robowflex_dart/include/robowflex_dart/world.h index a8a1607c5..378333d2f 100644 --- a/robowflex_dart/include/robowflex_dart/world.h +++ b/robowflex_dart/include/robowflex_dart/world.h @@ -232,10 +232,10 @@ namespace robowflex */ void removeSkeletonCollider(const std::string &name, const dart::dynamics::SkeletonPtr &skeleton); - dart::simulation::WorldPtr world_; ///< Underlying world. + dart::simulation::WorldPtr world_; ///< Underlying world. - Eigen::Vector3d low_{-5, -5, -5}; ///< Lower workspace bounds. - Eigen::Vector3d high_{5, 5, 5}; ///< Upper workspace bounds. + Eigen::Vector3d low_{-5, -5, -5}; ///< Lower workspace bounds. + Eigen::Vector3d high_{5, 5, 5}; ///< Upper workspace bounds. std::map robots_; ///< Robots in world. std::map structures_; ///< Structures in world. @@ -258,9 +258,9 @@ namespace robowflex dart::collision::CollisionDetectorPtr collider_; ///< Collision checker. const std::string name_; ///< Name of world. - std::recursive_mutex mutex_; ///< Internal lock. + std::recursive_mutex mutex_; ///< Internal lock. }; - } // namespace darts + } // namespace darts } // namespace robowflex #endif diff --git a/robowflex_dart/scripts/fetch_bimanual.cpp b/robowflex_dart/scripts/fetch_bimanual.cpp index c9c0746e6..0b1915afe 100644 --- a/robowflex_dart/scripts/fetch_bimanual.cpp +++ b/robowflex_dart/scripts/fetch_bimanual.cpp @@ -38,7 +38,8 @@ int main(int /*argc*/, char ** /*argv*/) // // Initial goal - touch fingertips // - const auto &touch = [&] { + const auto &touch = [&] + { darts::PlanBuilder builder(world); builder.addGroup("fetch1", "arm_with_torso"); builder.addGroup("fetch2", "arm_with_torso"); @@ -87,7 +88,8 @@ int main(int /*argc*/, char ** /*argv*/) return solved; }; - const auto &dance = [&] { + const auto &dance = [&] + { darts::PlanBuilder builder(world); builder.addGroup("fetch1", "arm_with_torso"); builder.addGroup("fetch2", "arm_with_torso"); @@ -132,16 +134,18 @@ int main(int /*argc*/, char ** /*argv*/) return solved; }; - window.run([&]() { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - while (true) + window.run( + [&]() { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - auto solved = touch(); - if (solved) - dance(); - } - }); + while (true) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + auto solved = touch(); + if (solved) + dance(); + } + }); return 0; } diff --git a/robowflex_dart/scripts/fetch_ik.cpp b/robowflex_dart/scripts/fetch_ik.cpp index d7692cf66..865b92a0d 100644 --- a/robowflex_dart/scripts/fetch_ik.cpp +++ b/robowflex_dart/scripts/fetch_ik.cpp @@ -68,26 +68,28 @@ int main(int /*argc*/, char ** /*argv*/) builder.setup(); builder.ss->print(); - window.run([&]() { - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - while (true) + window.run( + [&]() { - goal->startSampling(); - ompl::base::PlannerStatus solved = builder.ss->solve(30.0); - goal->stopSampling(); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - if (solved) + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + while (true) { - RBX_INFO("Found solution!"); - window.animatePath(builder, builder.getSolutionPath()); + goal->startSampling(); + ompl::base::PlannerStatus solved = builder.ss->solve(30.0); + goal->stopSampling(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + if (solved) + { + RBX_INFO("Found solution!"); + window.animatePath(builder, builder.getSolutionPath()); + } + else + RBX_WARN("No solution found"); + + builder.ss->clear(); } - else - RBX_WARN("No solution found"); - - builder.ss->clear(); - } - }); + }); return 0; } diff --git a/robowflex_dart/scripts/fetch_lift.cpp b/robowflex_dart/scripts/fetch_lift.cpp index 159f6b17a..aa3448d8e 100644 --- a/robowflex_dart/scripts/fetch_lift.cpp +++ b/robowflex_dart/scripts/fetch_lift.cpp @@ -56,7 +56,8 @@ int main(int /*argc*/, char ** /*argv*/) // // Touch the box // - const auto &touch = [&] { + const auto &touch = [&] + { darts::PlanBuilder builder(world); builder.addGroup("fetch1", "arm_with_torso"); builder.addGroup("fetch2", "arm_with_torso"); @@ -108,7 +109,8 @@ int main(int /*argc*/, char ** /*argv*/) // // Lift the box // - const auto &lift = [&] { + const auto &lift = [&] + { darts::PlanBuilder builder(world); builder.addGroup("fetch1", "arm_with_torso"); builder.addGroup("fetch2", "arm_with_torso"); @@ -168,7 +170,8 @@ int main(int /*argc*/, char ** /*argv*/) // // Lower the box // - const auto &lower = [&] { + const auto &lower = [&] + { darts::PlanBuilder builder(world); builder.addGroup("fetch1", "arm_with_torso"); builder.addGroup("fetch2", "arm_with_torso"); @@ -228,7 +231,8 @@ int main(int /*argc*/, char ** /*argv*/) // // Tuck the Fetch's arms // - const auto &tuck = [&] { + const auto &tuck = [&] + { darts::PlanBuilder builder(world); builder.addGroup("fetch1", "arm_with_torso"); builder.addGroup("fetch2", "arm_with_torso"); @@ -262,30 +266,32 @@ int main(int /*argc*/, char ** /*argv*/) return solved == ompl::base::PlannerStatus::EXACT_SOLUTION; }; - window.run([&]() { - RBX_INFO("Press enter"); - std::cin.ignore(); + window.run( + [&]() + { + RBX_INFO("Press enter"); + std::cin.ignore(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - while (true) - { - if (touch()) + while (true) { - auto *cube = scene->getFrame("box"); - fetch1->reparentFreeFrame(cube, "wrist_roll_link"); - if (lift()) + if (touch()) { - if (lower()) + auto *cube = scene->getFrame("box"); + fetch1->reparentFreeFrame(cube, "wrist_roll_link"); + if (lift()) { - scene->reparentFreeFrame(cube); - if (not tuck()) - break; + if (lower()) + { + scene->reparentFreeFrame(cube); + if (not tuck()) + break; + } } } } - } - }); + }); return 0; } diff --git a/robowflex_dart/scripts/fetch_pick.cpp b/robowflex_dart/scripts/fetch_pick.cpp index 2ad0cea6a..0e5c9b015 100644 --- a/robowflex_dart/scripts/fetch_pick.cpp +++ b/robowflex_dart/scripts/fetch_pick.cpp @@ -56,7 +56,8 @@ int main(int argc, char **argv) darts::Window window(world); - const auto &plan_to_pick = [&]() { + const auto &plan_to_pick = [&]() + { darts::PlanBuilder builder(world); builder.addGroup(fetch_name, GROUP); @@ -102,7 +103,8 @@ int main(int argc, char **argv) RBX_WARN("No solution found"); }; - const auto &plan_to_grasp = [&]() { + const auto &plan_to_grasp = [&]() + { darts::PlanBuilder builder(world); builder.addGroup(fetch_name, GROUP); builder.setStartConfigurationFromWorld(); @@ -146,7 +148,8 @@ int main(int argc, char **argv) RBX_WARN("No solution found"); }; - const auto &plan_to_place = [&]() { + const auto &plan_to_place = [&]() + { darts::PlanBuilder builder(world); builder.addGroup(fetch_name, GROUP); builder.setStartConfigurationFromWorld(); @@ -181,18 +184,20 @@ int main(int argc, char **argv) RBX_WARN("No solution found"); }; - window.run([&] { - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - plan_to_pick(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - plan_to_grasp(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + window.run( + [&] + { + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + plan_to_pick(); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + plan_to_grasp(); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - auto *cube = scene_dart->getFrame("Cube3"); - fetch_dart->reparentFreeFrame(cube, "wrist_roll_link"); + auto *cube = scene_dart->getFrame("Cube3"); + fetch_dart->reparentFreeFrame(cube, "wrist_roll_link"); - plan_to_place(); - }); + plan_to_place(); + }); return 0; } diff --git a/robowflex_dart/scripts/fetch_plan.cpp b/robowflex_dart/scripts/fetch_plan.cpp index 0dde0bcaa..19e05d8d0 100644 --- a/robowflex_dart/scripts/fetch_plan.cpp +++ b/robowflex_dart/scripts/fetch_plan.cpp @@ -58,50 +58,52 @@ int main(int argc, char **argv) } darts::Window window(world); - window.run([&] { - darts::PlanBuilder builder(world); - builder.addGroup("fetch1", "arm_with_torso"); - if (nfetch > 1) - builder.addGroup("fetch2", "arm_with_torso"); - if (nfetch > 2) - builder.addGroup("fetch3", "arm_with_torso"); - if (nfetch > 3) - builder.addGroup("fetch4", "arm_with_torso"); - - builder.setStartConfiguration({ - 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, // - 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, // - 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, // - 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, - }); - - builder.initialize(); - - auto goal = builder.getGoalConfiguration({ - 0.0, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007, - 0.13, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007, // - 0.38, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007, // - 0.26, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007, // - }); - builder.setGoal(goal); - - auto rrt = std::make_shared(builder.info, true); - rrt->setRange(1.); - builder.ss->setPlanner(rrt); - - builder.setup(); - builder.ss->print(); - - ompl::base::PlannerStatus solved = builder.ss->solve(30.0); - - if (solved) + window.run( + [&] { - RBX_INFO("Found solution!"); - window.animatePath(builder, builder.getSolutionPath()); - } - else - RBX_WARN("No solution found"); - }); + darts::PlanBuilder builder(world); + builder.addGroup("fetch1", "arm_with_torso"); + if (nfetch > 1) + builder.addGroup("fetch2", "arm_with_torso"); + if (nfetch > 2) + builder.addGroup("fetch3", "arm_with_torso"); + if (nfetch > 3) + builder.addGroup("fetch4", "arm_with_torso"); + + builder.setStartConfiguration({ + 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, // + 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, // + 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, // + 0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, + }); + + builder.initialize(); + + auto goal = builder.getGoalConfiguration({ + 0.0, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007, + 0.13, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007, // + 0.38, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007, // + 0.26, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007, // + }); + builder.setGoal(goal); + + auto rrt = std::make_shared(builder.info, true); + rrt->setRange(1.); + builder.ss->setPlanner(rrt); + + builder.setup(); + builder.ss->print(); + + ompl::base::PlannerStatus solved = builder.ss->solve(30.0); + + if (solved) + { + RBX_INFO("Found solution!"); + window.animatePath(builder, builder.getSolutionPath()); + } + else + RBX_WARN("No solution found"); + }); return 0; } diff --git a/robowflex_dart/scripts/fetch_robowflex_plan.cpp b/robowflex_dart/scripts/fetch_robowflex_plan.cpp index fa8a403d7..9f61222af 100644 --- a/robowflex_dart/scripts/fetch_robowflex_plan.cpp +++ b/robowflex_dart/scripts/fetch_robowflex_plan.cpp @@ -120,26 +120,28 @@ int main(int argc, char **argv) ompl::base::Cost(std::numeric_limits::infinity())); darts::Window window(world); - window.run([&] { - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - while (true) + window.run( + [&] { - goal->startSampling(); - ompl::base::PlannerStatus solved = builder.ss->solve(60.0); - goal->stopSampling(); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - if (solved) + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + while (true) { - RBX_INFO("Found solution!"); - window.animatePath(builder, builder.getSolutionPath()); + goal->startSampling(); + ompl::base::PlannerStatus solved = builder.ss->solve(60.0); + goal->stopSampling(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + if (solved) + { + RBX_INFO("Found solution!"); + window.animatePath(builder, builder.getSolutionPath()); + } + else + RBX_WARN("No solution found"); + + builder.ss->clear(); } - else - RBX_WARN("No solution found"); - - builder.ss->clear(); - } - }); + }); return 0; } diff --git a/robowflex_dart/scripts/fetch_se2.cpp b/robowflex_dart/scripts/fetch_se2.cpp index 704bba1c0..16458064e 100644 --- a/robowflex_dart/scripts/fetch_se2.cpp +++ b/robowflex_dart/scripts/fetch_se2.cpp @@ -86,17 +86,19 @@ int main(int argc, char **argv) builder.ss->print(); darts::Window window(world); - window.run([&] { - ompl::base::PlannerStatus solved = builder.ss->solve(5.0); - - if (solved) + window.run( + [&] { - RBX_INFO("Found solution!"); - window.animatePath(builder, builder.getSolutionPath(), 10); - } - else - RBX_WARN("No solution found"); - }); + ompl::base::PlannerStatus solved = builder.ss->solve(5.0); + + if (solved) + { + RBX_INFO("Found solution!"); + window.animatePath(builder, builder.getSolutionPath(), 10); + } + else + RBX_WARN("No solution found"); + }); return 0; } diff --git a/robowflex_dart/scripts/gui_test.cpp b/robowflex_dart/scripts/gui_test.cpp index 9bd170f55..14ba675e3 100644 --- a/robowflex_dart/scripts/gui_test.cpp +++ b/robowflex_dart/scripts/gui_test.cpp @@ -50,7 +50,8 @@ int main(int /*argc*/, char ** /*argv*/) options_ik.thickness = 1; options_ik.pose = fetch1->getFrame("wrist_roll_link")->getWorldTransform(); - options_ik.callback = [&](const auto &frame) { + options_ik.callback = [&](const auto &frame) + { auto &spec = ik_tsr->getSpecification(); spec.setPose(frame->getWorldTransform()); ik_tsr->updatePose(); @@ -82,7 +83,8 @@ int main(int /*argc*/, char ** /*argv*/) options_look.planar[1] = false; options_look.planar[2] = false; - options_look.callback = [&](const auto &frame) { + options_look.callback = [&](const auto &frame) + { auto &spec = look_tsr->getSpecification(); spec.setPose(frame->getWorldTransform()); look_tsr->updatePose(); @@ -99,28 +101,33 @@ int main(int /*argc*/, char ** /*argv*/) // Pick / Place Button bool picked = false; auto *cube = scene->getFrame("box"); - window.getWidget()->addButton("Pick/Place", [&] { - if (not picked) - fetch1->reparentFreeFrame(cube, "wrist_roll_link"); - else - scene->reparentFreeFrame(cube); + window.getWidget()->addButton("Pick/Place", + [&] + { + if (not picked) + fetch1->reparentFreeFrame(cube, "wrist_roll_link"); + else + scene->reparentFreeFrame(cube); - picked = not picked; - }); + picked = not picked; + }); // Setup reset button window.getWidget()->addText("Press button to reset robot state!"); - window.getWidget()->addButton("Reset", [&] { - // do it a few times since the IK tries to update - for (std::size_t i = 0; i < 3; ++i) - { - fetch1->getSkeleton()->setState(start); - ik_ret.target->setTransform(fetch1->getFrame("wrist_roll_link")->getWorldTransform()); - auto tf = fetch1->getFrame("head_camera_link")->getWorldTransform(); - tf.translation()[1] += 0.5; - look_ret.target->setTransform(tf); - } - }); + window.getWidget()->addButton("Reset", + [&] + { + // do it a few times since the IK tries to update + for (std::size_t i = 0; i < 3; ++i) + { + fetch1->getSkeleton()->setState(start); + ik_ret.target->setTransform( + fetch1->getFrame("wrist_roll_link")->getWorldTransform()); + auto tf = fetch1->getFrame("head_camera_link")->getWorldTransform(); + tf.translation()[1] += 0.5; + look_ret.target->setTransform(tf); + } + }); window.run(); return 0; diff --git a/robowflex_dart/src/gui.cpp b/robowflex_dart/src/gui.cpp index b9fe3816a..eb66bd75c 100644 --- a/robowflex_dart/src/gui.cpp +++ b/robowflex_dart/src/gui.cpp @@ -131,14 +131,16 @@ Window::InteractiveReturn Window::createInteractiveMarker(const InteractiveOptio r.dnd->setObstructable(options.obstructable); auto callback = options.callback; - r.signal = r.target->onTransformUpdated.connect([callback](const dart::dynamics::Entity *entity) { - if (entity) + r.signal = r.target->onTransformUpdated.connect( + [callback](const dart::dynamics::Entity *entity) { - const auto *cast = dynamic_cast(entity); - if (cast and callback) - callback(cast); - } - }); + if (entity) + { + const auto *cast = dynamic_cast(entity); + if (cast and callback) + callback(cast); + } + }); return r; } @@ -149,10 +151,12 @@ Window::DnDReturn Window::enableNodeDragNDrop(dart::dynamics::BodyNode *node, co auto *dnd = viewer_.enableDragAndDrop(node, true, true); r.dnd = dnd; - r.signal = node->onTransformUpdated.connect([dnd, callback](const dart::dynamics::Entity *entity) { - if (dnd->isMoving()) - callback(dynamic_cast(entity)); - }); + r.signal = node->onTransformUpdated.connect( + [dnd, callback](const dart::dynamics::Entity *entity) + { + if (dnd->isMoving()) + callback(dynamic_cast(entity)); + }); return r; } @@ -170,29 +174,31 @@ void Window::animatePath(const StateSpacePtr &space, const ompl::geometric::Path animation_->join(); animation_.reset(); } - auto thread = std::make_shared([&] { - std::size_t n = path.getStateCount(); - std::size_t i = times; - - std::unique_lock lk(m); - while ((times == 0) ? true : i--) + auto thread = std::make_shared( + [&] { - space->setWorldState(world_, path.getState(0)); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::size_t n = path.getStateCount(); + std::size_t i = times; - for (std::size_t j = 0; j < n; ++j) + std::unique_lock lk(m); + while ((times == 0) ? true : i--) { - space->setWorldState(world_, path.getState(j)); + space->setWorldState(world_, path.getState(0)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - std::this_thread::sleep_for(std::chrono::milliseconds((unsigned int)(1000 / fps))); - } + for (std::size_t j = 0; j < n; ++j) + { + space->setWorldState(world_, path.getState(j)); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - } + std::this_thread::sleep_for(std::chrono::milliseconds((unsigned int)(1000 / fps))); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } - active = false; - cv.notify_one(); - }); + active = false; + cv.notify_one(); + }); animation_ = thread; diff --git a/robowflex_dart/src/planning.cpp b/robowflex_dart/src/planning.cpp index f85630747..8e0691cd7 100644 --- a/robowflex_dart/src/planning.cpp +++ b/robowflex_dart/src/planning.cpp @@ -110,11 +110,13 @@ TSRGoal::TSRGoal(const PlanBuilder &builder, TSRPtr tsr) } TSRGoal::TSRGoal(const PlanBuilder &builder, const std::vector &tsrs) - : TSRGoal(builder.info, builder.world, [&] { - std::vector temp = builder.path_constraints; - temp.insert(temp.end(), tsrs.begin(), tsrs.end()); - return temp; - }()) + : TSRGoal(builder.info, builder.world, + [&] + { + std::vector temp = builder.path_constraints; + temp.insert(temp.end(), tsrs.begin(), tsrs.end()); + return temp; + }()) { } @@ -582,21 +584,24 @@ void PlanBuilder::setStateValidityChecker() if (ss) { // ss->setStateValidityChecker(std::make_shared(info, 1)); - ss->setStateValidityChecker([&](const ompl::base::State *state) -> bool { - const auto &as = toStateConst(state); - - world->lock(); - rspace->setWorldState(world, as); - bool r = not world->inCollision(); - world->unlock(); - return r; - }); + ss->setStateValidityChecker( + [&](const ompl::base::State *state) -> bool + { + const auto &as = toStateConst(state); + + world->lock(); + rspace->setWorldState(world, as); + bool r = not world->inCollision(); + world->unlock(); + return r; + }); } } ompl::base::StateValidityCheckerFn PlanBuilder::getSVCUnconstrained() { - return [&](const ompl::base::State *state) -> bool { + return [&](const ompl::base::State *state) -> bool + { const auto &as = fromUnconstrainedStateConst(state); world->lock(); @@ -609,7 +614,8 @@ ompl::base::StateValidityCheckerFn PlanBuilder::getSVCUnconstrained() ompl::base::StateValidityCheckerFn PlanBuilder::getSVCConstrained() { - return [&](const ompl::base::State *state) -> bool { + return [&](const ompl::base::State *state) -> bool + { const auto &as = fromConstrainedStateConst(state); world->lock(); diff --git a/robowflex_library/include/robowflex_library/benchmarking.h b/robowflex_library/include/robowflex_library/benchmarking.h index b8c1f0249..ef75d530a 100644 --- a/robowflex_library/include/robowflex_library/benchmarking.h +++ b/robowflex_library/include/robowflex_library/benchmarking.h @@ -487,14 +487,14 @@ namespace robowflex std::size_t trials_; ///< Number of trials to run each query for. bool timeout_; ///< If true, will re-run planners on queries until total time taken has exceeded the ///< allotted time. - bool enforce_single_thread_{true}; ///< If true, will request each planner to only use a single - ///< thread. - bool override_planning_time_{true}; ///< If true, will override request planning time with global - ///< allowed time. - - Profiler::Options options_; ///< Options for profiler. - Profiler profiler_; ///< Profiler to use for extracting data. - std::vector queries_; ///< Queries to test. + bool enforce_single_thread_{true}; ///< If true, will request each planner to only use a single + ///< thread. + bool override_planning_time_{true}; ///< If true, will override request planning time with global + ///< allowed time. + + Profiler::Options options_; ///< Options for profiler. + Profiler profiler_; ///< Profiler to use for extracting data. + std::vector queries_; ///< Queries to test. PreRunCallback pre_callback_; ///< Pre-run callback. PostRunCallback post_callback_; ///< Post-run callback. diff --git a/robowflex_library/include/robowflex_library/builder.h b/robowflex_library/include/robowflex_library/builder.h index 3a81a2675..b1daf28eb 100644 --- a/robowflex_library/include/robowflex_library/builder.h +++ b/robowflex_library/include/robowflex_library/builder.h @@ -434,16 +434,16 @@ namespace robowflex /** \} */ private: - const RobotConstPtr robot_; ///< The robot to build the request for. + const RobotConstPtr robot_; ///< The robot to build the request for. - PlannerConstPtr planner_; ///< The planner to build the request for. - std::string group_name_; ///< The group to plan for. - robot_model::JointModelGroup *jmg_{nullptr}; ///< The joint model group of the robot (from \a - ///< group_name_) + PlannerConstPtr planner_; ///< The planner to build the request for. + std::string group_name_; ///< The group to plan for. + robot_model::JointModelGroup *jmg_{nullptr}; ///< The joint model group of the robot (from \a + ///< group_name_) planning_interface::MotionPlanRequest request_; ///< The build request. - static const std::string DEFAULT_CONFIG; ///< Default planner configuration to use + static const std::string DEFAULT_CONFIG; ///< Default planner configuration to use }; } // namespace robowflex diff --git a/robowflex_library/include/robowflex_library/detail/cob4.h b/robowflex_library/include/robowflex_library/detail/cob4.h index 512c643e1..3202d9c61 100644 --- a/robowflex_library/include/robowflex_library/detail/cob4.h +++ b/robowflex_library/include/robowflex_library/detail/cob4.h @@ -72,10 +72,10 @@ namespace robowflex void closeRightGripper(); private: - static const std::string DEFAULT_URDF; ///< Default URDF - static const std::string DEFAULT_SRDF; ///< Default SRDF - static const std::string DEFAULT_LIMITS; ///< Default Limits - static const std::string DEFAULT_KINEMATICS; ///< Default kinematics + static const std::string DEFAULT_URDF; ///< Default URDF + static const std::string DEFAULT_SRDF; ///< Default SRDF + static const std::string DEFAULT_LIMITS; ///< Default Limits + static const std::string DEFAULT_KINEMATICS; ///< Default kinematics static const std::string RESOURCE_URDF; ///< URDF from robowflex_resources static const std::string RESOURCE_SRDF; ///< SRDF from robowflex_resources @@ -118,7 +118,7 @@ namespace robowflex static const std::string DEFAULT_CONFIG; ///< Default planning configuration. static const std::string RESOURCE_CONFIG; ///< Planning configuration from robowflex_resources. }; - } // namespace OMPL + } // namespace OMPL } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/detail/fetch.h b/robowflex_library/include/robowflex_library/detail/fetch.h index d30d19679..4d62c438f 100644 --- a/robowflex_library/include/robowflex_library/detail/fetch.h +++ b/robowflex_library/include/robowflex_library/detail/fetch.h @@ -64,10 +64,10 @@ namespace robowflex void closeGripper(); private: - static const std::string DEFAULT_URDF; ///< Default URDF - static const std::string DEFAULT_SRDF; ///< Default SRDF - static const std::string DEFAULT_LIMITS; ///< Default Limits - static const std::string DEFAULT_KINEMATICS; ///< Default kinematics + static const std::string DEFAULT_URDF; ///< Default URDF + static const std::string DEFAULT_SRDF; ///< Default SRDF + static const std::string DEFAULT_LIMITS; ///< Default Limits + static const std::string DEFAULT_KINEMATICS; ///< Default kinematics static const std::string RESOURCE_URDF; ///< URDF from robowflex_resources static const std::string RESOURCE_SRDF; ///< SRDF from robowflex_resources @@ -111,7 +111,7 @@ namespace robowflex static const std::string DEFAULT_CONFIG; ///< Default planning configuration. static const std::string RESOURCE_CONFIG; ///< Planning configuration from robowflex_resources. }; - } // namespace OMPL + } // namespace OMPL } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/detail/r2.h b/robowflex_library/include/robowflex_library/detail/r2.h index ed017683b..74c584a3f 100644 --- a/robowflex_library/include/robowflex_library/detail/r2.h +++ b/robowflex_library/include/robowflex_library/detail/r2.h @@ -87,7 +87,7 @@ namespace robowflex static const std::string CONFIG; ///< Default planning configuration. static const std::string PLUGIN; ///< Default planning plugin. }; - } // namespace OMPL + } // namespace OMPL } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/detail/ur5.h b/robowflex_library/include/robowflex_library/detail/ur5.h index 4b5c1cc65..0b68920e5 100644 --- a/robowflex_library/include/robowflex_library/detail/ur5.h +++ b/robowflex_library/include/robowflex_library/detail/ur5.h @@ -36,10 +36,10 @@ namespace robowflex bool initialize(); private: - static const std::string DEFAULT_URDF; ///< Default URDF - static const std::string DEFAULT_SRDF; ///< Default SRDF - static const std::string DEFAULT_LIMITS; ///< Default Limits - static const std::string DEFAULT_KINEMATICS; ///< Default kinematics + static const std::string DEFAULT_URDF; ///< Default URDF + static const std::string DEFAULT_SRDF; ///< Default SRDF + static const std::string DEFAULT_LIMITS; ///< Default Limits + static const std::string DEFAULT_KINEMATICS; ///< Default kinematics static const std::string RESOURCE_URDF; ///< URDF from robowflex_resources static const std::string RESOURCE_SRDF; ///< SRDF from robowflex_resources @@ -82,7 +82,7 @@ namespace robowflex static const std::string DEFAULT_CONFIG; ///< Default planning configuration. static const std::string RESOURCE_CONFIG; ///< Planning configuration from robowflex_resources. }; - } // namespace OMPL + } // namespace OMPL } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/io.h b/robowflex_library/include/robowflex_library/io.h index ce562314d..99d81387b 100644 --- a/robowflex_library/include/robowflex_library/io.h +++ b/robowflex_library/include/robowflex_library/io.h @@ -3,15 +3,15 @@ #ifndef ROBOWFLEX_IO_ #define ROBOWFLEX_IO_ -#include // for std::string -#include // for std::pair -#include // for std::ofstream +#include // for std::string +#include // for std::pair +#include // for std::ofstream -#include // for date operations +#include // for date operations #include // for message operations -#include // for YAML parsing +#include // for YAML parsing namespace robowflex { diff --git a/robowflex_library/include/robowflex_library/io/bag.h b/robowflex_library/include/robowflex_library/io/bag.h index 68e55d8ba..5d0e8995a 100644 --- a/robowflex_library/include/robowflex_library/io/bag.h +++ b/robowflex_library/include/robowflex_library/io/bag.h @@ -87,7 +87,7 @@ namespace robowflex const std::string file_; ///< File opened. rosbag::Bag bag_; ///< `rosbag` opened. }; - } // namespace IO + } // namespace IO } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/io/broadcaster.h b/robowflex_library/include/robowflex_library/io/broadcaster.h index a0a6a3aec..5a5668b12 100644 --- a/robowflex_library/include/robowflex_library/io/broadcaster.h +++ b/robowflex_library/include/robowflex_library/io/broadcaster.h @@ -83,7 +83,7 @@ namespace robowflex std::map static_; ///< Static transforms. }; - } // namespace IO + } // namespace IO } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/io/gnuplot.h b/robowflex_library/include/robowflex_library/io/gnuplot.h index 6a1d85d16..c52363361 100644 --- a/robowflex_library/include/robowflex_library/io/gnuplot.h +++ b/robowflex_library/include/robowflex_library/io/gnuplot.h @@ -47,8 +47,8 @@ namespace robowflex std::string title; ///< Title of the plot. std::string mode{"qt"}; ///< Terminal mode for GNUPlot - Axis x; ///< X-axis parameters. - Axis y; ///< Y-axis parameters. + Axis x; ///< X-axis parameters. + Axis y; ///< Y-axis parameters. }; /** \brief Configure a plot using common options. diff --git a/robowflex_library/include/robowflex_library/io/handler.h b/robowflex_library/include/robowflex_library/io/handler.h index 7554489fd..6f7f9cac6 100644 --- a/robowflex_library/include/robowflex_library/io/handler.h +++ b/robowflex_library/include/robowflex_library/io/handler.h @@ -3,9 +3,9 @@ #ifndef ROBOWFLEX_IO_HANDLER_ #define ROBOWFLEX_IO_HANDLER_ -#include // for std::string +#include // for std::string -#include // for YAML::Node +#include // for YAML::Node #include // for ros::NodeHandle @@ -89,15 +89,15 @@ namespace robowflex const std::string &getNamespace() const; private: - static const std::string UUID; ///< UUID of handler. + static const std::string UUID; ///< UUID of handler. - const std::string name_; ///< Name of handler. - const std::string namespace_; ///< Full namespace of handler. - ros::NodeHandle nh_; ///< ROS node handle. + const std::string name_; ///< Name of handler. + const std::string namespace_; ///< Full namespace of handler. + ros::NodeHandle nh_; ///< ROS node handle. std::vector params_; ///< Set parameter keys. }; - } // namespace IO + } // namespace IO } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/io/hdf5.h b/robowflex_library/include/robowflex_library/io/hdf5.h index 767a18643..700a924da 100644 --- a/robowflex_library/include/robowflex_library/io/hdf5.h +++ b/robowflex_library/include/robowflex_library/io/hdf5.h @@ -77,11 +77,11 @@ namespace robowflex const H5::DataSet dataset_; ///< Dataset being read from. const H5::DataSpace space_; ///< Size of the dataset. - const H5T_class_t type_; ///< Type of the dataset. - const int rank_; ///< Rank of the dataset. - const hsize_t *dims_; ///< Dimensions of the dataset (rank_ dimensions) + const H5T_class_t type_; ///< Type of the dataset. + const int rank_; ///< Rank of the dataset. + const hsize_t *dims_; ///< Dimensions of the dataset (rank_ dimensions) - const void *data_; ///< Data itself. + const void *data_; ///< Data itself. }; /** \brief An HDF5 File loaded into memory. @@ -133,7 +133,7 @@ namespace robowflex const H5::H5File file_; ///< The loaded HDF5 file. Node data_; ///< A recursive map of loaded data. }; - } // namespace IO + } // namespace IO } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/io/plugin.h b/robowflex_library/include/robowflex_library/io/plugin.h index 768d77e79..1bdf9a2ef 100644 --- a/robowflex_library/include/robowflex_library/io/plugin.h +++ b/robowflex_library/include/robowflex_library/io/plugin.h @@ -139,7 +139,7 @@ namespace robowflex std::mutex mutex_; ///< Class loading mutex std::map, BaseLoaderPtr> loaders_; ///< Cached loaders }; - } // namespace IO + } // namespace IO } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/io/visualization.h b/robowflex_library/include/robowflex_library/io/visualization.h index 9e0d12fca..6bcce30c2 100644 --- a/robowflex_library/include/robowflex_library/io/visualization.h +++ b/robowflex_library/include/robowflex_library/io/visualization.h @@ -245,17 +245,17 @@ namespace robowflex const RobotPose &pose, const Eigen::Vector4d &color, const Eigen::Vector3d &scale) const; - RobotConstPtr robot_; ///< Robot being visualized. - ros::NodeHandle nh_; ///< Handle for publishing. - ros::Publisher marker_pub_; ///< Marker publisher. - ros::Publisher trajectory_pub_; ///< Trajectory publisher. - ros::Publisher scene_pub_; ///< Scene publisher. - ros::Publisher pcd_pub_; ///< Pointcloud publisher. - ros::Publisher state_pub_; ///< State publisher. + RobotConstPtr robot_; ///< Robot being visualized. + ros::NodeHandle nh_; ///< Handle for publishing. + ros::Publisher marker_pub_; ///< Marker publisher. + ros::Publisher trajectory_pub_; ///< Trajectory publisher. + ros::Publisher scene_pub_; ///< Scene publisher. + ros::Publisher pcd_pub_; ///< Pointcloud publisher. + ros::Publisher state_pub_; ///< State publisher. std::multimap markers_; ///< Markers to publish. }; - } // namespace IO + } // namespace IO } // namespace robowflex diff --git a/robowflex_library/include/robowflex_library/log.h b/robowflex_library/include/robowflex_library/log.h index 5df3ea8a1..f94fe327a 100644 --- a/robowflex_library/include/robowflex_library/log.h +++ b/robowflex_library/include/robowflex_library/log.h @@ -45,7 +45,7 @@ namespace robowflex * \return String of formatted arguments. */ template - std::string formatRecurse(boost::format &f, T &&t, Args &&... args) + std::string formatRecurse(boost::format &f, T &&t, Args &&...args) { return formatRecurse(f % std::forward(t), std::forward(args)...); } @@ -57,7 +57,7 @@ namespace robowflex * \return String of formatted arguments. */ template - std::string format(const std::string &fmt, Args &&... args) + std::string format(const std::string &fmt, Args &&...args) { boost::format f(fmt); return formatRecurse(f, std::forward(args)...); diff --git a/robowflex_library/include/robowflex_library/macros.h b/robowflex_library/include/robowflex_library/macros.h index 351ba0644..5cac606b0 100644 --- a/robowflex_library/include/robowflex_library/macros.h +++ b/robowflex_library/include/robowflex_library/macros.h @@ -5,9 +5,9 @@ #include // for boost version macros -#include // for MoveIt version macros +#include // for MoveIt version macros -#include // for ROS version macros +#include // for ROS version macros /** \file */ diff --git a/robowflex_library/include/robowflex_library/planning.h b/robowflex_library/include/robowflex_library/planning.h index 71b4b1867..d7d2a7acc 100644 --- a/robowflex_library/include/robowflex_library/planning.h +++ b/robowflex_library/include/robowflex_library/planning.h @@ -124,7 +124,7 @@ namespace robowflex * \return True on success, false on failure. */ template - bool initialize(Args &&... args) + bool initialize(Args &&...args) { for (unsigned int i = 0; i < pool_.getThreadCount(); ++i) { @@ -160,7 +160,7 @@ namespace robowflex std::vector getPlannerConfigs() const override; private: - Pool pool_; ///< Thread pool + Pool pool_; ///< Thread pool std::queue planners_; ///< Motion planners std::mutex mutex_; ///< Planner mutex @@ -369,7 +369,7 @@ namespace robowflex std::vector configs_; ///< Planning configurations loaded from \a config_file in ///< initialize() }; - } // namespace OMPL + } // namespace OMPL namespace opt { @@ -527,7 +527,7 @@ namespace robowflex static const std::string DEFAULT_PLUGIN; ///< The default TrajOpt plugin. static const std::vector DEFAULT_ADAPTERS; ///< The default planning adapters. }; - } // namespace opt + } // namespace opt } // namespace robowflex #endif diff --git a/robowflex_library/include/robowflex_library/pool.h b/robowflex_library/include/robowflex_library/pool.h index 3753f3e5f..e24a1351f 100644 --- a/robowflex_library/include/robowflex_library/pool.h +++ b/robowflex_library/include/robowflex_library/pool.h @@ -129,7 +129,7 @@ namespace robowflex * \tparam Args Types of the function arguments. */ template - Job(const std::function &&function, Args &&... args) + Job(const std::function &&function, Args &&...args) : function_(std::bind(function, args...)), task_(function_), future_(task_.get_future()) { } @@ -206,7 +206,7 @@ namespace robowflex * which results in no execution of the function by the queue. */ template - std::shared_ptr> submit(const std::function &&function, Args &&... args) const + std::shared_ptr> submit(const std::function &&function, Args &&...args) const { auto job = std::make_shared>(std::forward>(function), std::forward(args)...); @@ -227,9 +227,9 @@ namespace robowflex void run(); private: - bool active_{false}; ///< Is thread pool active? - mutable std::mutex mutex_; ///< Job queue mutex. - mutable std::condition_variable cv_; ///< Job queue condition variable. + bool active_{false}; ///< Is thread pool active? + mutable std::mutex mutex_; ///< Job queue mutex. + mutable std::condition_variable cv_; ///< Job queue condition variable. std::vector threads_; ///< Threads. mutable std::queue> jobs_; ///< Jobs to execute. diff --git a/robowflex_library/include/robowflex_library/robot.h b/robowflex_library/include/robowflex_library/robot.h index 9153d0686..95f19f7af 100644 --- a/robowflex_library/include/robowflex_library/robot.h +++ b/robowflex_library/include/robowflex_library/robot.h @@ -787,11 +787,11 @@ namespace robowflex /** \} */ - const std::string name_; ///< Robot name. - IO::Handler handler_; ///< IO handler (namespaced with \a name_) + const std::string name_; ///< Robot name. + IO::Handler handler_; ///< IO handler (namespaced with \a name_) - std::string urdf_; ///< The URDF as a string. - std::string srdf_; ///< The SRDF as a string. + std::string urdf_; ///< The URDF as a string. + std::string srdf_; ///< The SRDF as a string. PostProcessXMLFunction urdf_function_; ///< URDF post-processing function. PostProcessXMLFunction srdf_function_; ///< SRDF post-processing function. @@ -803,7 +803,7 @@ namespace robowflex std::map imap_; ///< Kinematic solver allocator map. kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_; ///< Kinematic plugin loader. - robot_state::RobotStatePtr scratch_; ///< Scratch robot state. + robot_state::RobotStatePtr scratch_; ///< Scratch robot state. }; /** \cond IGNORE */ diff --git a/robowflex_library/scripts/fetch_benchmark.cpp b/robowflex_library/scripts/fetch_benchmark.cpp index 1f5b949b0..89677ac59 100644 --- a/robowflex_library/scripts/fetch_benchmark.cpp +++ b/robowflex_library/scripts/fetch_benchmark.cpp @@ -77,8 +77,8 @@ int main(int argc, char **argv) // Use the post-query callback to visualize the data live. IO::GNUPlotPlanDataSetOutputter plot("time"); - experiment.setPostQueryCallback( - [&](PlanDataSetPtr dataset, const PlanningQuery &) { plot.dump(*dataset); }); + experiment.setPostQueryCallback([&](PlanDataSetPtr dataset, const PlanningQuery &) + { plot.dump(*dataset); }); auto dataset = experiment.benchmark(4); diff --git a/robowflex_library/scripts/ur5_cylinder.cpp b/robowflex_library/scripts/ur5_cylinder.cpp index f14b0020b..d1f0b9c4f 100644 --- a/robowflex_library/scripts/ur5_cylinder.cpp +++ b/robowflex_library/scripts/ur5_cylinder.cpp @@ -59,7 +59,7 @@ int main(int argc, char **argv) pose, cylinder, // 0.15, 0.04, 16); // - rviz.addGoalMarker("goal", request); // Visualize the grasping regions + rviz.addGoalMarker("goal", request); // Visualize the grasping regions rviz.updateMarkers(); RBX_INFO("Scene and Goal displayed! Press enter to plan..."); diff --git a/robowflex_library/src/benchmarking.cpp b/robowflex_library/src/benchmarking.cpp index a15c17522..9d14b4022 100644 --- a/robowflex_library/src/benchmarking.cpp +++ b/robowflex_library/src/benchmarking.cpp @@ -178,38 +178,40 @@ bool Profiler::profilePlan(const PlannerPtr &planner, result.property_names.emplace_back("time REAL"); } - progress_thread.reset(new std::thread([&] { - bool at_least_once = options.progress_at_least_once; - while (true) + progress_thread.reset(new std::thread( + [&] { - // Sleep until the next update - IO::threadSleep(options.progress_update_rate); + bool at_least_once = options.progress_at_least_once; + while (true) + { + // Sleep until the next update + IO::threadSleep(options.progress_update_rate); - std::unique_lock lock(mutex); - if (not at_least_once and complete) - return; + std::unique_lock lock(mutex); + if (not at_least_once and complete) + return; - if (have_prog) - { - std::map data; + if (have_prog) + { + std::map data; - // Add time stamp - double time = IO::getSeconds(result.start, IO::getDate()); - data["time REAL"] = std::to_string(time); + // Add time stamp + double time = IO::getSeconds(result.start, IO::getDate()); + data["time REAL"] = std::to_string(time); - // Compute properties - for (const auto &property : prog_props) - data[property.first] = property.second(); + // Compute properties + for (const auto &property : prog_props) + data[property.first] = property.second(); - result.progress.emplace_back(data); - } + result.progress.emplace_back(data); + } - for (const auto &callback : prog_call) - callback(planner, scene, request, result); + for (const auto &callback : prog_call) + callback(planner, scene, request, result); - at_least_once = false; - } - })); + at_least_once = false; + } + })); } // Plan @@ -427,94 +429,100 @@ PlanDataSetPtr Experiment::benchmark(std::size_t n_threads) const std::size_t total_queries = todo.size(); for (std::size_t i = 0; i < n_threads; ++i) - threads.emplace_back(std::make_shared([&]() { - std::size_t id = IO::getThreadID(); - while (true) + threads.emplace_back(std::make_shared( + [&]() { - ThreadInfo info; - - { - std::unique_lock lock(mutex); - if (todo.empty()) // All done, exit. - return; - - info = todo.front(); - todo.pop(); - } - - RBX_INFO("[Thread %1%] Running Query %3% `%2%` Trial [%4%/%5%]", // - id, info.query->name, info.index, info.trial + 1, trials_); - - // If override, use global time. Else use query time. - double time_remaining = - (override_planning_time_) ? allowed_time_ : info.query->request.allowed_planning_time; - - std::size_t timeout_trial = 0; - while (time_remaining > 0.) + std::size_t id = IO::getThreadID(); + while (true) { - planning_interface::MotionPlanRequest request = info.query->request; - request.allowed_planning_time = time_remaining; - - if (enforce_single_thread_) - request.num_planning_attempts = 1; - - // Call pre-run callbacks - info.query->planner->preRun(info.query->scene, request); - - if (pre_callback_) - pre_callback_(*info.query); - - // Profile query - auto data = std::make_shared(); - profiler_.profilePlan(info.query->planner, // - info.query->scene, // - request, // - options_, // - *data); - - // Add experiment specific metrics - data->metrics.emplace("query_trial", (int)info.trial); - data->metrics.emplace("query_index", (int)info.index); - data->metrics.emplace("query_timeout_trial", (int)timeout_trial); - data->metrics.emplace("query_start_time", IO::getSeconds(dataset->start, data->start)); - data->metrics.emplace("query_finish_time", IO::getSeconds(dataset->start, data->finish)); - - data->query.name = log::format("%1%:%2%:%3%", info.query->name, info.trial, info.index); - - if (timeout_) - data->query.name = data->query.name + log::format(":%4%", timeout_trial); - - if (post_callback_) - post_callback_(*data, *info.query); + ThreadInfo info; { std::unique_lock lock(mutex); + if (todo.empty()) // All done, exit. + return; - dataset->addDataPoint(info.query->name, data); - completed_queries++; - - if (complete_callback_) - complete_callback_(dataset, *info.query); + info = todo.front(); + todo.pop(); } - if (timeout_) + RBX_INFO("[Thread %1%] Running Query %3% `%2%` Trial [%4%/%5%]", // + id, info.query->name, info.index, info.trial + 1, trials_); + + // If override, use global time. Else use query time. + double time_remaining = + (override_planning_time_) ? allowed_time_ : info.query->request.allowed_planning_time; + + std::size_t timeout_trial = 0; + while (time_remaining > 0.) { - time_remaining -= data->time; - RBX_INFO( // - "[Thread %1%] Running Query %3% `%2%` till timeout, %4% seconds remaining...", // - id, info.query->name, info.index, time_remaining); - timeout_trial++; + planning_interface::MotionPlanRequest request = info.query->request; + request.allowed_planning_time = time_remaining; + + if (enforce_single_thread_) + request.num_planning_attempts = 1; + + // Call pre-run callbacks + info.query->planner->preRun(info.query->scene, request); + + if (pre_callback_) + pre_callback_(*info.query); + + // Profile query + auto data = std::make_shared(); + profiler_.profilePlan(info.query->planner, // + info.query->scene, // + request, // + options_, // + *data); + + // Add experiment specific metrics + data->metrics.emplace("query_trial", (int)info.trial); + data->metrics.emplace("query_index", (int)info.index); + data->metrics.emplace("query_timeout_trial", (int)timeout_trial); + data->metrics.emplace("query_start_time", + IO::getSeconds(dataset->start, data->start)); + data->metrics.emplace("query_finish_time", + IO::getSeconds(dataset->start, data->finish)); + + data->query.name = + log::format("%1%:%2%:%3%", info.query->name, info.trial, info.index); + + if (timeout_) + data->query.name = data->query.name + log::format(":%4%", timeout_trial); + + if (post_callback_) + post_callback_(*data, *info.query); + + { + std::unique_lock lock(mutex); + + dataset->addDataPoint(info.query->name, data); + completed_queries++; + + if (complete_callback_) + complete_callback_(dataset, *info.query); + } + + if (timeout_) + { + time_remaining -= data->time; + RBX_INFO( // + "[Thread %1%] Running Query %3% `%2%` till timeout, %4% seconds " + "remaining...", // + id, info.query->name, info.index, time_remaining); + timeout_trial++; + } + else + time_remaining = 0; } - else - time_remaining = 0; - } - RBX_INFO("[Thread %1%] Completed Query %3% `%2%` Trial [%4%/%5%] Total: [%6%/%7%]", // - id, info.query->name, info.index, // - info.trial + 1, trials_, // - completed_queries, total_queries); - } - })); + RBX_INFO("[Thread %1%] Completed Query %3% `%2%` Trial [%4%/%5%] Total: [%6%/%7%]", // + id, info.query->name, info.index, // + info.trial + 1, trials_, // + completed_queries, total_queries); + } + })); for (const auto &thread : threads) thread->join(); diff --git a/robowflex_library/src/detail/ur5.cpp b/robowflex_library/src/detail/ur5.cpp index 88367ca9a..c7dda1212 100644 --- a/robowflex_library/src/detail/ur5.cpp +++ b/robowflex_library/src/detail/ur5.cpp @@ -6,28 +6,28 @@ using namespace robowflex; -const std::string // +const std::string // UR5Robot::DEFAULT_URDF{"package://ur_description/urdf/ur5_robotiq_robot_limited.urdf.xacro"}; -const std::string // +const std::string // UR5Robot::DEFAULT_SRDF{"package://ur5_robotiq85_moveit_config/config/ur5_robotiq85.srdf"}; -const std::string // +const std::string // UR5Robot::DEFAULT_LIMITS{"package://ur5_robotiq85_moveit_config/config/joint_limits.yaml"}; -const std::string // +const std::string // UR5Robot::DEFAULT_KINEMATICS{"package://ur5_robotiq85_moveit_config/config/kinematics.yaml"}; -const std::string // +const std::string // OMPL::UR5OMPLPipelinePlanner::DEFAULT_CONFIG{ "package://ur5_robotiq85_moveit_config/config/ompl_planning.yaml" // }; -const std::string // +const std::string // UR5Robot::RESOURCE_URDF{"package://robowflex_resources/ur/robots/ur5_robotiq_robot_limited.urdf.xacro"}; -const std::string // +const std::string // UR5Robot::RESOURCE_SRDF{"package://robowflex_resources/ur/config/ur5/ur5_robotiq85.srdf.xacro"}; -const std::string // +const std::string // UR5Robot::RESOURCE_LIMITS{"package://robowflex_resources/ur/config/ur5/joint_limits.yaml"}; -const std::string // +const std::string // UR5Robot::RESOURCE_KINEMATICS{"package://robowflex_resources/ur/config/ur5/kinematics.yaml"}; -const std::string // +const std::string // OMPL::UR5OMPLPipelinePlanner::RESOURCE_CONFIG{ "package://robowflex_resources/ur/config/ur5/ompl_planning.yaml" // }; diff --git a/robowflex_library/src/io.cpp b/robowflex_library/src/io.cpp index 8c79108bf..13cf4bdf1 100644 --- a/robowflex_library/src/io.cpp +++ b/robowflex_library/src/io.cpp @@ -10,10 +10,10 @@ #include // for process / thread IDs #include // for filesystem paths #include -#include // for UUID generation -#include // for UUID generation -#include // for UUID generation -#include // for package resolving +#include // for UUID generation +#include // for UUID generation +#include // for UUID generation +#include // for package resolving #include #include diff --git a/robowflex_library/src/io/broadcaster.cpp b/robowflex_library/src/io/broadcaster.cpp index a09aa4207..183a3827c 100644 --- a/robowflex_library/src/io/broadcaster.cpp +++ b/robowflex_library/src/io/broadcaster.cpp @@ -37,19 +37,21 @@ IO::RobotBroadcaster::~RobotBroadcaster() void IO::RobotBroadcaster::start() { active_ = true; - thread_.reset(new std::thread([&]() { - done_ = false; - - while (active_) + thread_.reset(new std::thread( + [&]() { - update(); + done_ = false; + + while (active_) + { + update(); - ros::WallDuration pause(1. / rate_); - pause.sleep(); - } + ros::WallDuration pause(1. / rate_); + pause.sleep(); + } - done_ = true; - })); + done_ = true; + })); } void IO::RobotBroadcaster::stop() diff --git a/robowflex_library/src/io/hdf5.cpp b/robowflex_library/src/io/hdf5.cpp index cb41b371c..db4397ec4 100644 --- a/robowflex_library/src/io/hdf5.cpp +++ b/robowflex_library/src/io/hdf5.cpp @@ -20,19 +20,23 @@ IO::HDF5Data::HDF5Data(const T &location, const std::string &name) , space_(dataset_.getSpace()) , type_(dataset_.getTypeClass()) , rank_(space_.getSimpleExtentNdims()) - , dims_([&] { - auto *dims = new hsize_t[rank_]; - space_.getSimpleExtentDims(dims); - return dims; - }()) - , data_([&] { - const auto &properties = getDataProperties(); - void *data = std::malloc(std::get<1>(properties) * // - std::accumulate(dims_, dims_ + rank_, 1, std::multiplies())); - - dataset_.read(data, std::get<0>(properties), space_, space_); - return data; - }()) + , dims_( + [&] + { + auto *dims = new hsize_t[rank_]; + space_.getSimpleExtentDims(dims); + return dims; + }()) + , data_( + [&] + { + const auto &properties = getDataProperties(); + void *data = std::malloc(std::get<1>(properties) * // + std::accumulate(dims_, dims_ + rank_, 1, std::multiplies())); + + dataset_.read(data, std::get<0>(properties), space_, space_); + return data; + }()) { } diff --git a/robowflex_library/src/io/visualization.cpp b/robowflex_library/src/io/visualization.cpp index a8776f376..8f18daf17 100644 --- a/robowflex_library/src/io/visualization.cpp +++ b/robowflex_library/src/io/visualization.cpp @@ -389,8 +389,8 @@ void IO::RVIZHelper::addGoalMarker(const std::string &name, const moveit_msgs::M const auto tolerances = {og.absolute_x_axis_tolerance, // og.absolute_y_axis_tolerance, // og.absolute_z_axis_tolerance}; - const auto axes = {Eigen::Vector3d::UnitX(), // - Eigen::Vector3d::UnitY(), // + const auto axes = {Eigen::Vector3d::UnitX(), // + Eigen::Vector3d::UnitY(), // Eigen::Vector3d::UnitZ()}; for (const auto &angles : boost::combine(tolerances, axes)) { diff --git a/robowflex_library/src/planning.cpp b/robowflex_library/src/planning.cpp index a7f33dfb7..f04fd713d 100644 --- a/robowflex_library/src/planning.cpp +++ b/robowflex_library/src/planning.cpp @@ -62,23 +62,25 @@ PoolPlanner::PoolPlanner(const RobotPtr &robot, unsigned int n, const std::strin std::shared_ptr> PoolPlanner::submit(const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) { - return pool_.submit(make_function([&] { - std::unique_lock lock(mutex_); - cv_.wait(lock, [&] { return !planners_.empty(); }); + return pool_.submit(make_function( + [&] + { + std::unique_lock lock(mutex_); + cv_.wait(lock, [&] { return !planners_.empty(); }); - auto planner = planners_.front(); - planners_.pop(); + auto planner = planners_.front(); + planners_.pop(); - lock.unlock(); + lock.unlock(); - auto result = planner->plan(scene, request); + auto result = planner->plan(scene, request); - lock.lock(); - planners_.emplace(planner); - cv_.notify_one(); + lock.lock(); + planners_.emplace(planner); + cv_.notify_one(); - return result; - })); + return result; + })); } planning_interface::MotionPlanResponse PoolPlanner::plan(const SceneConstPtr &scene, diff --git a/robowflex_library/src/robot.cpp b/robowflex_library/src/robot.cpp index 931a6e1eb..3f7fc53ec 100644 --- a/robowflex_library/src/robot.cpp +++ b/robowflex_library/src/robot.cpp @@ -421,32 +421,36 @@ bool Robot::loadKinematics(const std::string &group_name, bool load_subgroups) void Robot::setSRDFPostProcessAddPlanarJoint(const std::string &name) { - setSRDFPostProcessFunction([&, name](tinyxml2::XMLDocument &doc) -> bool { - tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); - virtual_joint->SetAttribute("name", name.c_str()); - virtual_joint->SetAttribute("type", "planar"); - virtual_joint->SetAttribute("parent_frame", "world"); - virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); + setSRDFPostProcessFunction( + [&, name](tinyxml2::XMLDocument &doc) -> bool + { + tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); + virtual_joint->SetAttribute("name", name.c_str()); + virtual_joint->SetAttribute("type", "planar"); + virtual_joint->SetAttribute("parent_frame", "world"); + virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); - doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); + doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); - return true; - }); + return true; + }); } void Robot::setSRDFPostProcessAddFloatingJoint(const std::string &name) { - setSRDFPostProcessFunction([&, name](tinyxml2::XMLDocument &doc) -> bool { - tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); - virtual_joint->SetAttribute("name", name.c_str()); - virtual_joint->SetAttribute("type", "floating"); - virtual_joint->SetAttribute("parent_frame", "world"); - virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); + setSRDFPostProcessFunction( + [&, name](tinyxml2::XMLDocument &doc) -> bool + { + tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); + virtual_joint->SetAttribute("name", name.c_str()); + virtual_joint->SetAttribute("type", "floating"); + virtual_joint->SetAttribute("parent_frame", "world"); + virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); - doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); + doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); - return true; - }); + return true; + }); } const std::string &Robot::getModelName() const @@ -655,10 +659,10 @@ Robot::IKQuery::IKQuery(const std::string &group, const RobotPose &pose, double Robot::IKQuery::IKQuery(const std::string &group, // const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation, // double radius, const Eigen::Vector3d &tolerance) - : IKQuery(group, // - Geometry::makeSphere(radius), // - TF::createPoseXYZ(position), // - orientation, // + : IKQuery(group, // + Geometry::makeSphere(radius), // + TF::createPoseXYZ(position), // + orientation, // tolerance) { } @@ -757,34 +761,37 @@ void Robot::IKQuery::addMetric(const Metric &metric_function) void Robot::IKQuery::addDistanceMetric(double weight) { addMetric([weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) { - return weight * result.distance; - }); + const kinematic_constraints::ConstraintEvaluationResult &result) + { return weight * result.distance; }); } void Robot::IKQuery::addCenteringMetric(double weight) { - addMetric([&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) { - const auto &jmg = state.getJointModelGroup(group); - const auto &min = state.getMinDistanceToPositionBounds(jmg); - double extent = min.second->getMaximumExtent() / 2.; - return weight * (extent - min.first) / extent; - }); + addMetric( + [&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, + const kinematic_constraints::ConstraintEvaluationResult &result) + { + const auto &jmg = state.getJointModelGroup(group); + const auto &min = state.getMinDistanceToPositionBounds(jmg); + double extent = min.second->getMaximumExtent() / 2.; + return weight * (extent - min.first) / extent; + }); } void Robot::IKQuery::addClearanceMetric(double weight) { - addMetric([&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) { - if (scene) + addMetric( + [&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, + const kinematic_constraints::ConstraintEvaluationResult &result) { - double v = scene->distanceToCollision(state); - return weight * v; - } + if (scene) + { + double v = scene->distanceToCollision(state); + return weight * v; + } - return 0.; - }); + return 0.; + }); } bool Robot::IKQuery::sampleRegion(RobotPose &pose, std::size_t index) const diff --git a/robowflex_library/src/trajectory.cpp b/robowflex_library/src/trajectory.cpp index 990f97ecf..f65277b84 100644 --- a/robowflex_library/src/trajectory.cpp +++ b/robowflex_library/src/trajectory.cpp @@ -238,10 +238,9 @@ double Trajectory::getSmoothness(const PathMetric &metric) const { double smoothness = 0.0; - auto distance = - (metric) ? metric : [](const robot_state::RobotState &a, const robot_state::RobotState &b) { - return a.distance(b); - }; + auto distance = (metric) ? metric : + [](const robot_state::RobotState &a, const robot_state::RobotState &b) + { return a.distance(b); }; // compute smoothness if (trajectory_->getWayPointCount() > 2) diff --git a/robowflex_library/src/yaml.cpp b/robowflex_library/src/yaml.cpp index 381c525f7..ae51c5c6a 100644 --- a/robowflex_library/src/yaml.cpp +++ b/robowflex_library/src/yaml.cpp @@ -1258,7 +1258,8 @@ namespace YAML bool convert::decode(const Node &node, ros::Duration &rhs) { - if (node.Type() == NodeType::Scalar) { + if (node.Type() == NodeType::Scalar) + { rhs.fromSec(node.as()); return true; } diff --git a/robowflex_movegroup/include/robowflex_movegroup/services.h b/robowflex_movegroup/include/robowflex_movegroup/services.h index 4c0d83d39..0d59c8bfa 100644 --- a/robowflex_movegroup/include/robowflex_movegroup/services.h +++ b/robowflex_movegroup/include/robowflex_movegroup/services.h @@ -118,19 +118,19 @@ namespace robowflex actionlib::SimpleActionClient eac_; ///< Execute trajectory ///< client. - ResultCallback callback_; ///< Callback function for move group results. + ResultCallback callback_; ///< Callback function for move group results. std::map requests_; ///< Move group requests - RobotPtr robot_; ///< Robot on the parameter server used by move group. + RobotPtr robot_; ///< Robot on the parameter server used by move group. - static const std::string MOVE_GROUP; ///< Name of move_group namespace. - static const std::string GET_SCENE; ///< Name of get scene service. - static const std::string APPLY_SCENE; ///< Name of apply scene service. - static const std::string CLEAR_OCTOMAP; ///< Name of clear octomap service. - static const std::string EXECUTE; ///< Name of execute trajectory service. + static const std::string MOVE_GROUP; ///< Name of move_group namespace. + static const std::string GET_SCENE; ///< Name of get scene service. + static const std::string APPLY_SCENE; ///< Name of apply scene service. + static const std::string CLEAR_OCTOMAP; ///< Name of clear octomap service. + static const std::string EXECUTE; ///< Name of execute trajectory service. }; - } // namespace movegroup + } // namespace movegroup } // namespace robowflex #endif diff --git a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h index 7924eb280..b20d489ca 100644 --- a/robowflex_ompl/include/robowflex_ompl/ompl_interface.h +++ b/robowflex_ompl/include/robowflex_ompl/ompl_interface.h @@ -121,16 +121,16 @@ namespace robowflex bool hybridize_; ///< Whether or not planner should hybridize solutions. bool interpolate_; ///< Whether or not planner should interpolate solutions. - mutable ID::Key last_scene_id_{ID::getNullKey()}; ///< ID of last scene. - mutable std::string last_request_hash_; ///< Hash of last request. + mutable ID::Key last_scene_id_{ID::getNullKey()}; ///< ID of last scene. + mutable std::string last_request_hash_; ///< Hash of last request. mutable ompl_interface::ModelBasedPlanningContextPtr context_; ///< Last context. mutable ompl::geometric::SimpleSetupPtr ss_; ///< Last OMPL simple setup used for ///< planning. - PrePlanCallback pre_plan_callback_; ///< Callback to be called just before planning. + PrePlanCallback pre_plan_callback_; ///< Callback to be called just before planning. }; - } // namespace OMPL + } // namespace OMPL } // namespace robowflex #endif diff --git a/robowflex_ompl/scripts/fetch_ompl_benchmark.cpp b/robowflex_ompl/scripts/fetch_ompl_benchmark.cpp index f1dd01eee..a15d26adf 100644 --- a/robowflex_ompl/scripts/fetch_ompl_benchmark.cpp +++ b/robowflex_ompl/scripts/fetch_ompl_benchmark.cpp @@ -20,7 +20,8 @@ Profiler::ComputeMetricCallback getNumVerticesCallback() return [](const PlannerPtr &planner, // const SceneConstPtr &scene, // const planning_interface::MotionPlanRequest &request, // - const PlanData &run) -> PlannerMetric { + const PlanData &run) -> PlannerMetric + { const auto &ompl_planner = std::dynamic_pointer_cast(planner); const auto &op = ompl_planner->getLastSimpleSetup()->getPlanner(); @@ -36,7 +37,8 @@ Profiler::ComputeMetricCallback getGoalDistanceCallback() return [](const PlannerPtr &planner, // const SceneConstPtr &scene, // const planning_interface::MotionPlanRequest &request, // - const PlanData &run) -> PlannerMetric { + const PlanData &run) -> PlannerMetric + { const auto &ompl_planner = std::dynamic_pointer_cast(planner); const auto &pdef = ompl_planner->getLastSimpleSetup()->getProblemDefinition(); diff --git a/robowflex_ompl/scripts/fetch_profile.cpp b/robowflex_ompl/scripts/fetch_profile.cpp index 30b951cd9..71b2af563 100644 --- a/robowflex_ompl/scripts/fetch_profile.cpp +++ b/robowflex_ompl/scripts/fetch_profile.cpp @@ -38,7 +38,8 @@ Profiler::ProgressCallback getGNUPlotCallback(IO::GNUPlotHelper &plotter, const return [&, field](const PlannerPtr &planner, // const SceneConstPtr &scene, // const planning_interface::MotionPlanRequest &request, // - const PlanData &result) { + const PlanData &result) + { IO::GNUPlotHelper::TimeSeriesOptions tso; // Plotting options for time series data tso.instance = field; tso.title = "Live Profiling"; @@ -60,13 +61,15 @@ Profiler::ProgressPropertyAllocator getNumVerticesAllocator() { return [](const PlannerPtr &planner, // const SceneConstPtr &scene, // - const planning_interface::MotionPlanRequest &request) -> Planner::ProgressProperty { + const planning_interface::MotionPlanRequest &request) -> Planner::ProgressProperty + { // Extract OMPL level information const auto &ompl_planner = std::dynamic_pointer_cast(planner); const auto &ss = ompl_planner->getLastSimpleSetup(); const auto &op = ss->getPlanner(); - return [op] { + return [op] + { ompl::base::PlannerData pd(op->getSpaceInformation()); op->getPlannerData(pd); @@ -82,7 +85,8 @@ Profiler::ProgressCallbackAllocator getRVIZGraphVisualizationAllocator(IO::RVIZH { return [rviz](const PlannerPtr &planner, // const SceneConstPtr &scene, // - const planning_interface::MotionPlanRequest &request) -> Profiler::ProgressCallback { + const planning_interface::MotionPlanRequest &request) -> Profiler::ProgressCallback + { // Extract OMPL level information const auto &ompl_planner = std::dynamic_pointer_cast(planner); const auto &ss = ompl_planner->getLastSimpleSetup(); @@ -91,7 +95,8 @@ Profiler::ProgressCallbackAllocator getRVIZGraphVisualizationAllocator(IO::RVIZH return [rviz, op](const PlannerPtr &planner, // const SceneConstPtr &scene, // const planning_interface::MotionPlanRequest &request, // - const PlanData &result) { + const PlanData &result) + { const auto &robot = planner->getRobot(); const auto &ss = op->getSpaceInformation()->getStateSpace(); @@ -156,7 +161,8 @@ Profiler::ComputeMetricCallback getGoalDistanceCallback() return [](const PlannerPtr &planner, // const SceneConstPtr &scene, // const planning_interface::MotionPlanRequest &request, // - const PlanData &run) -> PlannerMetric { + const PlanData &run) -> PlannerMetric + { const auto &ompl_planner = std::dynamic_pointer_cast(planner); const auto &pdef = ompl_planner->getLastSimpleSetup()->getProblemDefinition(); diff --git a/robowflex_tesseract/include/robowflex_tesseract/conversions.h b/robowflex_tesseract/include/robowflex_tesseract/conversions.h index c92e07aae..70a7fa377 100644 --- a/robowflex_tesseract/include/robowflex_tesseract/conversions.h +++ b/robowflex_tesseract/include/robowflex_tesseract/conversions.h @@ -23,7 +23,7 @@ namespace robowflex * \return True if the KDL environment was correctly loaded from scene. */ bool sceneToTesseractEnv(const robowflex::SceneConstPtr &scene, - tesseract::tesseract_ros::KDLEnvPtr env); + tesseract::tesseract_ros::KDLEnvPtr &env); /** \brief Add bodies attached to the robot scratch state to the KDL environment. * \param[in] state Robot state with objects attached. @@ -31,7 +31,7 @@ namespace robowflex * \return True if the KDL environment was correctly updated. */ bool addAttachedBodiesToTesseractEnv(const robot_state::RobotStatePtr &state, - tesseract::tesseract_ros::KDLEnvPtr env); + tesseract::tesseract_ros::KDLEnvPtr &env); /** \brief Transform a \a robot_state to a vector representing joint values for the manipulator (in * the order given by \a manip_joint_names). @@ -65,7 +65,7 @@ namespace robowflex const robot_state::RobotStatePtr &ref_state, const std::string &manip, const tesseract::tesseract_ros::KDLEnvPtr &env, - robot_trajectory::RobotTrajectoryPtr trajectory); + robot_trajectory::RobotTrajectoryPtr &trajectory); /** \brief Transform a \a robot_trajectory to a tesseract manipulator \a trajectory. * \param[in] robot_traj Robot Trajectory to transform. diff --git a/robowflex_tesseract/include/robowflex_tesseract/trajopt_planner.h b/robowflex_tesseract/include/robowflex_tesseract/trajopt_planner.h index cb171470e..38187c4db 100644 --- a/robowflex_tesseract/include/robowflex_tesseract/trajopt_planner.h +++ b/robowflex_tesseract/include/robowflex_tesseract/trajopt_planner.h @@ -146,8 +146,7 @@ namespace robowflex */ const robot_trajectory::RobotTrajectoryPtr &getTrajectory() const; - /** \brief Get the trajectory that resulted in the last call to plan() in Tesseract - * format. + /** \brief Get the trajectory that resulted in the last call to plan() in Tesseract format. * \return Last trajectory computed using plan(). */ const trajopt::TrajArray &getTesseractTrajectory() const; diff --git a/robowflex_tesseract/scripts/fetch_tabletop_goalpose.cpp b/robowflex_tesseract/scripts/fetch_tabletop_goalpose.cpp index ffeabfa10..e10d42dae 100644 --- a/robowflex_tesseract/scripts/fetch_tabletop_goalpose.cpp +++ b/robowflex_tesseract/scripts/fetch_tabletop_goalpose.cpp @@ -63,9 +63,13 @@ int main(int argc, char **argv) // Do motion planning using a goal pose for the end effector. auto result = planner->plan(scene, start_state, goal_ee_pose, ee); - if (result.first) - rviz->updateTrajectory(planner->getTrajectory()); + if (not result.first) + { + RBX_INFO("Trajectory could not be found"); + return 0; + } + rviz->updateTrajectory(planner->getTrajectory()); rviz->visualizeState(planner->getTrajectory()->getLastWayPointPtr()); RBX_INFO("Visualizing end state"); diff --git a/robowflex_tesseract/scripts/fetch_trajopt.cpp b/robowflex_tesseract/scripts/fetch_trajopt.cpp index 481314c52..b51d71001 100644 --- a/robowflex_tesseract/scripts/fetch_trajopt.cpp +++ b/robowflex_tesseract/scripts/fetch_trajopt.cpp @@ -1,8 +1,7 @@ /* Author: Carlos Quintero Pena */ -#include +#include #include -#include #include #include #include @@ -33,7 +32,11 @@ int main(int argc, char **argv) // Create a TrajOpt planner for Fetch. auto planner = std::make_shared(fetch, GROUP); - planner->initialize("torso_lift_link", "gripper_link"); + if (not planner->initialize("torso_lift_link", "gripper_link")) + { + RBX_ERROR("Planner could not be initialized"); + return 1; + } planner->options.num_waypoints = 3; // Create a motion planning request with a pose goal. diff --git a/robowflex_tesseract/src/conversions.cpp b/robowflex_tesseract/src/conversions.cpp index 1c7b8b819..da974f913 100644 --- a/robowflex_tesseract/src/conversions.cpp +++ b/robowflex_tesseract/src/conversions.cpp @@ -15,7 +15,7 @@ using namespace robowflex; bool hypercube::sceneToTesseractEnv(const robowflex::SceneConstPtr &scene, - tesseract::tesseract_ros::KDLEnvPtr env) + tesseract::tesseract_ros::KDLEnvPtr &env) { if (env->checkInitialized()) { @@ -120,7 +120,7 @@ bool hypercube::sceneToTesseractEnv(const robowflex::SceneConstPtr &scene, } bool hypercube::addAttachedBodiesToTesseractEnv(const robot_state::RobotStatePtr &state, - tesseract::tesseract_ros::KDLEnvPtr env) + tesseract::tesseract_ros::KDLEnvPtr &env) { moveit_msgs::RobotState state_msg; moveit::core::robotStateToRobotStateMsg(*state, state_msg); @@ -216,7 +216,7 @@ void hypercube::manipTesseractTrajToRobotTraj(const tesseract::TrajArray &tesser const robot_state::RobotStatePtr &ref_state, const std::string &manip, const tesseract::tesseract_ros::KDLEnvPtr &env, - robot_trajectory::RobotTrajectoryPtr trajectory) + robot_trajectory::RobotTrajectoryPtr &trajectory) { const robot_state::RobotState © = *ref_state; trajectory->clear(); diff --git a/robowflex_tesseract/src/trajopt_planner.cpp b/robowflex_tesseract/src/trajopt_planner.cpp index 7b882f19c..14458e8f6 100644 --- a/robowflex_tesseract/src/trajopt_planner.cpp +++ b/robowflex_tesseract/src/trajopt_planner.cpp @@ -2,7 +2,6 @@ // MoveIt #include -#include // Robowflex #include @@ -76,14 +75,14 @@ bool TrajOptPlanner::initialize(const std::string &base_link, const std::string if (options.verbose) RBX_INFO("Adding manipulator %s from %s to %s", manip_, base_link, tip_link); - TiXmlDocument srdf_doc; + tinyxml2::XMLDocument srdf_doc; srdf_doc.Parse(robot_->getSRDFString().c_str()); - auto *group_element = new TiXmlElement("group"); + auto *group_element = srdf_doc.NewElement("group"); group_element->SetAttribute("name", manip_.c_str()); srdf_doc.FirstChildElement("robot")->LinkEndChild(group_element); - auto *chain_element = new TiXmlElement("chain"); + auto *chain_element = srdf_doc.NewElement("chain"); chain_element->SetAttribute("base_link", base_link.c_str()); chain_element->SetAttribute("tip_link", tip_link.c_str()); group_element->LinkEndChild(chain_element); @@ -173,15 +172,19 @@ double TrajOptPlanner::getPlanningTime() const void TrajOptPlanner::fixJoints(const std::vector &joints) { if (!env_->hasManipulator(manip_)) + { throw Exception(1, "There is no loaded manipulator!"); + } else { - const auto &joint_names = env_->getManipulator(manip_)->getJointNames(); + auto joint_names = env_->getManipulator(manip_)->getJointNames(); for (const auto &name : joints) { auto it = std::find(joint_names.begin(), joint_names.end(), name); if (it == joint_names.end()) - throw Exception(1, "One of the joints to be fixed does not exist"); + { + throw Exception(1, "Joint to be fixed does not exist"); + } else { int index = std::distance(joint_names.begin(), it); @@ -268,6 +271,7 @@ TrajOptPlanner::PlannerResult TrajOptPlanner::plan(const SceneConstPtr &scene, // Add collision costs to all waypoints in the trajectory. options.default_safety_margin_coeffs = options.joint_state_safety_margin_coeffs; + addCollisionAvoidance(pci); // Add goal state. @@ -294,7 +298,7 @@ TrajOptPlanner::PlannerResult TrajOptPlanner::plan(const SceneConstPtr &scene, auto link_it = std::find(begin_it, end_it, link); if (link_it == end_it) { - RBX_ERROR("Link %s is not part of robot manipulator", link); + RBX_ERROR("Link %s is not part of robot manipulator", link.c_str()); return PlannerResult(false, false); } @@ -344,7 +348,7 @@ TrajOptPlanner::PlannerResult TrajOptPlanner::plan(const SceneConstPtr &scene, auto link_it = std::find(begin_it, end_it, link); if (link_it == end_it) { - RBX_ERROR("Link %s is not part of robot manipulator", link); + RBX_ERROR("Link %s is not part of robot manipulator", link.c_str()); return PlannerResult(false, false); } @@ -389,7 +393,8 @@ TrajOptPlanner::PlannerResult TrajOptPlanner::plan(const SceneConstPtr &scene, c auto goal_it = std::find(begin_it, end_it, goal_link); if ((start_it == end_it) or (goal_it == end_it)) { - RBX_ERROR("Given links %s or %s are not part of robot manipulator", start_link, goal_link); + RBX_ERROR("Given links %s or %s are not part of robot manipulator", start_link.c_str(), + goal_link.c_str()); return PlannerResult(false, false); }