diff --git a/robowflex_library/CMakeLists.txt b/robowflex_library/CMakeLists.txt index 4643fdbbf..ec67d47db 100644 --- a/robowflex_library/CMakeLists.txt +++ b/robowflex_library/CMakeLists.txt @@ -89,6 +89,7 @@ list(APPEND SOURCES src/detail/ur5.cpp src/detail/fetch.cpp src/detail/cob4.cpp + src/detail/stretch.cpp ) list(APPEND INCLUDES @@ -143,7 +144,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_mobile_manipulation) ## ## 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..29ead1d7e --- /dev/null +++ b/robowflex_library/include/robowflex_library/detail/stretch.h @@ -0,0 +1,114 @@ +/* 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. + * \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 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. + */ + 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/include/robowflex_library/robot.h b/robowflex_library/include/robowflex_library/robot.h index 9153d0686..30e968241 100644 --- a/robowflex_library/include/robowflex_library/robot.h +++ b/robowflex_library/include/robowflex_library/robot.h @@ -190,6 +190,34 @@ 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 (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 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. * \param[in] group Joint group name to load. diff --git a/robowflex_library/scripts/stretch_mobile_manipulation.cpp b/robowflex_library/scripts/stretch_mobile_manipulation.cpp new file mode 100644 index 000000000..722767b6f --- /dev/null +++ b/robowflex_library/scripts/stretch_mobile_manipulation.cpp @@ -0,0 +1,108 @@ +/* Author: Carlos Quintero-Peña */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace robowflex; + +/* \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"; + +int main(int argc, char **argv) +{ + // Startup ROS + ROS ros(argc, argv); + + // Create the default Stretch robot. + 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); + 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(); + + // Visualize the scene (start state) in RViz. + scene->getCurrentState() = *stretch->getScratchState(); + rviz.updateScene(scene); + 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 + + // 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}); + query.scene = scene; + + 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. + 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/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/src/detail/stretch.cpp b/robowflex_library/src/detail/stretch.cpp new file mode 100644 index 000000000..cfc7b43ca --- /dev/null +++ b/robowflex_library/src/detail/stretch.cpp @@ -0,0 +1,127 @@ +/* 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 addVirtual, bool addBaseManip) +{ + 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. + 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"); + 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)); + 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/src/robot.cpp b/robowflex_library/src/robot.cpp index 931a6e1eb..824f4f28a 100644 --- a/robowflex_library/src/robot.cpp +++ b/robowflex_library/src/robot.cpp @@ -387,7 +387,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); @@ -449,6 +449,82 @@ 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 { + 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->InsertFirstChild(member_element); + } + + doc.FirstChildElement("robot")->InsertFirstChild(group_element); + + return true; + }); +} + +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(); diff --git a/robowflex_library/yaml/test_stretch.yml b/robowflex_library/yaml/test_stretch.yml new file mode 100644 index 000000000..3ab9ba758 --- /dev/null +++ b/robowflex_library/yaml/test_stretch.yml @@ -0,0 +1,41 @@ +name: (noname) +robot_state: + joint_state: + position: [0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0] + 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: + - child_frame_id: world + 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.05, -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]