MoveIt! – Constraint-aware planning

By Erdal PekelC++, Gazebo, MoveIt!, Robot Operating System, Robotics, ROS, Simulation, Software Development

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.

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.

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.

Implementation

We will implement a node called robot_control_node where we

  1. connect to the move group of the panda robots arm,
  2. create the move group planning scene publisher,
  3. read collision objects from the filesystem and
  4. publish collision objects to the MoveIt! planning scene.

System dependencies & Preliminaries

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

{
  "dimensions": {
    "x": 2.0,
    "y": 0.1,
    "z": 2.5
  },
  "position": {
    "x": -1.0,
    "y": -0.6,
    "z": 0.0
  },
  "orientation": {
    "w": 1.0,
    "x": 0.0,
    "y": 0.0,
    "z": 0.0
  }
}

The orientation is defined as a quaternion. Here 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.

Project setup

We will use the panda_simulation repository from my last blog post 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 and also add the MoveIt! dependencies to our package.xml. I will only mention the additional ROS dependencies introduced:

moveit_core
moveit_visual_tools
moveit_ros_planning
moveit_ros_planning_interface

Our new node is called robot_control_node 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!).

ROS Node

1. Connecting to the move group of Panda’s arm

A ROS node that is supposed to connect to move group should spin asynchronously with AsyncSpinner 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.

ros::init(argc, argv, "robot_control_node");

ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();

moveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);

2. Create the move group planning scene publisher

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 takes our desired changes (diff) 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 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.

ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
ros::WallDuration sleep_t(0.5);
while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
  sleep_t.sleep();
}
moveit_msgs::PlanningScene planning_scene;

3. Read collision objects from the filesystem

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:

moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
{
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = "world";
  collision_object.id = name;

  const Json::Value dimensions = root["dimensions"];
  ROS_INFO_STREAM("Extracted dimensions: " << dimensions);
  // Define a box to add to the world.
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = dimensions["x"].asDouble();
  primitive.dimensions[1] = dimensions["y"].asDouble();
  primitive.dimensions[2] = dimensions["z"].asDouble();

  const Json::Value position = root["position"];
  ROS_INFO_STREAM("Extracted position: " << position);

  const Json::Value orientation = root["orientation"];
  ROS_INFO_STREAM("Extracted orientation: " << orientation);
  // Define a pose for the box (specified relative to frame_id)
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = orientation["w"].asDouble();
  box_pose.orientation.x = orientation["x"].asDouble();
  box_pose.orientation.y = orientation["y"].asDouble();
  box_pose.orientation.z = orientation["z"].asDouble();
  box_pose.position.x = position["x"].asDouble() + primitive.dimensions[0] / 2.0;
  box_pose.position.y = position["y"].asDouble() + primitive.dimensions[1] / 2.0;
  box_pose.position.z = position["z"].asDouble() + primitive.dimensions[2] / 2.0;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  return std::move(collision_object);
}
  • With collision_object.header.frame_id = “world” we define the reference coordinate system of the object
  • The position of a bounding box is defined as it’s center in the planning scene! That’s why we add half of its dimension to its position

4. Publish collision objects to the planning scene

We will finally publish the collision objects to the planning scene. This is done by adding collision objects to the collision_objects vector of the scene objects world member:

// Publish the collision objects to the scene
for (const auto &collision_object : collision_objects)
{
  collision_object.header.frame_id = move_group_arm.getPlanningFrame();
  planning_scene.world.collision_objects.push_back(collision_object);
}

ROS_INFO_STREAM("# collision objects " << planning_scene.world.collision_objects.size());
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);

It is also important to note that we are setting the is_diff member to true. This way we do not alter a state that another node might have set before.

Result

The custom ROS package as well as the modified franka_ros and panda_moveit_config repositories that are needed for the simulation are available in my GitHub profile.

The next part of this series will be about implementing a ROS node that adds these collision objects to the GAZEBO simulation environment so that they can be part of the dynamics of the scene.