movement_primitives.kinematics.Chain

class movement_primitives.kinematics.Chain(tm, joint_names, base_frame, ee_frame, verbose=0)

Bases: object

Kinematic chain.

You should not call this constructor manually. Use Kinematics to create a kinematic chain.

Parameters:

tmFastUrdfTransformManager

Transformation manager

joint_nameslist

Names of joints that should be used

base_framestr

Name of the base link

ee_framestr

Name of the end-effector link

verboseint, optional (default: 0)

Verbosity level

Methods

ee_pose_error(joint_angles, desired_pose[, ...])	Compute pose error.
forward(joint_angles)	Forward kinematics.
forward_trajectory(Q)	Compute forward kinematics for a trajectory.
inverse(desired_pose, initial_joint_angles)	Inverse kinematics.
inverse_trajectory(H[, ...])	Compute inverse kinematics for a trajectory.
inverse_with_random_restarts(desired_pose[, ...])	Compute inverse kinematics with multiple random restarts.
local_inverse_with_random_restarts(...[, ...])	Compute inverse kinematics with multiple random restarts.

ee_pose_error(joint_angles, desired_pose, orientation_weight=1.0, position_weight=1.0)

Compute pose error.

Parameters:

joint_anglesarray-like, shape (n_joints,)

Actual joint angles for which we compute forward kinematics.

desired_posearray-like, shape (4, 4)

Desired pose.

orientation_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the orientation error.

position_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the position error.

Returns:

pose_errorfloat

Weighted error between actual pose and desired pose.

forward(joint_angles)

Forward kinematics.

Parameters:

joint_anglesarray, shape (n_joints,)

Joint angles

Returns:

ee2basearray, shape (4, 4)

Transformation from end-effector to base frame

forward_trajectory(Q)

Compute forward kinematics for a trajectory.

Parameters:

Qarray-like, shape (n_steps, n_joints)

Joint angles.

Returns:

Harray, shape (n_steps, 4, 4)

End-effector poses.

inverse(desired_pose, initial_joint_angles, return_error=False, bounds=None, solver='SLSQP', orientation_weight=1.0, position_weight=1.0)

Inverse kinematics.

Parameters:

desired_posearray, shape (4, 4)

Desired transformation from end-effector to base frame

initial_joint_anglesarray, shape (n_joints,)

Initial guess for joint angles

return_errorbool, optional (default: False)

Return error in addition to joint angles

boundsarray, shape (n_joints, 2), optional (default: joint limits)

Bounds for joint angle optimization

solverstr, optional (default: ‘SLSQP’)

Optimizer to solve inverse kinematics problem. Possible options: ‘SLSQP’, ‘L-BFGS-B’, and ‘COBYLA’.

orientation_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the orientation error.

position_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the position error.

Returns:

joint_anglesarray, shape (n_joints,)

Solution

errorfloat, optional

Pose error

inverse_trajectory(H, initial_joint_angles=None, interval=0.3141592653589793, random_restarts=True, random_state=<module 'numpy.random' from '/home/dfki.uni-bremen.de/afabisch/anaconda3/lib/python3.9/site-packages/numpy/random/__init__.py'>, solver='SLSQP', orientation_weight=1.0, position_weight=1.0)

Compute inverse kinematics for a trajectory.

Parameters:

Harray-like, shape (n_steps, 4, 4)

Desired end-effector poses.

initial_joint_anglesarray-like, shape (n_joints,), optional (default: None)

Initial guess for joint angles.

intervalfloat

We will search for a solution within the range [joint_angles - interval, joint_angles + interval] in each step.

random_restartsbool, optional (default: True)

Allow random restarts if no solution is found.

random_statenp.random.RandomState, optional (default: np.random)

Random state.

solverstr, optional (default: ‘SLSQP’)

Optimizer to solve inverse kinematics problem. Possible options: ‘SLSQP’, ‘L-BFGS-B’, and ‘COBYLA’.

orientation_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the orientation error.

position_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the position error.

Returns:

Qarray, shape (n_steps, n_joints)

Solution

inverse_with_random_restarts(desired_pose, n_restarts=10, tolerance=0.001, random_state=<module 'numpy.random' from '/home/dfki.uni-bremen.de/afabisch/anaconda3/lib/python3.9/site-packages/numpy/random/__init__.py'>, solver='SLSQP', orientation_weight=1.0, position_weight=1.0)

Compute inverse kinematics with multiple random restarts.

Parameters:

desired_posearray-like, shape (4, 4)

Desired pose.

n_restartsint, optional (default: 10)

Maximum number of allowed restarts.

tolerancefloat, optional (default: 1e-3)

Required tolerance to abort.

random_statenp.random.RandomState, optional (default: np.random)

Random state.

solverstr, optional (default: ‘SLSQP’)

Optimizer to solve inverse kinematics problem. Possible options: ‘SLSQP’, ‘L-BFGS-B’, and ‘COBYLA’.

orientation_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the orientation error.

position_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the position error.

Returns:

joint_anglesarray, shape (n_joints,)

Solution

local_inverse_with_random_restarts(desired_pose, joint_angles, interval, n_restarts=10, tolerance=0.001, random_state=<module 'numpy.random' from '/home/dfki.uni-bremen.de/afabisch/anaconda3/lib/python3.9/site-packages/numpy/random/__init__.py'>, solver='SLSQP', orientation_weight=1.0, position_weight=1.0)

Compute inverse kinematics with multiple random restarts.

Parameters:

desired_posearray-like, shape (4, 4)

Desired pose.

joint_anglesarray-like, shape (n_joints,)

Initial guess for joint angles.

intervalfloat

We will search for a solution within the range [joint_angles - interval, joint_angles + interval].

n_restartsint, optional (default: 10)

Maximum number of allowed restarts.

tolerancefloat, optional (default: 1e-3)

Required tolerance to abort.

random_statenp.random.RandomState, optional (default: np.random)

Random state.

solverstr, optional (default: ‘SLSQP’)

Optimizer to solve inverse kinematics problem. Possible options: ‘SLSQP’, ‘L-BFGS-B’, and ‘COBYLA’.

orientation_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the orientation error.

position_weightfloat, optional (default: 1.0)

Should be between 0.0 and 1.0 and represent the weighting for minimizing the position error.

Returns:

joint_anglesarray, shape (n_joints,)

Solution