reachy.parts.kinematic

Kinematic forward and inverse utility module.

  • Allow the creation of kinematic chain (following a simplified DH notation)

  • Compute the forward kinematic

  • Provide optimization via scipy for inverse approximation

Module Contents

Classes

Link

Simplified Link following DH notation.

Chain

Chain of Link forming a kinematic chain.

Functions

translation_matrix(translation)

Create homogenous matrix given a 3D translation vector.

position_dist(P, Q)

Compute euclidian distance between two 3D position.

rotation_dist(P, Q)

Compute rotation distance between two 3D rotation.

pose_dist(M1, M2, threshold=-1)

Compute distance between two pose.

Bases: object

Simplified Link following DH notation.

Parameters

  • translation (ndarray) – 3d translation from the previous link

  • rotation (ndarray) – 3d euler rotation (xyz) from the previous link

transformation_matrix(self, theta)

Compute local transformation matrix for given angle.

Parameters

theta (float) – joint rotation angle (in radians)

class reachy.parts.kinematic.Chain(links)

Bases: object

Chain of Link forming a kinematic chain.

Parameters

links (list) – list of Link

property bounds(self)

Get bounds for each link.

forward(self, joints)

Compute forward kinematics of the chain given joints configurations.

Parameters

joints (ndarray) – N*J array joint rotation angle for each link (in radians)

Returns

4x4 homogeneous matrix pose of the end effector

Return type

ndarray

Warning

this is a vectorized version of the forward!

inverse(self, poses, q0s, maxiter=10)

Approximate the inverse kinematics of the chain given end pose.

Parameters

  • poses (ndarray) – N*4*4 homogeneous matrix poses for the end effector

  • q0s (ndarray) – N*J initial joint configuration used to bootstrap the optimization

  • maxiter (int) – maximum number of iteration to run on the optimizer

Warning

this is a vectorized version of the forward!

_inverse(self, target, q0, maxiter)
reachy.parts.kinematic.translation_matrix(translation)

Create homogenous matrix given a 3D translation vector.

reachy.parts.kinematic.position_dist(P, Q)

Compute euclidian distance between two 3D position.

reachy.parts.kinematic.rotation_dist(P, Q)

Compute rotation distance between two 3D rotation.

reachy.parts.kinematic.pose_dist(M1, M2, threshold=- 1)

Compute distance between two pose.

The distance is defined as the sum of the position distance and the rotation distance where 1° of error is equal to 1mm error distance.