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, start_qpos, mask=None, n_init_qpos=20, threshold=0.001)[source]

Inverse kinematics

Parameters:

  • goal_pose – [x,y,z,qw,qx,qy,qz] pose of the goal

  • start_qpos – initial configuration

  • mask – if the value at a given index is True, the joint is not used in the IK

  • n_init_qpos – number of random initial configurations

  • threshold – threshold for the distance between the goal pose and the result pose

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.