Detecting Collision

In this tutorial, we will see how to use the planner to detect collisions without planning a path. There are two APIs that are wrappers around some fcl library functions to provide a more convenient interface. In particular, we have check_for_self_collision and check_for_env_collision. As the name suggests, the former checks for robot self-collision, while the latter checks for collision between the robot and its environment.

Setting Up the Planner

We will use the convenient function setup_planner provided by mplib.examples.demo_setup.DemoSetup to load the robot and create a planner. We will also make a function to print out the collisions detected.

    def print_collisions(self, collisions):
        """Helper function to abstract away the printing of collisions"""
        if len(collisions) == 0:
            print("No collision")
            return
        for collision in collisions:
            print(
                f"{collision.link_name1} of entity {collision.object_name1} collides"
                f" with {collision.link_name2} of entity {collision.object_name2}"
            )

We will also create a floor as one of the collision objects to demonstrate the check_for_env_collision API.

        floor = fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
        # create a collision object for the floor, with a 10cm offset in the z direction
        floor_fcl_collision_object = fcl.CollisionObject(floor, Pose(p=[0, 0, -0.1]))
        # update the planning world with the floor collision object
        self.planner.planning_world.add_object("floor", floor_fcl_collision_object)

Note that we call floor an object because it is not an articulated object. The function to add an object to the planning world is add_object. This can also be used to update the pose of the object or change it our entirely.

Collision Time!

We will now test several configurations to see how the planner detects collisions. First, we will set the robot to a self-collision-free qpos and check for self-collision. This should return no collision. Note that the full joint configuration is not provided here. Instead, on the movegroup related joints are set. The rest of the joints are set to the current joint angle.

        # if the joint qpos does not include the gripper joints,
        # it will be set to the current gripper joint angle
        self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
        self.print_collisions(
            self.planner.check_for_self_collision(self_collision_free_qpos)
        )

Next, we will put the robot into a self-collision qpos and check for self-collision. This should return a collision.

        self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
        self.print_collisions(
            self.planner.check_for_self_collision(self_collision_qpos)
        )

Then, we do the same thing with environment collision as we put the robot into a pose that collides with the floor. Additionally, we also try to plan a path to this qpos. This will cause the planner to timeout.

        # this qpos causes several joints to dip below the floor
        env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0]
        self.print_collisions(self.planner.check_for_env_collision(env_collision_qpos))

        print("\n----- env-collision causing planner to timeout -----")
        status, path = self.planner.planner.plan(
            start_state=self_collision_free_qpos, goal_states=[env_collision_qpos]
        )
        print(status, path)

Finally, we remove the floor and check for environment collision again. This should return no collision.

        self.planner.remove_normal_object("floor")
        self.print_collisions(self.planner.check_for_env_collision(env_collision_qpos))