Inverse kinematics and motion planning#

This tutorial builds a complete pick-and-place task with a Franka arm: solve inverse kinematics (IK) for a target end-effector pose, plan a collision-free path to that configuration, then close the gripper and lift a cube. It also covers the pose conventions IK expects, and why the approach runs under position control while the grasp runs under force control.

The complete script is examples/tutorials/IK_motion_planning_grasp.py.

A Franka arm positioned above a small cube on the ground plane, viewed in the Genesis World viewer.

Motion planning runs on the sampling-based planners built into Genesis World, so the example needs no extra dependency. See Path planning for the planners and their parameters.

Scene and robot setup#

Load a ground plane, a small cube to grasp, and the Franka arm, then build the scene:

cube = scene.add_entity(
    gs.morphs.Box(
        size=(0.04, 0.04, 0.04),
        pos=(0.65, 0.0, 0.02),  # meters, Z-up
    )
)
franka = scene.add_entity(
    gs.morphs.MJCF(file="xml/franka_emika_panda/panda.xml"),
)
scene.build()

The Franka has nine degrees of freedom (dof): seven arm joints and two gripper fingers. Splitting them into two index arrays lets you command the arm and the fingers independently:

motors_dof = np.arange(7)
fingers_dof = np.arange(7, 9)

Position control is a PD controller, so it needs per-dof stiffness (kp) and damping (kv) gains, plus a force range. The values below are tuned for the Franka; a different robot needs its own, and a well-authored URDF or MJCF may already provide them.

franka.set_dofs_kp(
    np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
)
franka.set_dofs_kv(
    np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
)
franka.set_dofs_force_range(
    np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
    np.array([87, 87, 87, 87, 12, 12, 12, 100, 100]),
)

Solving inverse kinematics#

IK answers the question “what joint angles put the end-effector at this pose?” In Genesis World it is a method on the robot entity: name the link that acts as the end-effector, give it a target pose, and it returns a full-body configuration (qpos).

end_effector = franka.get_link("hand")

qpos = franka.inverse_kinematics(
    link=end_effector,
    pos=np.array([0.65, 0.0, 0.25]),  # world-frame position, meters
    quat=np.array([0, 1, 0, 0]),  # w-x-y-z; 180 deg about X, gripper points down
)

The target pos and quat are in the world frame, using the right-handed, Z-up coordinate system and the scalar-first (w, x, y, z) quaternion convention. Here (0, 1, 0, 0) is a 180-degree rotation about the world X-axis, which orients the gripper to point straight down at the table.

The returned qpos covers every dof, including the fingers. Setting the finger entries opens the gripper before the approach:

qpos[-2:] = 0.04  # open gripper, meters per finger

Planning a path to the configuration#

IK gives a goal configuration but not how to get there. plan_path finds a collision-free trajectory from the current configuration to qpos_goal and returns a list of waypoints, one per simulation step:

path = franka.plan_path(
    qpos_goal=qpos,
    num_waypoints=200,  # 200 steps at dt=0.01 s -> 2 s of motion
)

for waypoint in path:
    franka.control_dofs_position(waypoint)
    scene.step()

# let the PD controller settle onto the final waypoint
for i in range(100):
    scene.step()

Executing the path steps the simulation once per waypoint. The extra 100 steps at the end matter: the PD controller leaves the arm trailing its commanded target by a small error, and stepping a little longer lets it converge onto the last waypoint before the next phase begins.

Tip

scene.draw_debug_path(path, franka) visualizes the planned trajectory in the viewer, and scene.clear_debug_object(...) removes it afterward. The example uses both to render the path while the arm follows it.

Grasping and lifting#

The rest of the task is a sequence of IK solves and control commands. To reach down to the cube, solve IK for a lower target and drive only the arm dofs with position control:

qpos = franka.inverse_kinematics(
    link=end_effector,
    pos=np.array([0.65, 0.0, 0.130]),
    quat=np.array([0, 1, 0, 0]),
)
franka.control_dofs_position(qpos[:-2], motors_dof)  # arm only; leave fingers as-is
for i in range(100):
    scene.step()

To grasp, switch the fingers from position control to force control. Position control would command a target opening, so it depends on the cube’s exact width and on the fingers reaching it precisely; force control applies a steady squeezing force instead, and the fingers close until the cube stops them:

franka.control_dofs_position(qpos[:-2], motors_dof)
franka.control_dofs_force(np.array([-0.5, -0.5]), fingers_dof)  # 0.5 N inward per finger
for i in range(100):
    scene.step()

Finally, solve IK for a raised target and hold the grasp while the arm lifts:

qpos = franka.inverse_kinematics(
    link=end_effector,
    pos=np.array([0.65, 0.0, 0.28]),
    quat=np.array([0, 1, 0, 0]),
)
franka.control_dofs_position(qpos[:-2], motors_dof)
for i in range(200):
    scene.step()

The fingers stay under force control from the grasp step, so the cube rises with the gripper.

See also#