Inverse Kinematics

Inverse kinematics determine the joint positions that provide the desired pose for the robot’s end-effectors. In mplib, you can solve the inverse kinematics of the move_group link with:

planner.IK(target_pose, init_qpos, n_init_qpos = 20, threshold = 1e-3)

Planner.IK() internally implements a numerical method and takes the following arguments:

• target_pose: a 7-dim list specifies the target pose of the move_group link. The first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part.

• init_qpos: a list describes the joint positions of all the active joints (e.g., given by SAPIEN). It will be used as the initial state for the numerical method.

• n_init_qpos=20: besides the provided initial state, the method also samples extra initial states to run the algorithm for at most n_init_qpos times. In this way, it can avoid local minimums and increase the success rate.

• threshold=1e-3: a threshold for determining whether the calculated pose is close enough to to the target pose.

It returns a tuple of two elements:

• status: a string indicates the status.

• result: a NumPy array describes the calculated joint positions.

If planner.IK() fails, please increase n_init_qpos or double-check whether the target pose is reachable.