Using a depth camera together with your Moveit setup has many advantages. The robot can avoid collisions in unknown environments and even react to changes in its surrounding. However, connecting a depth camera to your setup and using it correctly in your application can be a challenge. In this tutorial I will show you how to add a Kinect camera to your environment, visualize the data in Rviz and connect it to Moveit. Then I will show you how to do collision avoidance and how to properly use the depth data in a pick and place application. But first, let me give you a brief explanation of what a depth camera actually does.

What is a depth camera?

With a normal camera you get a specific amount of pixels, depending on the resolution, and some color information for each pixel. With a depth camera, you get information on how far that pixel is away from the camera. That way you know the distance of the objects to the lens of the camera – that’s the depth information.

There are different techniques for measuring the depth for each pixel. Some sensors project a certain light pattern on the object and compute the distance based on how this pattern is deformed. Stereo cameras on the other hand have two sensors with a known distance between each other and calculate the depth information by comparing the data coming from both sensors. Time of flight cameras emit light which is reflected back by the objects. The camera receives the reflected light and measures the time between emitting and receiving to compute the distance.

These different techniques have pros and cons in attributes like accuracy, robustness to noise and cost. Some are better suited for indoor applications and short distances while some are suitable for outdoor use and can look far.

In this tutorial we use the (simulated) Kinect camera from Microsoft which is an RGBD camera. That means it provides RGB color images as well as depth information.

How to add the Kinect depth camera to your environment

In order to make use of the kinect camera for our robot we first need to get the camera model and add it to our robot URDF file. Then we need to modify our Moveit configuration that we created in this previous tutorial to use the depth information.

First, we get the Kinect model from the common sensors repository (this repository contains a bunch of sensors commonly used in ROS). Go ahead and clone this repository into the src directory of your catkin workspace:

~/catkin_ws/src$ git clone https://github.com/dairal/common-sensors

Maybe you already have the necessary Moveit configuration package for the UR5 robot arm and the robotiq gripper installed from the previous tutorials. If not, please do so by following the instructions in the Quickstart section.

When this is done, we can add the Kinect URDF to our URDF file for the UR5 robot arm plus the gripper. Open the ur5_robotiq85_gripper.urdf.xacro file in the ur_description subfolder inside the universal_robot repository. Add the lines 81 – 84 at the end of the file before the closing </robot> tag. (You could also create a new URDF called ur5_robotiq85_gripper_kinect.urdf.xacro where you include the ur5_robotiq85_gripper.urdf.xacro file and add the lines for the Kinect sensor. Just remember to modify the moveit configuration package where it refers to the URDF path)

    </gripper>
  </gazebo>

    <!--Kinect-->
	<!-- position of kinect has to be changed in common_sensors/urdf/sensors/kinect_properties.urdf.xacro-->	
	<xacro:include filename="$(find common_sensors)/urdf/sensors/kinect.urdf.xacro" />
	<xacro:sensor_kinect parent="world"/>
  
</robot>

As the comment above the include line says, we can change the position and orientation of the sensor. This is important such that the camera captures the view of the scene that we need for our application. Go ahead and open kinect_properties.urdf.xacro file inside common_sensors/urdf/sensors. Define the following properties:

	<property name="cam_px" value="0.0" />
	<property name="cam_py" value="0.0" />
	<property name="cam_pz" value="1.0" />
	<property name="cam_or" value="0" />
	<property name="cam_op" value="1.25" />
	<property name="cam_oy" value="1.54" />

Now the camera points towards our scene. The last thing which needs to be configured are the sensor parameters inside the Moveit configuration package (This is already done in the package). The sensors_3d.yaml file in the config subfolder of the ur5_gripper_moveit_config package contains the following lines:

# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
  - filtered_cloud_topic: filtered_cloud
    max_range: 5.0
    max_update_rate: 1.0
    padding_offset: 0.1
    padding_scale: 1.0
    point_cloud_topic: /camera1/depth/points
    point_subsample: 1
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater

Usually it is not necessary to know what exactly these parameters do so I will only give you a brief explanation:

• filtered_cloud_topic: The depth data is published on this topic. The robot itself is already filtered out.
• max_range: Maximum distance that is measured
• max_update_rate: Maximum rate at which the octomap is updated
• padding_offset/ padding_scale: Padding is used to compensate for noise and artifacts in the depth data. This is especially important for the self-filtering of the robot. These parameters allow to adjust padding scale and offset.
• point_cloud_topic: Topic on which the raw point cloud data is published
• point_subsample: One out of point_subsample points is used (in our case we use every sample)
• sensor_plugin: The Moveit-plugin we are using for the depth data

Now we are done with the configuration and ready to visualize our camera data!

Visualizing depth camera data in Rviz

Let’s open our environment in Rviz and Gazebo by running:

$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch

When you look at Rviz now you might not see any point cloud data yet. If that is the case, click Add on the bottom of the Displays section on the left. Choose PointCloud2 out of the list as shown in the picture below and press OK. Now there is an item called PointCloud2 added which you can expand by clicking the arrow on its left. In the row topic choose /camera1/depth/points, then you should see the data.

Now if your environment is empty you might still not see anything apart from some artifacts but don’t worry. The picture below shows data from the already loaded pick and place scenario which we will add as a next step. However, as you can see from the picture below we don’t get an image of the whole environment, just what can be detected from the angle and field of view of the depth camera. You might also recognize that Moveit automatically filters out our robot from the point cloud data.

The grid representation you see in Rviz is called octomap. It is a representation of the elements in the environment that are occupied by collision objects. Moveit can use this octomap for motion planning right away.

Using the depth camera in a pick and place scenario

Now we want to use the depth camera in our pick and place scenario that we already implemented in the last tutorial. If you have already done it then you can skip the next three steps. If not, clone the repository that contains the pick and place implementation and build the catkin workspace:

~/catkin_ws/src$ git clone https://github.com/dairal/ur5_simple_pick_and_place
~/catkin_ws/src$ cd ..
~/catkin_ws/$ catkin_make

Now we need to specify the world we want to load in Gazebo and we need to apply an offset to the spawn position of the robot.

In order to load the world, we need to modify the gazebo.launch file inside the Moveit Configuration package. Go ahead and change line 9 to load the pick and place world as a default:

    <arg name="world_name" default="$(find ur5_simple_pick_and_place)/world/simple_pick_and_place"/>

In this world the robot is placed on a table so we need to modify the position where the robot arm is spawned. Therefore, we need to change the offset for the z-axis in line 18 to the height of the table (roughly 1.21 m):

    <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 1.21"

If Rviz and Gazebo are still running from before without the pick and place world, close both and restart them:

$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch

Now you should see the pick and place environment loaded in Gazebo with the robot sitting on the table. The goal is to grasp the blue box on the right side of the green obstacle and place it on the plate on the left side. If you run the unmodified implementation from the last tutorial with the following command you will run into errors:

$  rosrun ur5_simple_pick_and_place pick_and_place_collision

It will show a message saying that it can’t find a motion plan for certain goals. That is because Moveit now checks for collisions between the robot and the octomap. Because Moveit doesn’t know about the blue box that we want to grasp, it thinks that moving to that position will cause a collision.

In order for the pick and place task to work we need to modify two things:

1. We need to add the blue box to the planning scene to be able to deactivate collision checking between the gripper and the box.
2. After we grasp the object we need to attach the object to the gripper such that Moveit takes it into account for collision checking

The changes are implemented in the pick_and_place_collision_depth_camera.cpp file inside the ur5_simple_pick_and_place repository so you don’t need to do the following steps manually. You can have a look at the complete file here.

1. Add the blue box to the planning scene and deactivate collision checking

In the previous tutorial we had to add the green obstacle to the planning scene to avoid collision with it. We don’t need to do that anymore because our depth camera automatically detects the collision objects in our environment. Instead, we define the object we want to grasp and add it to the planning scene.

collision_object.id = "blue_box";

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.06;
    primitive.dimensions[1] = 0.06;
    primitive.dimensions[2] = 0.06;

    geometry_msgs::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.3;
    box_pose.position.y = 0.5;
    box_pose.position.z = 1.045 - 1.21;

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

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);

    planning_scene_interface.applyCollisionObjects(collision_objects);

We define a primitive shape and define the dimensions of our blue box in lines 42 to 47. Then we set the box pose in lines 49 to 53. Finally, we add shape and pose to our object collision_object, add the object to our collision_objects vector (line 60) and add the vector to our planning scene (line 62).

When running the code later, we will see that after adding the blue box (which appears green in Rviz) to the scene, Moveit clears the octomap around it.

Now that Moveit knows about our blue box we can go ahead and disable collision checking between the box and the gripper. The two links that will actually grasp the box are the left and right finger tip link, respectively.

Via the LockedPlanningSceneRW class we gain access to the AllowedCollisionMatrix of the underlying planning scene in lines 69/70. Using the AllowedCollisionMatrix we can tell Moveit to disable collision checking between certain links/objects. In lines 71 and 72 we do exactly that for the blue box and the left and right tip link. Then we apply the changes to the original planning scene.

    planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
    collision_detection::AllowedCollisionMatrix& acm = ls->getAllowedCollisionMatrixNonConst();
    acm.setEntry("blue_box", "robotiq_85_left_finger_tip_link", true);
    acm.setEntry("blue_box", "robotiq_85_right_finger_tip_link", true);
    std::cout << "\nAllowedCollisionMatrix:\n";
    acm.print(std::cout);
    moveit_msgs::PlanningScene diff_scene;
    ls->getPlanningSceneDiffMsg(diff_scene);

    planning_scene_interface.applyPlanningScene(diff_scene); 

2. Attach the object to the gripper

As soon as the robot grasped the blue box we want to tell Moveit about the attached object. That way Moveit takes the grasped object into account for computing the motions and doesn’t bump into obstacles with the blue box.

Moveit provides the AttachedCollisionObject message to attach the object. We add the id of our object (“blue_box” in our case) to the message in line 149. Moveit will add a fixed joint between the link_name we define in line 150 and our object. Then we need to add the additional links that will be in contact with the grasped object (line 151). At the end we apply the attached collision object to the planning scene.

    moveit_msgs::AttachedCollisionObject aco;
    aco.object.id = collision_object.id;
    aco.link_name = "robotiq_85_right_finger_tip_link";
    aco.touch_links.push_back("robotiq_85_left_finger_tip_link");
    aco.object.operation = moveit_msgs::CollisionObject::ADD;
    planning_scene_interface.applyAttachedCollisionObject(aco);

During execution we will see the colour of the box changing in the Rviz visualization after it was attached to the gripper.

That’s it! After building, we can now run the task by executing:

$  rosrun ur5_simple_pick_and_place pick_and_place_collision_depth_camera

When you look at the visualization in Rviz you can see what the robot sees and plans. In Gazebo you can see what actually happens in the (simulated) world.

If you execute it multiple times you might see that the robot sometimes does awkward motions (like in the video) or fails to find a motion plan. That is a common problem when applying motion planning algorithms and dealing with those problems requires good knowledge of the algorithm and the planning scene.

Conclusion

We saw that adding a depth camera for motion planning makes the task more flexible, robust and safer because it can react to changes in the environment. On the other hand it also adds more complexity to the implementation. It is also important to know that the motion planning algorithm we are using only takes objects into account that exist in the scene while it computes the movement. It does not adapt the computed trajectory if new objects occur during motion. However, this is possible to using a different algorithms.

There are a lot more things that can be done when we add a camera to our setup. The next tutorial shows how we can use the OpenCV library to use Computer Vision in our robot applications. Also check out the other tutorials in the tutorials overview. I hope you found this article helpful and I am always happy to receive your feedback in the comments section!