From a7879213aee9194804c4a8d6d12bea7e75258411 Mon Sep 17 00:00:00 2001 From: CarlosQuinteroPena Date: Tue, 28 Jun 2022 16:02:02 -0500 Subject: [PATCH 1/7] Add Stretch robot class and scripts --- robowflex_library/CMakeLists.txt | 4 +- .../robowflex_library/detail/stretch.h | 102 +++++++++++++++++ robowflex_library/scripts/stretch_test.cpp | 67 ++++++++++++ .../scripts/stretch_visualization.cpp | 93 ++++++++++++++++ robowflex_library/src/detail/stretch.cpp | 103 ++++++++++++++++++ robowflex_library/yaml/test_stretch.yml | 45 ++++++++ 6 files changed, 413 insertions(+), 1 deletion(-) create mode 100644 robowflex_library/include/robowflex_library/detail/stretch.h create mode 100644 robowflex_library/scripts/stretch_test.cpp create mode 100644 robowflex_library/scripts/stretch_visualization.cpp create mode 100644 robowflex_library/src/detail/stretch.cpp create mode 100644 robowflex_library/yaml/test_stretch.yml diff --git a/robowflex_library/CMakeLists.txt b/robowflex_library/CMakeLists.txt index 6052cc570..ba7e84ca9 100644 --- a/robowflex_library/CMakeLists.txt +++ b/robowflex_library/CMakeLists.txt @@ -86,6 +86,7 @@ list(APPEND SOURCES src/detail/ur5.cpp src/detail/fetch.cpp src/detail/cob4.cpp + src/detail/stretch.cpp ) list(APPEND INCLUDES @@ -140,7 +141,8 @@ add_script(shadowhand_ik) add_script(cob4_test) add_script(cob4_visualization) add_script(cob4_multi_target) - +add_script(stretch_test) +add_script(stretch_visualization) ## ## Tests ## diff --git a/robowflex_library/include/robowflex_library/detail/stretch.h b/robowflex_library/include/robowflex_library/detail/stretch.h new file mode 100644 index 000000000..4868fa37a --- /dev/null +++ b/robowflex_library/include/robowflex_library/detail/stretch.h @@ -0,0 +1,102 @@ +/* Author: Carlos Quintero-Peña */ + +#ifndef ROBOWFLEX_STRETCH_ +#define ROBOWFLEX_STRETCH_ + +#include +#include + +namespace robowflex +{ + /** \cond IGNORE */ + ROBOWFLEX_CLASS_FORWARD(StretchRobot); + /* \endcond */ + + /** \class robowflex::StretchRobotPtr + \brief A shared pointer wrapper for robowflex::StretchRobot. */ + + /** \class robowflex::StretchRobotConstPtr + \brief A const shared pointer wrapper for robowflex::StretchRobot. */ + + /** \brief Convenience class that describes the default setup for Stretch. + * Will first attempt to load configuration and description from the robowflex_resources package. + * See https://github.com/KavrakiLab/robowflex_resources for this package. + * If this package is not available, then stretch_description / stretch_moveit_config packages will be + * used. + */ + class StretchRobot : public Robot + { + public: + /** \brief Constructor. + */ + StretchRobot(); + + /** \brief Initialize the robot with stretch_arm, stretch_gripper and stretch_head kinematics. + * \return True on success, false on failure. + */ + bool initialize(); + + /** \brief Points the Stretch's head to a point in the world frame. + * \param[in] point The point to look at. + */ + void pointHead(const Eigen::Vector3d &point); + + /** \brief Opens the Stretch's gripper. + */ + void openGripper(); + + /** \brief Closes the Stretch's gripper. + */ + 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 RESOURCE_URDF; ///< URDF from robowflex_resources + static const std::string RESOURCE_SRDF; ///< SRDF from robowflex_resources + static const std::string RESOURCE_LIMITS; ///< Limits from robowflex_resources + static const std::string RESOURCE_KINEMATICS; ///< kinematics from robowflex_resources + }; + + namespace OMPL + { + /** \cond IGNORE */ + ROBOWFLEX_CLASS_FORWARD(StretchOMPLPipelinePlanner); + /* \endcond */ + + /** \class robowflex::OMPL::StretchOMPLPipelinePlannerPtr + \brief A shared pointer wrapper for robowflex::OMPL::StretchOMPLPipelinePlanner. */ + + /** \class robowflex::OMPL::StretchOMPLPipelinePlannerConstPtr + \brief A const shared pointer wrapper for robowflex::OMPL::StretchOMPLPipelinePlanner. */ + + /** \brief Convenience class for the default motion planning pipeline for Stretch. + */ + class StretchOMPLPipelinePlanner : public OMPLPipelinePlanner + { + public: + /** \brief Constructor. + * \param[in] robot Robot to create planner for. + * \param[in] name Namespace of this planner. + */ + StretchOMPLPipelinePlanner(const RobotPtr &robot, const std::string &name = ""); + + /** \brief Initialize the planning context. All parameter provided are defaults. + * \param[in] settings Settings to set on the parameter server. + * \param[in] adapters Planning adapters to load. + * \return True on success, false on failure. + */ + bool initialize(const Settings &settings = Settings(), + const std::vector &adapters = DEFAULT_ADAPTERS); + + private: + static const std::string DEFAULT_CONFIG; ///< Default planning configuration. + static const std::string RESOURCE_CONFIG; ///< Planning configuration from robowflex_resources. + }; + } // namespace OMPL +} // namespace robowflex + +#endif diff --git a/robowflex_library/scripts/stretch_test.cpp b/robowflex_library/scripts/stretch_test.cpp new file mode 100644 index 000000000..e2d3d5044 --- /dev/null +++ b/robowflex_library/scripts/stretch_test.cpp @@ -0,0 +1,67 @@ +/* Author: Carlos Quintero-Peña */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace robowflex; + +/* \file stretch_test.cpp + * A simple script that demonstrates motion planning with the Stretch robot. The + * resulting trajectory is output to a YAML file. This file can be visualized + * using Blender. See the corresponding robowflex_visualization readme. + */ + +static const std::string GROUP = "stretch_arm"; + +int main(int argc, char **argv) +{ + // Startup ROS + ROS ros(argc, argv); + + // Create the default Stretch robot. + auto stretch = std::make_shared(); + stretch->initialize(); + + // Open Gripper + stretch->openGripper(); + + // Create an empty scene. + auto scene = std::make_shared(stretch); + scene->toYAMLFile("ex_stretch.yml"); + + // Create the default planner for the Stretch. + auto planner = std::make_shared(stretch, "default"); + planner->initialize(); + + // Sets the Stretch's head pose to look at a point. + stretch->pointHead({0, -2, 10}); + + // Create a motion planning request with a pose goal. + MotionRequestBuilder request(planner, GROUP); + stretch->setGroupState(GROUP, {0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0}); // home + request.setStartConfiguration(stretch->getScratchState()); + + stretch->setGroupState(GROUP, {0.9967, 0.0, 0.13, 0.13, 0.13, 0.13, 4}); // extended + request.setGoalConfiguration(stretch->getScratchState()); + + request.setConfig("RRTConnect"); + + // Do motion planning! + planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); + if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) + return 1; + + // Create a trajectory object for better manipulation. + auto trajectory = std::make_shared(res.trajectory_); + + // Output path to a file for visualization. + trajectory->toYAMLFile("stretch_path.yml"); + + return 0; +} diff --git a/robowflex_library/scripts/stretch_visualization.cpp b/robowflex_library/scripts/stretch_visualization.cpp new file mode 100644 index 000000000..3bc61f323 --- /dev/null +++ b/robowflex_library/scripts/stretch_visualization.cpp @@ -0,0 +1,93 @@ +/* Author: Carlos Quintero-Peña */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace robowflex; + +/* \file stretch_visualization.cpp + * A simple script that demonstrates how to use RViz with Robowflex with the + * Stretch robot. See https://kavrakilab.github.io/robowflex/rviz.html for how to + * use RViz visualization. Here, the scene, start/goal states, and motion plan + * displayed in RViz. + */ + +static const std::string GROUP = "stretch_arm"; + +int main(int argc, char **argv) +{ + // Startup ROS + ROS ros(argc, argv); + + // Create the default Stretch robot. + auto stretch = std::make_shared(); + stretch->initialize(); + + // Create an RViz visualization helper. Publishes all topics and parameter under `/robowflex` by + // default. + IO::RVIZHelper rviz(stretch); + IO::RobotBroadcaster bc(stretch); + bc.start(); + + RBX_INFO("RViz Initialized! Press enter to continue (after your RViz is setup)..."); + std::cin.get(); + + // Load a scene from a YAML file. + auto scene = std::make_shared(stretch); + scene->fromYAMLFile("package://robowflex_library/yaml/test_stretch.yml"); + + // Create the default planner for the Stretch. + auto planner = std::make_shared(stretch, "default"); + planner->initialize(); + + // Create a motion planning request. + MotionRequestBuilder request(planner, GROUP); + stretch->setGroupState(GROUP, {0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0}); // home + request.setStartConfiguration(stretch->getScratchState()); + + // Visualize the scene (start state) in RViz. + rviz.updateScene(scene); + ROS_INFO("Visualizing start state"); + std::cin.get(); + + // Create IK query. + auto query = + Robot::IKQuery(GROUP, "link_wrist_yaw", *stretch->getScratchState(), Eigen::Vector3d{0, 0.23, -0.71}); + query.scene = scene; + stretch->setFromIK(query); + request.setGoalConfiguration(stretch->getScratchState()); + + // Visualize the goal state in RViz. + scene->getCurrentState() = *stretch->getScratchState(); + rviz.updateScene(scene); + ROS_INFO("Visualizing goal state"); + std::cin.get(); + + // Do motion planning! + planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest()); + if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS) + return 1; + + // Publish the trajectory to a topic to display in RViz. + rviz.updateTrajectory(res); + RBX_INFO("Visualizing trajectory."); + std::cin.get(); + + // Create a trajectory object for better manipulation. + auto trajectory = std::make_shared(res.trajectory_); + + // Output path to a file for visualization. + trajectory->toYAMLFile("stretch_pick.yml"); + + RBX_INFO("Press enter to exit."); + std::cin.get(); + + return 0; +} diff --git a/robowflex_library/src/detail/stretch.cpp b/robowflex_library/src/detail/stretch.cpp new file mode 100644 index 000000000..32514d2d8 --- /dev/null +++ b/robowflex_library/src/detail/stretch.cpp @@ -0,0 +1,103 @@ +/* Author: Carlos Quintero-Peña */ + +#include + +#include +#include +#include + +using namespace robowflex; + +const std::string StretchRobot::DEFAULT_URDF{"package://stretch_description/urdf/stretch_description.xacro"}; +const std::string StretchRobot::DEFAULT_SRDF{"package://stretch_moveit_config/config/" + "stretch_description.srdf"}; +const std::string StretchRobot::DEFAULT_LIMITS{"package://stretch_moveit_config/config/joint_limits.yaml"}; +const std::string StretchRobot::DEFAULT_KINEMATICS{"package://stretch_moveit_config/config/kinematics.yaml"}; +const std::string // + OMPL::StretchOMPLPipelinePlanner::DEFAULT_CONFIG{"package://stretch_moveit_config/config/" + "ompl_planning.yaml"}; + +const std::string StretchRobot::RESOURCE_URDF{"package://robowflex_resources/stretch/urdf/" + "stretch_description.xacro"}; +const std::string StretchRobot::RESOURCE_SRDF{"package://robowflex_resources/stretch/config/" + "stretch_description.srdf"}; +const std::string StretchRobot::RESOURCE_LIMITS{"package://robowflex_resources/stretch/config/" + "joint_limits.yaml"}; +const std::string // + StretchRobot::RESOURCE_KINEMATICS{"package://robowflex_resources/stretch/config/kinematics.yaml"}; +const std::string // + OMPL::StretchOMPLPipelinePlanner::RESOURCE_CONFIG{"package://robowflex_resources/stretch/config/" + "ompl_planning.yaml"}; + +StretchRobot::StretchRobot() : Robot("stretch") +{ +} + +bool StretchRobot::initialize() +{ + bool success = false; + + // First attempt the `robowflex_resources` package, then attempt the "actual" resource files. + if (IO::resolvePackage(RESOURCE_URDF).empty() or IO::resolvePackage(RESOURCE_SRDF).empty()) + { + RBX_INFO("Initializing Stretch with `stretch_{description, moveit_config}`"); + success = Robot::initialize(DEFAULT_URDF, DEFAULT_SRDF, DEFAULT_LIMITS, DEFAULT_KINEMATICS); + } + else + { + RBX_INFO("Initializing Stretch with `robowflex_resources`"); + success = Robot::initialize(RESOURCE_URDF, RESOURCE_SRDF, RESOURCE_LIMITS, RESOURCE_KINEMATICS); + } + + loadKinematics("stretch_arm"); + loadKinematics("stretch_gripper"); + loadKinematics("stretch_head"); + + StretchRobot::openGripper(); + + return success; +} + +void StretchRobot::pointHead(const Eigen::Vector3d &point) +{ + const RobotPose point_pose = RobotPose(Eigen::Translation3d(point)); + const RobotPose point_pan = getLinkTF("link_head_pan").inverse() * point_pose; + const RobotPose point_tilt = getLinkTF("link_head_tilt").inverse() * point_pose; + + const double pan = atan2(point_pan.translation().y(), point_pan.translation().x()); + const double tilt = -atan2(point_tilt.translation().z(), + hypot(point_tilt.translation().x(), point_tilt.translation().y())); + + const std::map angles = {{"joint_head_pan", pan}, {"joint_head_tilt", tilt}}; + + Robot::setState(angles); +} + +void StretchRobot::openGripper() +{ + const std::map angles = {{"joint_gripper_finger_left", 0.3}, + {"joint_gripper_finger_right", 0.3}}; + + Robot::setState(angles); +} + +void StretchRobot::closeGripper() +{ + const std::map angles = {{"joint_gripper_finger_left", 0.0}, + {"joint_gripper_finger_right", 0.0}}; + + Robot::setState(angles); +} + +OMPL::StretchOMPLPipelinePlanner::StretchOMPLPipelinePlanner(const RobotPtr &robot, const std::string &name) + : OMPLPipelinePlanner(robot, name) +{ +} + +bool OMPL::StretchOMPLPipelinePlanner::initialize(const Settings &settings, + const std::vector &adapters) +{ + if (IO::resolvePackage(RESOURCE_CONFIG).empty()) + return OMPLPipelinePlanner::initialize(DEFAULT_CONFIG, settings, DEFAULT_PLUGIN, adapters); + return OMPLPipelinePlanner::initialize(RESOURCE_CONFIG, settings, DEFAULT_PLUGIN, adapters); +} diff --git a/robowflex_library/yaml/test_stretch.yml b/robowflex_library/yaml/test_stretch.yml new file mode 100644 index 000000000..b6ce2059c --- /dev/null +++ b/robowflex_library/yaml/test_stretch.yml @@ -0,0 +1,45 @@ +name: (noname) +robot_state: + joint_state: + position: [0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0] + header: + frame_id: base_link + name: [joint_left_wheel, joint_head_pan, joint_head_tilt, joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw, joint_gripper_finger_left, joint_gripper_finger_right, joint_right_wheel] +robot_model_name: stretch +fixed_frame_transforms: + - header: + frame_id: base_link + child_frame_id: base_link + transform: + translation: [0, 0, 0] + rotation: [0, 0, 0, 1] +world: + collision_objects: + - id: table_right + meshes: + - resource: "package://robowflex_resources/objects/cafe_table.dae" + dimensions: [0.025, 0.025, 0.025] + mesh_poses: + - position: [0, -1.0, -0.08] + orientation: [0, 0, 0, 1] + - id: Cube1 + primitives: + - type: box + dimensions: [0.07, 0.07, 0.07] + primitive_poses: + - position: [-0.225, -0.68, 0.72] + orientation: [0, 0, 0, 1] + - id: Cube2 + primitives: + - type: box + dimensions: [0.07, 0.07, 0.07] + primitive_poses: + - position: [-0.025, -0.68, 0.72] + orientation: [0, 0, 0, 1] + - id: Cube3 + primitives: + - type: box + dimensions: [0.07, 0.07, 0.07] + primitive_poses: + - position: [0.175, -0.68, 0.72] + orientation: [0, 0, 0, 1] From 4030faea288d421722459e54d629a8994dfbf521 Mon Sep 17 00:00:00 2001 From: CarlosQuinteroPena Date: Thu, 7 Jul 2022 22:26:47 -0500 Subject: [PATCH 2/7] add stretch class and test scripts --- .../robowflex_library/detail/stretch.h | 4 +- .../scripts/stretch_visualization.cpp | 51 +++++++++++++++---- robowflex_library/src/detail/stretch.cpp | 22 +++++++- robowflex_library/src/robot.cpp | 7 ++- robowflex_library/yaml/test_stretch.yml | 8 +-- 5 files changed, 73 insertions(+), 19 deletions(-) diff --git a/robowflex_library/include/robowflex_library/detail/stretch.h b/robowflex_library/include/robowflex_library/detail/stretch.h index 4868fa37a..8ad96cbd7 100644 --- a/robowflex_library/include/robowflex_library/detail/stretch.h +++ b/robowflex_library/include/robowflex_library/detail/stretch.h @@ -34,7 +34,7 @@ namespace robowflex /** \brief Initialize the robot with stretch_arm, stretch_gripper and stretch_head kinematics. * \return True on success, false on failure. */ - bool initialize(); + bool initialize(bool addVirtual = true); /** \brief Points the Stretch's head to a point in the world frame. * \param[in] point The point to look at. @@ -48,6 +48,8 @@ namespace robowflex /** \brief Closes the Stretch's gripper. */ void closeGripper(); + + void setBasePose(double x, double y, double theta); private: static const std::string DEFAULT_URDF; ///< Default URDF diff --git a/robowflex_library/scripts/stretch_visualization.cpp b/robowflex_library/scripts/stretch_visualization.cpp index 3bc61f323..d53380c56 100644 --- a/robowflex_library/scripts/stretch_visualization.cpp +++ b/robowflex_library/scripts/stretch_visualization.cpp @@ -19,7 +19,7 @@ using namespace robowflex; * displayed in RViz. */ -static const std::string GROUP = "stretch_arm"; +static const std::string GROUP = "mobile_base_manipulator"; int main(int argc, char **argv) { @@ -28,7 +28,7 @@ int main(int argc, char **argv) // Create the default Stretch robot. auto stretch = std::make_shared(); - stretch->initialize(); + stretch->initialize(true); // Create an RViz visualization helper. Publishes all topics and parameter under `/robowflex` by // default. @@ -46,22 +46,53 @@ int main(int argc, char **argv) // Create the default planner for the Stretch. auto planner = std::make_shared(stretch, "default"); planner->initialize(); - - // Create a motion planning request. - MotionRequestBuilder request(planner, GROUP); - stretch->setGroupState(GROUP, {0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0}); // home - request.setStartConfiguration(stretch->getScratchState()); - + +// stretch->setBasePose(1.0, 0.0, 0.0); + // Visualize the scene (start state) in RViz. + scene->getCurrentState() = *stretch->getScratchState(); rviz.updateScene(scene); ROS_INFO("Visualizing start state"); std::cin.get(); + + // Create a motion planning request. + MotionRequestBuilder request(planner, GROUP); + stretch->setGroupState(GROUP, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0}); // home + + std::cout << "Number of dofs of this group " << stretch->getScratchState()->getJointModelGroup(GROUP)->getVariableCount() << std::endl; + for (const auto &name : stretch->getScratchState()->getJointModelGroup(GROUP)->getActiveJointModelNames()) + std::cout << name << std::endl; + + const auto &solver = stretch->getScratchState()->getJointModelGroup(GROUP)->getSolverInstance(); + if (solver) + std::cout << "Pointer is not null!" << std::endl; + + auto frames = solver->getTipFrame(); + std::cout << frames << std::endl; + + std::cin.get(); + + // Set start configuration. + request.setStartConfiguration(stretch->getScratchState()); // Create IK query. auto query = - Robot::IKQuery(GROUP, "link_wrist_yaw", *stretch->getScratchState(), Eigen::Vector3d{0, 0.23, -0.71}); + Robot::IKQuery(GROUP, "link_wrist_yaw", *stretch->getScratchState(), Eigen::Vector3d{-0.2, 0.23, -0.71}); query.scene = scene; - stretch->setFromIK(query); + auto tips(query.tips); + std::cout << "Number of tips " << tips.size() << ", tip: " << tips[0] << std::endl; +// std::cout << stretch->getSolverTipFrames(GROUP)[0] << std::endl; + std::cout << stretch->getSolverTipFrames("stretch_arm")[0] << std::endl; + + std::cin.get(); + + if (not stretch->setFromIK(query)) + { + RBX_ERROR("IK solution not found"); + return 1; + } + + // Set goal configuration. request.setGoalConfiguration(stretch->getScratchState()); // Visualize the goal state in RViz. diff --git a/robowflex_library/src/detail/stretch.cpp b/robowflex_library/src/detail/stretch.cpp index 32514d2d8..27a6528ae 100644 --- a/robowflex_library/src/detail/stretch.cpp +++ b/robowflex_library/src/detail/stretch.cpp @@ -33,8 +33,11 @@ StretchRobot::StretchRobot() : Robot("stretch") { } -bool StretchRobot::initialize() +bool StretchRobot::initialize(bool addVirtual) { + if (addVirtual) + setSRDFPostProcessAddPlanarJoint("base_joint"); + bool success = false; // First attempt the `robowflex_resources` package, then attempt the "actual" resource files. @@ -52,6 +55,8 @@ bool StretchRobot::initialize() loadKinematics("stretch_arm"); loadKinematics("stretch_gripper"); loadKinematics("stretch_head"); +// loadKinematics("mobile_base"); + loadKinematics("mobile_base_manipulator"); StretchRobot::openGripper(); @@ -89,6 +94,21 @@ void StretchRobot::closeGripper() Robot::setState(angles); } +void StretchRobot::setBasePose(double x, double y, double theta) +{ + if (hasJoint("base_joint/x") && hasJoint("base_joint/y") && hasJoint("base_joint/theta")) + { + std::cout << "H" << std::endl; + const std::map pose = { + {"base_joint/x", x}, {"base_joint/y", y}, {"base_joint/theta", theta}}; + + scratch_->setVariablePositions(pose); + scratch_->update(); + } + else + RBX_WARN("base_joint does not exist, cannot move base! You need to set addVirtual to true"); +} + OMPL::StretchOMPLPipelinePlanner::StretchOMPLPipelinePlanner(const RobotPtr &robot, const std::string &name) : OMPLPipelinePlanner(robot, name) { diff --git a/robowflex_library/src/robot.cpp b/robowflex_library/src/robot.cpp index e3a8a2401..a06b92219 100644 --- a/robowflex_library/src/robot.cpp +++ b/robowflex_library/src/robot.cpp @@ -350,6 +350,8 @@ bool Robot::loadKinematics(const std::string &group_name, bool load_subgroups) robot_model::SolverAllocatorFn allocator = kinematics_->getLoaderFunction(loader_->getSRDF()); const auto &groups = kinematics_->getKnownGroups(); + for (const auto &g : groups) + std::cout << g << std::endl; if (groups.empty()) { RBX_ERROR("No kinematics plugins defined. Fill and load kinematics.yaml!"); @@ -379,6 +381,7 @@ bool Robot::loadKinematics(const std::string &group_name, bool load_subgroups) for (const auto &name : load_names) { + std::cout << name << std::endl; // Check if kinematics have already been loaded for this group. if (imap_.find(name) != imap_.end()) continue; @@ -387,7 +390,7 @@ bool Robot::loadKinematics(const std::string &group_name, bool load_subgroups) std::find(groups.begin(), groups.end(), name) == groups.end()) { RBX_ERROR("No JMG or Kinematics defined for `%s`!", name); - return false; + continue; } robot_model::JointModelGroup *jmg = model_->getJointModelGroup(name); @@ -866,6 +869,8 @@ bool Robot::setFromIK(const IKQuery &query, robot_state::RobotState &state) cons // Sample new target poses from regions. query.sampleRegions(targets); + std::cout << "Number of tips " << tips.size() << ", number of targets " << targets.size() << std::endl; + #if ROBOWFLEX_AT_LEAST_MELODIC // Multi-tip IK: Will delegate automatically to RobotState::setFromIKSubgroups() if the kinematics // solver doesn't support multi-tip queries. diff --git a/robowflex_library/yaml/test_stretch.yml b/robowflex_library/yaml/test_stretch.yml index b6ce2059c..3ab9ba758 100644 --- a/robowflex_library/yaml/test_stretch.yml +++ b/robowflex_library/yaml/test_stretch.yml @@ -2,14 +2,10 @@ name: (noname) robot_state: joint_state: position: [0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0] - header: - frame_id: base_link name: [joint_left_wheel, joint_head_pan, joint_head_tilt, joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw, joint_gripper_finger_left, joint_gripper_finger_right, joint_right_wheel] robot_model_name: stretch fixed_frame_transforms: - - header: - frame_id: base_link - child_frame_id: base_link + - child_frame_id: world transform: translation: [0, 0, 0] rotation: [0, 0, 0, 1] @@ -20,7 +16,7 @@ world: - resource: "package://robowflex_resources/objects/cafe_table.dae" dimensions: [0.025, 0.025, 0.025] mesh_poses: - - position: [0, -1.0, -0.08] + - position: [0, -1.05, -0.08] orientation: [0, 0, 0, 1] - id: Cube1 primitives: From 90c4a77c27d60d919cd598bc559178aca0cc888c Mon Sep 17 00:00:00 2001 From: CarlosQuinteroPena Date: Mon, 11 Jul 2022 15:08:08 -0500 Subject: [PATCH 3/7] Add srdf post process function to add joint groups to robot class --- .../include/robowflex_library/robot.h | 8 ++++++ robowflex_library/src/robot.cpp | 26 +++++++++++++++---- 2 files changed, 29 insertions(+), 5 deletions(-) diff --git a/robowflex_library/include/robowflex_library/robot.h b/robowflex_library/include/robowflex_library/robot.h index 45e066c79..a11cf8293 100644 --- a/robowflex_library/include/robowflex_library/robot.h +++ b/robowflex_library/include/robowflex_library/robot.h @@ -190,6 +190,14 @@ namespace robowflex */ void setSRDFPostProcessAddFloatingJoint(const std::string &name); + /** \brief Adds a planning group with name \name and members \members, where each element is a pair + * that contains the type of group (first) and its name (second). + * \param[in] name Name for new joint. + * \param[in] members Members (type, name) that make up the planning group. + */ + void setSRDFPostProcessAddJointGroup(const std::string &name, + const std::vector> &members); + /** \brief Loads the kinematics plugin for a joint group and its subgroups. No kinematics are loaded * by default. * \param[in] group Joint group name to load. diff --git a/robowflex_library/src/robot.cpp b/robowflex_library/src/robot.cpp index a06b92219..4debbd052 100644 --- a/robowflex_library/src/robot.cpp +++ b/robowflex_library/src/robot.cpp @@ -350,8 +350,6 @@ bool Robot::loadKinematics(const std::string &group_name, bool load_subgroups) robot_model::SolverAllocatorFn allocator = kinematics_->getLoaderFunction(loader_->getSRDF()); const auto &groups = kinematics_->getKnownGroups(); - for (const auto &g : groups) - std::cout << g << std::endl; if (groups.empty()) { RBX_ERROR("No kinematics plugins defined. Fill and load kinematics.yaml!"); @@ -381,7 +379,6 @@ bool Robot::loadKinematics(const std::string &group_name, bool load_subgroups) for (const auto &name : load_names) { - std::cout << name << std::endl; // Check if kinematics have already been loaded for this group. if (imap_.find(name) != imap_.end()) continue; @@ -452,6 +449,27 @@ void Robot::setSRDFPostProcessAddFloatingJoint(const std::string &name) }); } +void Robot::setSRDFPostProcessAddJointGroup(const std::string &name, + const std::vector> &members) +{ + setSRDFPostProcessFunction([&name, &members](tinyxml2::XMLDocument &doc) -> bool { + tinyxml2::XMLElement *group_element = doc.NewElement("group"); + group_element->SetAttribute("name", name.c_str()); + + for (const auto &member : members) + { + tinyxml2::XMLElement *member_element = doc.NewElement(member.first.c_str()); + member_element->SetAttribute("name", member.second.c_str()); + + group_element->FirstChildElement(member.first.c_str())->InsertFirstChild(member_element); + } + + doc.FirstChildElement("robot")->InsertFirstChild(group_element); + + return true; + }); +} + const std::string &Robot::getModelName() const { return model_->getName(); @@ -869,8 +887,6 @@ bool Robot::setFromIK(const IKQuery &query, robot_state::RobotState &state) cons // Sample new target poses from regions. query.sampleRegions(targets); - std::cout << "Number of tips " << tips.size() << ", number of targets " << targets.size() << std::endl; - #if ROBOWFLEX_AT_LEAST_MELODIC // Multi-tip IK: Will delegate automatically to RobotState::setFromIKSubgroups() if the kinematics // solver doesn't support multi-tip queries. From f922eb1372bf2dd5475c5df2fbf669ec56cddee9 Mon Sep 17 00:00:00 2001 From: CarlosQuinteroPena Date: Tue, 12 Jul 2022 11:00:40 -0500 Subject: [PATCH 4/7] Add post process functions to add mobile base manipulator groups and kinematics solver programatically --- robowflex_library/CMakeLists.txt | 2 +- .../robowflex_library/detail/stretch.h | 18 ++++-- .../include/robowflex_library/robot.h | 28 +++++++-- ...on.cpp => stretch_mobile_manipulation.cpp} | 51 +++++----------- robowflex_library/src/detail/stretch.cpp | 46 +++++++------- robowflex_library/src/robot.cpp | 61 ++++++++++++++++++- 6 files changed, 136 insertions(+), 70 deletions(-) rename robowflex_library/scripts/{stretch_visualization.cpp => stretch_mobile_manipulation.cpp} (65%) diff --git a/robowflex_library/CMakeLists.txt b/robowflex_library/CMakeLists.txt index ba7e84ca9..8a58829ce 100644 --- a/robowflex_library/CMakeLists.txt +++ b/robowflex_library/CMakeLists.txt @@ -142,7 +142,7 @@ add_script(cob4_test) add_script(cob4_visualization) add_script(cob4_multi_target) add_script(stretch_test) -add_script(stretch_visualization) +add_script(stretch_mobile_manipulation) ## ## Tests ## diff --git a/robowflex_library/include/robowflex_library/detail/stretch.h b/robowflex_library/include/robowflex_library/detail/stretch.h index 8ad96cbd7..29ead1d7e 100644 --- a/robowflex_library/include/robowflex_library/detail/stretch.h +++ b/robowflex_library/include/robowflex_library/detail/stretch.h @@ -31,10 +31,22 @@ namespace robowflex */ StretchRobot(); - /** \brief Initialize the robot with stretch_arm, stretch_gripper and stretch_head kinematics. + /** \brief Initialize the robot. + * \param[in] addVirtual Whether a virtual joint for the base should be added to the srdf descrition + * or not. + * \param[in] addBaseManip Whether a base + manipulator group should be added to the srdf + * representation or not. If set, two groups will be added, one for the base and one for the mobile + * base manipulator. * \return True on success, false on failure. */ - bool initialize(bool addVirtual = true); + bool initialize(bool addVirtual = true, bool addBaseManip = false); + + /** \brief Sets the base pose of the Stretch robot (a virtual planar joint) + * \param[in] x The x position. + * \param[in] y The y position. + * \param[in] theta The angle. + */ + void setBasePose(double x, double y, double theta); /** \brief Points the Stretch's head to a point in the world frame. * \param[in] point The point to look at. @@ -48,8 +60,6 @@ namespace robowflex /** \brief Closes the Stretch's gripper. */ void closeGripper(); - - void setBasePose(double x, double y, double theta); private: static const std::string DEFAULT_URDF; ///< Default URDF diff --git a/robowflex_library/include/robowflex_library/robot.h b/robowflex_library/include/robowflex_library/robot.h index a11cf8293..459f22ea8 100644 --- a/robowflex_library/include/robowflex_library/robot.h +++ b/robowflex_library/include/robowflex_library/robot.h @@ -191,12 +191,32 @@ namespace robowflex void setSRDFPostProcessAddFloatingJoint(const std::string &name); /** \brief Adds a planning group with name \name and members \members, where each element is a pair - * that contains the type of group (first) and its name (second). - * \param[in] name Name for new joint. + * that contains the type (joint, link, group, etc) and its name. + * \param[in] name Name of new group. * \param[in] members Members (type, name) that make up the planning group. */ - void setSRDFPostProcessAddJointGroup(const std::string &name, - const std::vector> &members); + void setSRDFPostProcessAddGroup(const std::string &name, + const std::vector> &members); + + /** \brief Adds a mobile manipulator group composed of one group for the mobile base and another one + * for the manipulator. + * \param[in] base_group Name to be given to the mobile base group. + * \param[in] manip_group Name of the existing manipulator group. + * \param[in] base_manip_group Name to be given to + * the mobile base manipulator group. + */ + void setSRDFPostProcessAddMobileManipulatorGroup(const std::string &base_group, + const std::string &manip_group, + const std::string &base_manip_group); + + /** \brief Adds a mobile manipulator solver plugin to the kinematics configuration description. + * \param[in] base_manip_group Name of the base manipulator group. + * \param[in] search_resolution Search resolution parameter for the kinematics solver. + * \param[in] timeout Timeout parameter for the kinematics solver. + */ + void setKinematicsPostProcessAddBaseManipulatorPlugin(const std::string &base_manip_group, + double search_resolution = 0.005, + double timeout = 0.1); /** \brief Loads the kinematics plugin for a joint group and its subgroups. No kinematics are loaded * by default. diff --git a/robowflex_library/scripts/stretch_visualization.cpp b/robowflex_library/scripts/stretch_mobile_manipulation.cpp similarity index 65% rename from robowflex_library/scripts/stretch_visualization.cpp rename to robowflex_library/scripts/stretch_mobile_manipulation.cpp index d53380c56..b56db43e8 100644 --- a/robowflex_library/scripts/stretch_visualization.cpp +++ b/robowflex_library/scripts/stretch_mobile_manipulation.cpp @@ -12,11 +12,9 @@ using namespace robowflex; -/* \file stretch_visualization.cpp - * A simple script that demonstrates how to use RViz with Robowflex with the - * Stretch robot. See https://kavrakilab.github.io/robowflex/rviz.html for how to - * use RViz visualization. Here, the scene, start/goal states, and motion plan - * displayed in RViz. +/* \file stretch_mobile_manipulation.cpp + * A simple script that demonstrates how to use the Stretch robot to plan using the mobile base and the + * manipulator with Robowflex. The scene, start/goal states, and motion plan are displayed in RViz. */ static const std::string GROUP = "mobile_base_manipulator"; @@ -28,10 +26,10 @@ int main(int argc, char **argv) // Create the default Stretch robot. auto stretch = std::make_shared(); - stretch->initialize(true); + // Initialize the robot with the addBaseManip flag set. + stretch->initialize(false, true); - // Create an RViz visualization helper. Publishes all topics and parameter under `/robowflex` by - // default. + // Create an RViz visualization helper. Publishes all topics and parameter under `/robowflex` by default. IO::RVIZHelper rviz(stretch); IO::RobotBroadcaster bc(stretch); bc.start(); @@ -46,52 +44,31 @@ int main(int argc, char **argv) // Create the default planner for the Stretch. auto planner = std::make_shared(stretch, "default"); planner->initialize(); - -// stretch->setBasePose(1.0, 0.0, 0.0); - + // Visualize the scene (start state) in RViz. scene->getCurrentState() = *stretch->getScratchState(); rviz.updateScene(scene); - ROS_INFO("Visualizing start state"); + ROS_INFO("Visualizing start state. Press Enter to continue."); std::cin.get(); - + // Create a motion planning request. MotionRequestBuilder request(planner, GROUP); stretch->setGroupState(GROUP, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0}); // home - - std::cout << "Number of dofs of this group " << stretch->getScratchState()->getJointModelGroup(GROUP)->getVariableCount() << std::endl; - for (const auto &name : stretch->getScratchState()->getJointModelGroup(GROUP)->getActiveJointModelNames()) - std::cout << name << std::endl; - - const auto &solver = stretch->getScratchState()->getJointModelGroup(GROUP)->getSolverInstance(); - if (solver) - std::cout << "Pointer is not null!" << std::endl; - - auto frames = solver->getTipFrame(); - std::cout << frames << std::endl; - - std::cin.get(); - + // Set start configuration. request.setStartConfiguration(stretch->getScratchState()); // Create IK query. - auto query = - Robot::IKQuery(GROUP, "link_wrist_yaw", *stretch->getScratchState(), Eigen::Vector3d{-0.2, 0.23, -0.71}); + auto query = Robot::IKQuery(GROUP, "link_wrist_yaw", *stretch->getScratchState(), + Eigen::Vector3d{0.2, 0.23, -0.71}); query.scene = scene; - auto tips(query.tips); - std::cout << "Number of tips " << tips.size() << ", tip: " << tips[0] << std::endl; -// std::cout << stretch->getSolverTipFrames(GROUP)[0] << std::endl; - std::cout << stretch->getSolverTipFrames("stretch_arm")[0] << std::endl; - - std::cin.get(); - + if (not stretch->setFromIK(query)) { RBX_ERROR("IK solution not found"); return 1; } - + // Set goal configuration. request.setGoalConfiguration(stretch->getScratchState()); diff --git a/robowflex_library/src/detail/stretch.cpp b/robowflex_library/src/detail/stretch.cpp index 27a6528ae..cfc7b43ca 100644 --- a/robowflex_library/src/detail/stretch.cpp +++ b/robowflex_library/src/detail/stretch.cpp @@ -33,11 +33,17 @@ StretchRobot::StretchRobot() : Robot("stretch") { } -bool StretchRobot::initialize(bool addVirtual) +bool StretchRobot::initialize(bool addVirtual, bool addBaseManip) { - if (addVirtual) + if (addBaseManip) + { + const std::string &mob_base_manip = "mobile_base_manipulator"; + setSRDFPostProcessAddMobileManipulatorGroup("mobile_base", "stretch_arm", mob_base_manip); + setKinematicsPostProcessAddBaseManipulatorPlugin(mob_base_manip); + } + else if (addVirtual) setSRDFPostProcessAddPlanarJoint("base_joint"); - + bool success = false; // First attempt the `robowflex_resources` package, then attempt the "actual" resource files. @@ -54,15 +60,28 @@ bool StretchRobot::initialize(bool addVirtual) loadKinematics("stretch_arm"); loadKinematics("stretch_gripper"); - loadKinematics("stretch_head"); -// loadKinematics("mobile_base"); - loadKinematics("mobile_base_manipulator"); + if (addBaseManip) + loadKinematics("mobile_base_manipulator"); StretchRobot::openGripper(); return success; } +void StretchRobot::setBasePose(double x, double y, double theta) +{ + if (hasJoint("base_joint/x") && hasJoint("base_joint/y") && hasJoint("base_joint/theta")) + { + const std::map pose = { + {"base_joint/x", x}, {"base_joint/y", y}, {"base_joint/theta", theta}}; + + scratch_->setVariablePositions(pose); + scratch_->update(); + } + else + RBX_WARN("base_joint does not exist, cannot move base! You need to set addVirtual to true"); +} + void StretchRobot::pointHead(const Eigen::Vector3d &point) { const RobotPose point_pose = RobotPose(Eigen::Translation3d(point)); @@ -94,21 +113,6 @@ void StretchRobot::closeGripper() Robot::setState(angles); } -void StretchRobot::setBasePose(double x, double y, double theta) -{ - if (hasJoint("base_joint/x") && hasJoint("base_joint/y") && hasJoint("base_joint/theta")) - { - std::cout << "H" << std::endl; - const std::map pose = { - {"base_joint/x", x}, {"base_joint/y", y}, {"base_joint/theta", theta}}; - - scratch_->setVariablePositions(pose); - scratch_->update(); - } - else - RBX_WARN("base_joint does not exist, cannot move base! You need to set addVirtual to true"); -} - OMPL::StretchOMPLPipelinePlanner::StretchOMPLPipelinePlanner(const RobotPtr &robot, const std::string &name) : OMPLPipelinePlanner(robot, name) { diff --git a/robowflex_library/src/robot.cpp b/robowflex_library/src/robot.cpp index 4debbd052..b9d445361 100644 --- a/robowflex_library/src/robot.cpp +++ b/robowflex_library/src/robot.cpp @@ -449,8 +449,8 @@ void Robot::setSRDFPostProcessAddFloatingJoint(const std::string &name) }); } -void Robot::setSRDFPostProcessAddJointGroup(const std::string &name, - const std::vector> &members) +void Robot::setSRDFPostProcessAddGroup(const std::string &name, + const std::vector> &members) { setSRDFPostProcessFunction([&name, &members](tinyxml2::XMLDocument &doc) -> bool { tinyxml2::XMLElement *group_element = doc.NewElement("group"); @@ -461,7 +461,7 @@ void Robot::setSRDFPostProcessAddJointGroup(const std::string &name, tinyxml2::XMLElement *member_element = doc.NewElement(member.first.c_str()); member_element->SetAttribute("name", member.second.c_str()); - group_element->FirstChildElement(member.first.c_str())->InsertFirstChild(member_element); + group_element->InsertFirstChild(member_element); } doc.FirstChildElement("robot")->InsertFirstChild(group_element); @@ -470,6 +470,61 @@ void Robot::setSRDFPostProcessAddJointGroup(const std::string &name, }); } +void Robot::setSRDFPostProcessAddMobileManipulatorGroup(const std::string &base_group, + const std::string &manip_group, + const std::string &base_manip_group) +{ + setSRDFPostProcessFunction( + [&, base_group, manip_group, base_manip_group](tinyxml2::XMLDocument &doc) -> bool { + // Add mobile base joint. + const std::string &base_joint_name = "base_joint"; + tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); + virtual_joint->SetAttribute("name", base_joint_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); + + // Add mobile base group using the created joint. + tinyxml2::XMLElement *group_base = doc.NewElement("group"); + group_base->SetAttribute("name", base_group.c_str()); + tinyxml2::XMLElement *base_joint = doc.NewElement("joint"); + base_joint->SetAttribute("name", base_joint_name.c_str()); + group_base->InsertFirstChild(base_joint); + doc.FirstChildElement("robot")->InsertFirstChild(group_base); + + // Add mobile base manipulator group. + tinyxml2::XMLElement *base_manip = doc.NewElement("group"); + base_manip->SetAttribute("name", base_manip_group.c_str()); + tinyxml2::XMLElement *manip = doc.NewElement("group"); + manip->SetAttribute("name", manip_group.c_str()); + base_manip->InsertFirstChild(manip); + tinyxml2::XMLElement *base = doc.NewElement("group"); + base->SetAttribute("name", base_group.c_str()); + base_manip->InsertFirstChild(base); + doc.FirstChildElement("robot")->InsertFirstChild(base_manip); + + return true; + }); +} + +void Robot::setKinematicsPostProcessAddBaseManipulatorPlugin(const std::string &base_manip_group, + double search_resolution, double timeout) +{ + setKinematicsPostProcessFunction( + [&, base_manip_group, search_resolution, timeout](YAML::Node &node) -> bool { + YAML::Node ks_node; + ks_node["kinematics_solver"] = "base_manipulator_kinematics_plugin/" + "BaseManipulatorKinematicsPlugin"; + ks_node["kinematics_solver_search_resolution"] = search_resolution; + ks_node["kinematics_solver_timeout"] = timeout; + + node[base_manip_group.c_str()] = ks_node; + + return true; + }); +} + const std::string &Robot::getModelName() const { return model_->getName(); From 685c024e78d80f2f97542d2e6df027f16d8424ed Mon Sep 17 00:00:00 2001 From: CarlosQuinteroPena Date: Tue, 12 Jul 2022 11:38:41 -0500 Subject: [PATCH 5/7] Change lambda introducer for setSRDFPostProcessAddGroup --- robowflex_library/src/robot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robowflex_library/src/robot.cpp b/robowflex_library/src/robot.cpp index b9d445361..60b0054e9 100644 --- a/robowflex_library/src/robot.cpp +++ b/robowflex_library/src/robot.cpp @@ -452,7 +452,7 @@ void Robot::setSRDFPostProcessAddFloatingJoint(const std::string &name) void Robot::setSRDFPostProcessAddGroup(const std::string &name, const std::vector> &members) { - setSRDFPostProcessFunction([&name, &members](tinyxml2::XMLDocument &doc) -> bool { + setSRDFPostProcessFunction([&, name, members](tinyxml2::XMLDocument &doc) -> bool { tinyxml2::XMLElement *group_element = doc.NewElement("group"); group_element->SetAttribute("name", name.c_str()); From 212886fc13f29293cfbca54979aaaa215afc5489 Mon Sep 17 00:00:00 2001 From: CarlosQuinteroPena Date: Tue, 12 Jul 2022 15:36:10 -0500 Subject: [PATCH 6/7] Add example of how to set the turning the radius in stretch_mobile_manipulation --- robowflex_library/scripts/stretch_mobile_manipulation.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/robowflex_library/scripts/stretch_mobile_manipulation.cpp b/robowflex_library/scripts/stretch_mobile_manipulation.cpp index b56db43e8..31691fc29 100644 --- a/robowflex_library/scripts/stretch_mobile_manipulation.cpp +++ b/robowflex_library/scripts/stretch_mobile_manipulation.cpp @@ -28,6 +28,10 @@ int main(int argc, char **argv) auto stretch = std::make_shared(); // Initialize the robot with the addBaseManip flag set. stretch->initialize(false, true); + // Set the turning radius for the mobile base; + auto radius = 0.5 * (stretch->getLinkTF("link_left_wheel").translation() - stretch->getLinkTF("link_right_wheel").translation()).norm(); + auto base_model = static_cast(stretch->getModel()->getJointModel("base_joint")); + base_model->setTurningRadius(radius); // Create an RViz visualization helper. Publishes all topics and parameter under `/robowflex` by default. IO::RVIZHelper rviz(stretch); From 5be2a8a25adb5d3bbbfb1fbc590ddd94a056f351 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Fri, 29 Jul 2022 10:36:13 +0800 Subject: [PATCH 7/7] Fixed linting --- robowflex_library/scripts/stretch_mobile_manipulation.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/robowflex_library/scripts/stretch_mobile_manipulation.cpp b/robowflex_library/scripts/stretch_mobile_manipulation.cpp index 31691fc29..722767b6f 100644 --- a/robowflex_library/scripts/stretch_mobile_manipulation.cpp +++ b/robowflex_library/scripts/stretch_mobile_manipulation.cpp @@ -29,8 +29,11 @@ int main(int argc, char **argv) // Initialize the robot with the addBaseManip flag set. stretch->initialize(false, true); // Set the turning radius for the mobile base; - auto radius = 0.5 * (stretch->getLinkTF("link_left_wheel").translation() - stretch->getLinkTF("link_right_wheel").translation()).norm(); - auto base_model = static_cast(stretch->getModel()->getJointModel("base_joint")); + auto radius = 0.5 * (stretch->getLinkTF("link_left_wheel").translation() - + stretch->getLinkTF("link_right_wheel").translation()) + .norm(); + auto base_model = // + static_cast(stretch->getModel()->getJointModel("base_joint")); base_model->setTurningRadius(radius); // Create an RViz visualization helper. Publishes all topics and parameter under `/robowflex` by default.