Stretch robot and mobile manipulation planning

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
 ##

robowflex_library/include/robowflex_library/detail/stretch.h

@@ -0,0 +1,114 @@
+/* Author: Carlos Quintero-Peña */
+
+#ifndef ROBOWFLEX_STRETCH_
+#define ROBOWFLEX_STRETCH_
+
+#include <robowflex_library/planning.h>
+#include <robowflex_library/robot.h>
+
+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<std::string> &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

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<std::pair<std::string, std::string>> &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.

robowflex_library/scripts/stretch_mobile_manipulation.cpp

@@ -0,0 +1,108 @@
+/* Author: Carlos Quintero-Peña */
+
+#include <robowflex_library/builder.h>
+#include <robowflex_library/detail/stretch.h>
+#include <robowflex_library/geometry.h>
+#include <robowflex_library/io/broadcaster.h>
+#include <robowflex_library/io/visualization.h>
+#include <robowflex_library/log.h>
+#include <robowflex_library/scene.h>
+#include <robowflex_library/trajectory.h>
+#include <robowflex_library/util.h>
+
+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<StretchRobot>();
+    // 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<moveit::core::PlanarJointModel *>(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<Scene>(stretch);
+    scene->fromYAMLFile("package://robowflex_library/yaml/test_stretch.yml");
+
+    // Create the default planner for the Stretch.
+    auto planner = std::make_shared<OMPL::StretchOMPLPipelinePlanner>(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<Trajectory>(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;
+}

robowflex_library/scripts/stretch_test.cpp

@@ -0,0 +1,67 @@
+/* Author: Carlos Quintero-Peña */
+
+#include <robowflex_library/builder.h>
+#include <robowflex_library/detail/stretch.h>
+#include <robowflex_library/scene.h>
+#include <robowflex_library/trajectory.h>
+#include <robowflex_library/util.h>
+#include <robowflex_library/random.h>
+#include <robowflex_library/io/visualization.h>
+#include <robowflex_library/log.h>
+
+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<StretchRobot>();
+    stretch->initialize();
+
+    // Open Gripper
+    stretch->openGripper();
+
+    // Create an empty scene.
+    auto scene = std::make_shared<Scene>(stretch);
+    scene->toYAMLFile("ex_stretch.yml");
+
+    // Create the default planner for the Stretch.
+    auto planner = std::make_shared<OMPL::StretchOMPLPipelinePlanner>(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<Trajectory>(res.trajectory_);
+
+    // Output path to a file for visualization.
+    trajectory->toYAMLFile("stretch_path.yml");
+
+    return 0;
+}