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(
goal_pose: Pose,
start_qpos: ndarray,
mask: Sequence[bool] | ndarray | None = None,
*,
n_init_qpos: int = 20,
threshold: float = 1e-3,
return_closest: bool = False,
verbose: bool = False,
) → tuple[str, list[ndarray] | ndarray | None][source]

Compute inverse kinematics

Parameters:

  • goal_pose – goal pose

  • start_qpos – initial configuration, (ndof,) np.floating np.ndarray.

  • mask – qpos mask to disable IK sampling, (ndof,) bool np.ndarray.

  • n_init_qpos – number of random initial configurations to sample.

  • threshold – distance threshold for marking sampled IK as success. distance is position error norm + quaternion error norm.

  • return_closest – whether to return the qpos that is closest to start_qpos, considering equivalent joint values.

  • verbose – whether to print collision info if any collision exists.

Returns:

(status, q_goals)

status: IK status, “Success” if succeeded.

q_goals: list of sampled IK qpos, (ndof,) np.floating np.ndarray.

IK is successful if q_goals is not None. If return_closest, q_goals is np.ndarray if successful and None if not successful.

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.

  • mask: a list of 0/1 values with the same length as init_qpos. It specifies which joints are disabled (1). For example, if you want to solve the inverse kinematics of the first 2 joints, you can set mask=[0,0,1,1,1,1,1].

  • 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.

Note

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