{"id":123,"date":"2019-03-01T20:40:35","date_gmt":"2019-03-01T20:40:35","guid":{"rendered":"http:\/\/erdalpekel.de\/?p=123"},"modified":"2021-02-13T09:24:24","modified_gmt":"2021-02-13T09:24:24","slug":"panda-in-gazebo-constraint-aware-planning","status":"publish","type":"post","link":"https:\/\/erdalpekel.de\/?p=123","title":{"rendered":"MoveIt! – Constraint-aware planning"},"content":{"rendered":"\n

In the first article of this series I outlined the integration of the Panda robot from FRANKA EMIKA into the Gazebo simulation environment. We did so by modifying the packages franka_ros and libfranka which FRANKA EMIKA provides such that the additional information required by Gazebo was added to the robot model and the controller configuration. For a detailed look you can read the article<\/a>.<\/p>\n\n\n\n

In this post we are going to utilize MoveIt! in order to navigate Panda in a cluttered environment in the Gazebo simulation. I will provide you with sample json files that contain information about the objects the robot should avoid. We will spawn the robot directly into this environment.<\/p>\n\n\n\n

In MoveIt! we will add objects around the robot. These should raise awareness when moving. This way, we have the ability to put constraints on the robots motion planning depending on its environment.<\/p>\n\n\n\n

Implementation<\/h2>\n\n\n\n

We will implement a node called robot_control_node where we <\/p>\n\n\n\n

  1. connect to the move group of the panda robots arm,<\/li>
  2. create the move group planning scene publisher,<\/li>
  3. read collision objects from the filesystem and<\/li>
  4. publish collision objects to the MoveIt!<\/em> planning scene.<\/li><\/ol>\n\n\n\n

    System dependencies & Preliminaries<\/h3>\n\n\n\n

    Our dependencies are boost filesystem<\/strong><\/a> for reading from the directory ~\/.panda_simulation and jsoncpp<\/strong><\/a> for parsing the collision objects files which we store as json files. Let’s have a look at a sample file:<\/p>\n\n\n\n

    {\n  \"dimensions\": {\n    \"x\": 2.0,\n    \"y\": 0.1,\n    \"z\": 2.5\n  },\n  \"position\": {\n    \"x\": -1.0,\n    \"y\": -0.6,\n    \"z\": 0.0\n  },\n  \"orientation\": {\n    \"w\": 1.0,\n    \"x\": 0.0,\n    \"y\": 0.0,\n    \"z\": 0.0\n  }\n}<\/code><\/pre>\n\n\n\n
    The orientation is defined as a quaternion. Here<\/a> is my resource of choice for visualization of quaternions. I will outline the reference frame of the position field in a moment. Dimensions defines the dimensions of the bounding box that. Yes, we are going to define the collision objects as bounding boxes in this tutorial.<\/p>\n\n\n\n
    Project setup<\/h3>\n\n\n\n
    We will use the panda_simulation<\/a> repository from my last blog post<\/a> as a basis for our implementation. We will need to add the previously defined system dependencies as well as the some MoveIt! packages that we use in the new node to our CMakeLists.txt<\/a> and also add the MoveIt! dependencies to our package.xml<\/a>. I will only mention the additional ROS dependencies introduced:<\/p>\n\n\n\n
    moveit_core\nmoveit_visual_tools\nmoveit_ros_planning\nmoveit_ros_planning_interface<\/code><\/pre>\n\n\n\n
    Our new node is called robot_control_node<\/em> because connecting to the move group of panda’s arm gives us the ability to control the robot (which we will hopefully do in our next blog post!).<\/p>\n\n\n\n
    ROS Node<\/h3>\n\n\n\n
    1. Connecting to the move group of Panda’s arm<\/h4>\n\n\n\n
    A ROS node that is supposed to connect to move group should spin asynchronously with AsyncSpinner<\/a><\/em> for better compatibility with the nature asynchronous MoveIt! API calls. A suitable example for this is that issuing plan or move calls will presumably take a long time and should not block the process that the ROS node is running in.<\/p>\n\n\n\n
    ros::init(argc, argv, \"robot_control_node\");\n\nros::NodeHandle node_handle;\nros::AsyncSpinner spinner(1);\nspinner.start();\n\nmoveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);<\/code><\/pre>\n\n\n\n
    2. Create the move group planning scene publisher<\/h4>\n\n\n\n
    This is crucial: Unfortunately it is extremely hard to find a mapping between MoveIt! core packages \/ modules and associated ROS topics. In our case the topic \/planning_scene<\/em> takes our desired changes (diff<\/em>) to the current planning scene of the planning scene. This is the interface to the MoveIt! Planning Scene ROS API. We will make our node sleep until there is actually a scene that subscribes to the diff that we want to publish. The planning_scene<\/em> object that we define at the end is our interface to the planning scene. With it we are going to publish the collision objects in the correct format.<\/p>\n\n\n\n
    ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>(\"planning_scene\", 1);\nros::WallDuration sleep_t(0.5);\nwhile (planning_scene_diff_publisher.getNumSubscribers() < 1)\n{\n  sleep_t.sleep();\n}\nmoveit_msgs::PlanningScene planning_scene;<\/code><\/pre>\n\n\n\n
    3. Read collision objects from  the filesystem<\/h4>\n\n\n\n
    We are now ready to read json files defining collision objects from a predefined directory in the users home folder (~\/.panda_simulation) and publish the objects to the planning scene. The important part is the helper function that extracts the obstacles when given a json object:<\/p>\n\n\n\n
    moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)\n{\n  moveit_msgs::CollisionObject collision_object;\n  collision_object.header.frame_id = \"world\";\n  collision_object.id = name;\n\n  const Json::Value dimensions = root[\"dimensions\"];\n  ROS_INFO_STREAM(\"Extracted dimensions: \" << dimensions);\n  \/\/ Define a box to add to the world.\n  shape_msgs::SolidPrimitive primitive;\n  primitive.type = primitive.BOX;\n  primitive.dimensions.resize(3);\n  primitive.dimensions[0] = dimensions[\"x\"].asDouble();\n  primitive.dimensions[1] = dimensions[\"y\"].asDouble();\n  primitive.dimensions[2] = dimensions[\"z\"].asDouble();\n\n  const Json::Value position = root[\"position\"];\n  ROS_INFO_STREAM(\"Extracted position: \" << position);\n\n  const Json::Value orientation = root[\"orientation\"];\n  ROS_INFO_STREAM(\"Extracted orientation: \" << orientation);\n  \/\/ Define a pose for the box (specified relative to frame_id)\n  geometry_msgs::Pose box_pose;\n  box_pose.orientation.w = orientation[\"w\"].asDouble();\n  box_pose.orientation.x = orientation[\"x\"].asDouble();\n  box_pose.orientation.y = orientation[\"y\"].asDouble();\n  box_pose.orientation.z = orientation[\"z\"].asDouble();\n  box_pose.position.x = position[\"x\"].asDouble() + primitive.dimensions[0] \/ 2.0;\n  box_pose.position.y = position[\"y\"].asDouble() + primitive.dimensions[1] \/ 2.0;\n  box_pose.position.z = position[\"z\"].asDouble() + primitive.dimensions[2] \/ 2.0;\n\n  collision_object.primitives.push_back(primitive);\n  collision_object.primitive_poses.push_back(box_pose);\n  collision_object.operation = collision_object.ADD;\n\n  return std::move(collision_object);\n}<\/code><\/pre>\n\n\n\n