Collision Avoidance

In Plan a Path, we talked about how to plan paths for the robot. However, in that tutorial, we didn’t take the environment model into account. The robot will avoid self-collisions (i.e., collisions between the robot links), but may collide with the environment.

This tutorial will introduce two ways two ways to avoid collisions: add environment point clouds and attach a box. As shown in the right figure, the robot needs to move the red box to the place of the green box while avoiding collision with the blue box. The full script can be downloaded here demo.py.

The above figures show the benefit of collision avoidance:

Left: w/o point cloud, w/o attach. The robot arm hits the blue box.
Middle: w/ point cloud, w/o attach. The red box hits the blue box.
Right: w/ point cloud, w/ attach. There is no collision.

Add environment point clouds

One way to model the environment and avoid collision is through point clouds. The point cloud may come from the sensor observations or be sampled from the mesh surfaces. For example, we can add a point cloud for the blue box with planner.update_point_cloud():

    def add_point_cloud(self):
        import trimesh
        box = trimesh.creation.box([0.1, 0.4, 0.2])
        points, _ = trimesh.sample.sample_surface(box, 1000)
        points += [0.55, 0, 0.1]
        self.planner.update_point_cloud(points)
        return 

planner.update_point_cloud() takes two arguments. The first one is a NumPy array of shape $(n \times 3)$ , which describes the coordinates of the points. The coordinates should be represented in the frame of the robot arm’s root link. The second (optional) argument is resolution = 1e-3, which describes the resolution of each point.

After adding the point cloud, we can avoid collisions between the robot and the point cloud by setting use_point_cloud to be True. Both planner.plan() and planner.plan_screw() support this flag:

    def move_to_pose(self, pose):
        result = self.planner.plan_screw(pose, self.robot.get_qpos(), time_step=1/250,
                                        use_point_cloud=self.use_point_cloud, use_attach=self.use_attach)
        if result['status'] != "Success":
            result = self.planner.plan(pose, self.robot.get_qpos(), time_step=1/250,
                                        use_point_cloud=self.use_point_cloud, use_attach=self.use_attach)
            if result['status'] != "Success":
                print(result['status'])
                return -1 
        self.follow_path(result)
        return 0

You don’t need to provide the point cloud for each planner.plan() or planner.plan_screw() call. You can use planner.update_point_cloud() to update the point cloud once it’s changed.

Note

Please remember to remove the points of the robot arm if the points come from the sensor observation. Otherwise, there will always be collisions, and the planner may fail to plan a valid path.

Attach a box

As shown in the above figure (middle one), after adding the point cloud of the blue box, the robot will not collide with it. However, the red box moves with the robot, and it may still collide with the blue box. To address this issue, we can attach a box to the robot, so that we can avoid the collision between the attached box and the environment point cloud:

            self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])

planner.update_attached_box() takes three arguments:

size: a list with three elements indicates the size of the attached box.
pose: a list with seven elements indicates the relative pose from the box to the attached link. The first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part.
link_id = -1: optional, an integer indicates the id of the link that the box is attached to. The link id is determined by the user_link_names (during Configuration), and starts from 0. The default value -1 indicates the move_group link.

After adding the attached box, we can avoid collisions between the attached box and the point cloud by setting both use_point_cloud and use_attach to be True. Both planner.plan() and planner.plan_screw() support the flags.

You can use planner.update_attached_box() to update the box once it’s changed.

As shown in the above figure (the right one), after adding the point cloud of the blue box and attaching the red box to the move_group link, there is no collision.