Planning With Fixed Joints

The planner also has the ability to temporarily fix certain joints during planning. The above shows the robot arm move by itself to pick up a red cube before staying in place and letting the base carry the fixed arm. For this tutorial, we will need a different URDF than the one we have used in the previous tutorials. In particular, this URDF has a set of x and y linear tracks that allows the arm to move horizontally. To load this URDF, we do the following:

        self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf")
        link_names = [
            "track_x",
            "track_y",
            "panda_link0",
            "panda_link1",
            "panda_link2",
            "panda_link3",
            "panda_link4",
            "panda_link5",
            "panda_link6",
            "panda_link7",
            "panda_hand",
            "panda_leftfinger",
            "panda_rightfinger",
        ]
        joint_names = [
            "move_x",
            "move_y",
            "panda_joint1",
            "panda_joint2",
            "panda_joint3",
            "panda_joint4",
            "panda_joint5",
            "panda_joint6",
            "panda_joint7",
            "panda_finger_joint1",
            "panda_finger_joint2",
        ]
        self.setup_planner(
            urdf_path="./data/panda/panda_on_rail.urdf",
            srdf_path="./data/panda/panda_on_rail.srdf",
            link_names=link_names,
            joint_names=joint_names,
            joint_vel_limits=np.ones(9),
            joint_acc_limits=np.ones(9),
        )

The optional link_names and joint_names parameters used to order the joints and links in a certain way are in this case used to show what the joints of the models are. Then, we set up the planning scene as usual and first move the arm on the track before moving the arm itself to grab the object.

        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]

        self.add_point_cloud()
        # also add the target as a collision object so we don't hit it
        fcl_red_cube = fcl.Box([0.04, 0.04, 0.14])
        collision_object = fcl.CollisionObject(fcl_red_cube, Pose(p=[0.7, 0, 0.07]))
        self.planner.planning_world.add_object("target", collision_object)

        # go above the target
        pickup_pose[2] += 0.2
        self.move_in_two_stage(pickup_pose)

Notice we have abstracted away how to decouple this motion into two stages. Here is the function definition:

    def move_in_two_stage(self, pose, has_attach=False):
        """
        first, we do a full IK but only generate motions for the base
        then, do another partial IK for the arm and generate motions for the arm
        """
        # do a full ik to the pose
        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos())
        if status != "Success":
            print("IK failed")
            sys.exit(1)
        # now fix arm joints and plan a path to the goal
        result = self.planner.plan_qpos(
            goal_qposes,
            self.robot.get_qpos(),
            time_step=1 / 250,
            fixed_joint_indices=range(2, 9),
        )
        # execute the planned path
        self.follow_path(result)
        result = self.plan_without_base(pose, has_attach)
        # execute the planned path
        self.follow_path(result)

The highlighted line is how we ignore the arm joints during planning. We ignore joints 2-9, keeping only joint 0 and 1 active. We then do the same thing except the joints fixed are 0 and 1, and the active joints are 2-9.