Forward kinematics

Rigid transformations

Rotations and rigid-body transformations can be represented in many ways. For rotations, the three main formats used in pymanoid are:

  • Roll-pitch-yaw angles: that is to say Euler angles corresponding to the sequence (1, 2, 3).

  • Quaternions: 4D vectors [w x y z], with the scalar term w coming first following the OpenRAVE convention.

  • Rotation matrices: \(3 \times 3\) matrices \(R\) whose inverse is equal to their transpose.

Rigid-body transformations can be represented by:

  • Poses: 7D vectors consisting of the quaternion of the orientation followed by its position.

  • Transformation matrices: \(4 \times 4\) matrices \(T\).

Functions are provided to convert between all these representations. Most of them are adapted from the comprehensive guide [Diebel06].

pymanoid.transformations.apply_transform(T, p)

Apply a transformation matrix T to point coordinates p.

Parameters

  • T ((4, 4) array) – Homogeneous transformation matrix.

  • p ((7,) or (3,) array) – Pose or point coordinates.

Returns

Tp – Result after applying the transformation.

Return type

(7,) or (3,) array

Notes

For a single point, it is faster to apply the matrix multiplication directly rather than calling the OpenRAVE function:

In [33]: %timeit dot(T, hstack([p, 1]))[:3]
100000 loops, best of 3: 3.82 µs per loop

In [34]: %timeit list(transformPoints(T, [p]))
100000 loops, best of 3: 6.4 µs per loop
pymanoid.transformations.crossmat(x)

Cross-product matrix of a 3D vector.

Parameters

x ((3,) array) – Vector on the left-hand side of the cross product.

Returns

C – Cross-product matrix of x.

Return type

(3, 3) array

pymanoid.transformations.integrate_angular_acceleration(R, omega, omegad, dt)

Integrate constant angular acceleration \(\dot{\omega}\) for a duration dt starting from the orientation \(R\) and angular velocity \(\omega\).

Parameters

  • R ((3, 3) array) – Rotation matrix.

  • omega ((3,) array) – Initial angular velocity.

  • omegad ((3,) array) – Constant angular acceleration.

  • dt (scalar) – Duration.

Returns

Ri – Rotation matrix after integration.

Return type

(3, 3) array

Notes

This function applies Rodrigues’ rotation formula. See also the following nice post on integrating rotations.

pymanoid.transformations.integrate_body_acceleration(T, v, a, dt)

Integrate constant body acceleration \(a\) for a duration dt starting from the affine transform \(T\) and body velocity \(v\).

Parameters

  • T ((4, 4) array) – Affine transform of the rigid body.

  • v ((3,) array) – Initial body velocity of the rigid body.

  • a ((3,) array) – Constant body acceleration.

  • dt (scalar) – Duration.

Returns

T_f – Affine transform after integration.

Return type

(3, 3) array

Notes

With full frame notations, the affine transform \(T = {}^W T_B\) maps vectors from the rigid body frame \(B\) to the inertial frame \(W\). Its body velocity \(v = \begin{bmatrix} v_B & \omega \end{bmatrix}\) is a twist in the inertial frame, with coordinates taken at the origin of \(B\). The same goes for its body acceleration \(a = \begin{bmatrix} a_B & \dot{\omega} \end{bmatrix}\).

pymanoid.transformations.magnus_expansion(omega, omegad, dt)

Compute the integral \(\Omega\) obtained by integrating \(\dot{\omega}\) for a duration dt starting from \(\omega\).

Parameters

  • omega ((3,) array) – Initial angular velocity.

  • omegad ((3,) array) – Constant angular acceleration.

  • dt (scalar) – Duration.

Returns

Omega – Integral of omegad for dt starting from omega.

Return type

(3,) array

Notes

This function only computes the first three terms of the Magnus expansion. See <https://github.com/jrl-umi3218/RBDyn/pull/66/files> if you need a more advanced version.

pymanoid.transformations.pose_from_transform(T)

Pose vector from a homogeneous transformation matrix.

Parameters

T ((4, 4) array) – Homogeneous transformation matrix.

Returns

pose – Pose vector [qw qx qy qz x y z] of the transformation matrix.

Return type

(7,) array

pymanoid.transformations.quat_from_rotation_matrix(R)

Quaternion from rotation matrix.

Parameters

R ((3, 3) array) – Rotation matrix.

Returns

quat – Quaternion in [w x y z] format.

Return type

(4,) array

pymanoid.transformations.quat_from_rpy(rpy)

Quaternion frmo roll-pitch-yaw angles.

Parameters

rpy ((3,) array) – Vector of roll-pitch-yaw angles in [rad].

Returns

quat – Quaternion in [w x y z] format.

Return type

(4,) array

Notes

Roll-pitch-yaw are Euler angles corresponding to the sequence (1, 2, 3).

pymanoid.transformations.rotation_matrix_from_quat(quat)

Rotation matrix from quaternion.

Parameters

quat ((4,) array) – Quaternion in [w x y z] format.

Returns

R – Rotation matrix.

Return type

(3, 3) array

pymanoid.transformations.rotation_matrix_from_rpy(rpy)

Rotation matrix from roll-pitch-yaw angles.

Parameters

rpy ((3,) array) – Vector of roll-pitch-yaw angles in [rad].

Returns

R – Rotation matrix.

Return type

(3, 3) array

pymanoid.transformations.rpy_from_quat(quat)

Roll-pitch-yaw angles of a quaternion.

Parameters

quat ((4,) array) – Quaternion in [w x y z] format.

Returns

rpy – Array of roll-pitch-yaw angles, in [rad].

Return type

(3,) array

Notes

Roll-pitch-yaw are Euler angles corresponding to the sequence (1, 2, 3).

pymanoid.transformations.rpy_from_rotation_matrix(R)

Roll-pitch-yaw angles of rotation matrix.

Parameters

R (array) – Rotation matrix.

Returns

rpy – Array of roll-pitch-yaw angles, in [rad].

Return type

(3,) array

Notes

Roll-pitch-yaw are Euler angles corresponding to the sequence (1, 2, 3).

pymanoid.transformations.transform_from_pose(pose)

Transformation matrix from a pose vector.

Parameters

pose ((7,) array) – Pose vector [qw qx qy qz x y z].

Returns

T – Homogeneous transformation matrix of the pose vector.

Return type

(4, 4) array

pymanoid.transformations.transform_inverse(T)

Inverse of a transformation matrix. Yields the same result but faster than numpy.linalg.inv() on such matrices.

Parameters

T ((4, 4) array) – Homogeneous transformation matrix.

Returns

T_inv – Inverse of T.

Return type

(4, 4) array

Rigid body model

class pymanoid.body.Body(rave_body, pos=None, rpy=None, pose=None, color=None, visible=True)

Base class for rigid bodies. Wraps OpenRAVE’s KinBody type.

Parameters

  • rave_body (openravepy.KinBody) – OpenRAVE body to wrap.

  • pos (array, shape=(3,), optional) – Initial position in inertial frame.

  • rpy (array, shape=(3,), optional) – Initial orientation in inertial frame.

  • pose (array, shape=(7,), optional) – Initial pose. Supersedes pos and rpy if they are provided at the same time.

  • color (char, optional) – Color code in matplotlib convention (‘b’ for blue, ‘g’ for green, …).

  • visible (bool, optional) – Visibility in the GUI.

property R

Rotation matrix R from local to world coordinates.

property T

Homogeneous coordinates of the rigid body.

These coordinates describe the orientation and position of the rigid body by the 4 x 4 transformation matrix

\[\begin{split}T = \left[ \begin{array}{cc} R & p \\ 0_{1 \times 3} & 1 \end{array} \right]\end{split}\]

where R is a 3 x 3 rotation matrix and p is the vector of position coordinates.

Notes

More precisely, T is the transformation matrix from the body frame to the world frame: if \(\tilde{p}_\mathrm{body} = [x\ y\ z\ 1]\) denotes the homogeneous coordinates of a point in the body frame, then the homogeneous coordinates of this point in the world frame are \(\tilde{p}_\mathrm{world} = T \tilde{p}_\mathrm{body}\).

property adjoint_matrix

Adjoint matrix converting wrenches in the local frame \(\cal L\) to the inertial frame \(\cal W\), that is the matrix \({}^{\cal W}A_{\cal L}\) such that:

\[{}^{\cal W}w = {}^{\cal W}A_{\cal L} {}^{\cal L} w\]
Returns

A – Adjoint matrix.

Return type

array

apply_twist(v, omega, dt)

Apply a twist \([v\ \omega]\) defined in the local coordinate frame.

Parameters

  • v ((3,) array) – Linear velocity in local frame.

  • omega ((3,) array) – Angular velocity in local frame.

  • dt (scalar) – Duration of twist application in [s].

property b

Binormal vector directing the y-axis of the body frame.

dist(point)

Distance from the body frame origin to another point.

Parameters

point (array or Point) – Point to compute the distance to.

hide()

Make the body invisible.

property index

OpenRAVE index of the body.

Notes

This index is notably used to compute jacobians and hessians.

property n

Normal vector directing the z-axis of the body frame.

property name

Body name.

property normal

Normal vector directing the z-axis of the body frame.

property p

Position coordinates [x y z] in the world frame.

property pitch

Pitch angle of the body orientation.

property pos

Position coordinates [x y z] in the world frame.

property pose

Body pose as a 7D quaternion + position vector.

The pose vector \([q_w\,q_x\,q_y\,q_z\,x\,y\,z]\) consists of a quaternion \(q = [q_w\,q_x\,q_y\,q_z]\) (with the real term \(q_w\) coming first) for the body orientation, followed by the coordinates \(p = [x\,y\,z]\) in the world frame.

property quat

Quaternion of the rigid body orientation.

remove()

Remove body from OpenRAVE environment.

property roll

Roll angle of the body orientation.

property rotation_matrix

Rotation matrix R from local to world coordinates.

property rpy

Roll-pitch-yaw angles.

They correspond to Euleur angles for the sequence (1, 2, 3). See [Diebel06] for details.

set_color(color)

Set the color of the rigid body.

Parameters

color (tuple or string) – RGB tuple, or color code in matplotlib convention.

set_name(name)

Set body name in OpenRAVE scope.

namestring

Body name.

set_pitch(pitch)

Set the pitch angle of the body orientation.

Parameters

pitch (scalar) – Pitch angle in [rad].

set_pos(pos)

Set the position of the body in the world frame.

Parameters

pos (array, shape=(3,)) – 3D vector of position coordinates.

set_pose(pose)

Set the 7D pose of the body orientation.

Parameters

pose ((7,) array) – Pose of the body, i.e. quaternion + position in world frame.

set_quat(quat)

Set the quaternion of the body orientation.

Parameters

quat ((4,) array) – Quaternion in (w, x, y, z) format.

set_roll(roll)

Set the roll angle of the body orientation.

Parameters

roll (scalar) – Roll angle in [rad].

set_rotation_matrix(R)

Set the orientation of the rigid body.

Recall that this orientation is described by the rotation matrix R from the body frame to the world frame.

Parameters

R ((3, 3) array) – Rotation matrix.

set_rpy(rpy)

Set the roll-pitch-yaw angles of the body orientation.

Parameters

rpy (scalar triplet) – Triplet (r, p, y) of roll-pitch-yaw angles.

set_transform(T)

Set homogeneous coordinates of the rigid body.

Parameters

T (array, shape=(4, 4)) – Transform matrix.

set_transparency(transparency)

Set the transparency of the rigid body.

Parameters

transparency (double, optional) – Transparency value from 0 (opaque) to 1 (invisible).

set_x(x)

Set the x-coordinate of the body in the world frame.

Parameters

x (scalar) – New x-coordinate.

set_y(y)

Set the y-coordinate of the body in the world frame.

Parameters

y (scalar) – New y-coordinate.

set_yaw(yaw)

Set the yaw angle of the body orientation.

Parameters

yaw (scalar) – Yaw angle in [rad].

set_z(z)

Set the z-coordinate of the body in the world frame.

Parameters

z (scalar) – New z-coordinate.

show()

Make the body visible.

property t

Tangent vector directing the x-axis of the body frame.

property transform

Homogeneous coordinates of the rigid body.

These coordinates describe the orientation and position of the rigid body by the 4 x 4 transformation matrix

\[\begin{split}T = \left[ \begin{array}{cc} R & p \\ 0_{1 \times 3} & 1 \end{array} \right]\end{split}\]

where R is a 3 x 3 rotation matrix and p is the vector of position coordinates.

Notes

More precisely, T is the transformation matrix from the body frame to the world frame: if \(\tilde{p}_\mathrm{body} = [x\ y\ z\ 1]\) denotes the homogeneous coordinates of a point in the body frame, then the homogeneous coordinates of this point in the world frame are \(\tilde{p}_\mathrm{world} = T \tilde{p}_\mathrm{body}\).

translate(translation)

Apply a translation to the body.

Parameters

translation ((3,) array) – Offset to apply to the position (world coordinates) of the body.

property x

x-coordinate in the world frame.

property y

y-coordinate in the world frame.

property yaw

Yaw angle of the body orientation.

property z

z-coordinate in the world frame.

class pymanoid.body.Box(X, Y, Z, pos=None, rpy=None, pose=None, color='r', visible=True, dZ=0.0)

Rectangular box.

Parameters

  • X (scalar) – Box half-length in [m].

  • Y (scalar) – Box half-width in [m].

  • Z (scalar) – Box half-height in [m].

  • pos (array, shape=(3,)) – Initial position in the world frame.

  • rpy (array, shape=(3,)) – Initial orientation in the world frame.

  • pose (array, shape=(7,)) – Initial pose in the world frame.

  • color (char) – Color letter in [‘r’, ‘g’, ‘b’].

  • visible (bool, optional) – Visibility in the GUI.

  • dZ (scalar, optional) – Shift in box normal coordinates used to make Contact slabs.

class pymanoid.body.Cube(size, pos=None, rpy=None, pose=None, color='r', visible=True)

Cube.

Parameters

  • size (scalar) – Half-length of a side of the cube in [m].

  • pos (array, shape=(3,)) – Initial position in the world frame.

  • rpy (array, shape=(3,)) – Initial orientation in the world frame.

  • pose (array, shape=(7,)) – Initial pose in the world frame.

  • color (char) – Color letter in [‘r’, ‘g’, ‘b’].

  • visible (bool, optional) – Visibility in the GUI.

class pymanoid.body.Manipulator(manipulator, pos=None, rpy=None, pose=None, color=None, visible=True, shape=None, friction=None)

Manipulators are special bodies with an end-effector property.

Parameters

  • manipulator (openravepy.KinBody) – OpenRAVE manipulator object.

  • pos (array, shape=(3,), optional) – Initial position in inertial frame.

  • rpy (array, shape=(3,), optional) – Initial orientation in inertial frame.

  • pose (array, shape=(7,), optional) – Initial pose. Supersedes pos and rpy if they are provided at the same time.

  • color (char, optional) – Color code in matplotlib convention (‘r’ for red, ‘b’ for blue, etc.).

  • visible (bool, optional) – Visibility in the GUI.

  • shape ((scalar, scalar), optional) – Dimensions (half-length, half-width) of a contact patch in [m].

  • friction (scalar, optional) – Static friction coefficient for potential contacts.

property force

Resultant of contact forces applied on the effector (if defined). Coordinates are given in the end-effector frame.

get_contact(pos=None, shape=None)

Get contact located at the current manipulator pose.

Parameters

  • pos ((3,) array, optional) – Override manipulator position with this one.

  • shape ((scalar, scalar), optional) – Dimensions (half-length, half-width) of contact patch in [m].

Returns

contact – Contact located at manipulator pose.

Return type

Contact

property index

Index used in Jacobian and Hessian computations.

property moment

Moment of contact forces applied on the effector (if defined). Coordinates are given in the end-effector frame.

class pymanoid.body.Point(pos, vel=None, accel=None, size=0.01, color='r', visible=True)

Points represented by cubes with a default size.

Parameters

  • pos (array, shape=(3,)) – Initial position in the world frame.

  • vel (array, shape=(3,), optional) – Initial velocity in the world frame.

  • accel (array, shape=(3,), optional) – Initial acceleration in the world frame.

  • size (scalar, optional) – Half-length of a side of the cube in [m].

  • color (char) – Color letter in [‘r’, ‘g’, ‘b’].

  • visible (bool, optional) – Visibility in the GUI.

copy(color='r', visible=True)

Copy constructor.

Parameters

  • color (char, optional) – Color of the copy, in [‘r’, ‘g’, ‘b’].

  • visible (bool, optional) – Should the copy be visible?

integrate_constant_accel(pdd, dt)

Apply Euler integration for a constant acceleration.

Parameters

  • pdd (array, shape=(3,)) – Point acceleration in the world frame.

  • dt (scalar) – Duration in [s].

integrate_constant_jerk(pddd, dt)

Apply Euler integration for a constant jerk.

Parameters

  • pddd (array, shape=(3,)) – Point jerk in the world frame.

  • dt (scalar) – Duration in [s].

property pd

Point velocity.

property pdd

Point acceleration.

set_accel(pdd)

Update the point acceleration.

Parameters

pdd (array, shape=(3,)) – Acceleration coordinates in the world frame.

set_vel(pd)

Update the point velocity.

Parameters

pd (array, shape=(3,)) – Velocity coordinates in the world frame.

property xd

Point velocity along x-axis.

property xdd

Point acceleration along x-axis.

property yd

Point velocity along y-axis.

property ydd

Point acceleration along y-axis.

property zd

Point velocity along z-axis.

property zdd

Point acceleration along z-axis.

class pymanoid.body.PointMass(pos, mass, vel=None, color='r', visible=True, size=None)

Point with a mass property and a size proportional to it.

Parameters

  • pos ((3,) array) – Initial position in the world frame.

  • mass (scalar) – Total mass in [kg].

  • vel ((3,) array, optional) – Initial velocity in the world frame.

  • color (char, optional) – Color letter in [‘r’, ‘g’, ‘b’].

  • visible (bool, optional) – Visibility in the GUI.

  • size (scalar, optional) – Half-length of a side of the CoM cube handle, in [m].

copy(color='r', visible=True)

Copy constructor.

Parameters

  • color (char, optional) – Color of the copy, in [‘r’, ‘g’, ‘b’].

  • visible (bool, optional) – Should the copy be visible?

property momentum

Linear momentum in the world frame.

Robot model

class pymanoid.robot.Humanoid(path, root_body)

Humanoid robots add a free base and centroidal computations.

property R

Rotation matrix R of from free-flyer to the world frame.

Returns

R – Rotation matrix.

Return type

array, shape=(3, 3)

property T

Rotation matrix R of from free-flyer to the world frame.

Returns

R – Rotation matrix.

Return type

array, shape=(3, 3)

property b

Binormal vector directing the y-axis of the free-flyer frame.

property cam

Centroidal angular momentum.

property com

Position of the center of mass in the world frame.

property comd

Velocity of the center of mass in the world frame.

compute_angular_momentum(p)

Compute the angular momentum with respect to point p.

Parameters

p (array, shape=(3,)) – Application point p in world coordinates.

Returns

am – Angular momentum of the robot at p.

Return type

array, shape=(3,)

compute_angular_momentum_hessian(p)

Returns the Hessian tensor \(H(q)\) such that the rate of change of the angular momentum with respect to point P is

\[\dot{L}_P(q, \dot{q}) = J(q) \ddot{q} + \dot{q}^T H(q) \dot{q}\]

where \(J(q)\) is the angular-momentum jacobian.

Parameters

p (array, shape=(3,)) – Application point in world coordinates.

Returns

H – Hessian tensor of the angular momentum at the application point.

Return type

array, shape=(3, 3, 3)

compute_angular_momentum_jacobian(p)

Compute the Jacobian matrix J(q) such that the angular momentum of the robot at P is given by:

\[L_P(q, \dot{q}) = J(q) \dot{q}\]
Parameters

p (array, shape=(3,)) – Application point P in world coordinates.

Returns

J_am – Jacobian giving the angular momentum of the robot at P.

Return type

array, shape=(3,)

compute_cam()

Compute the centroidal angular momentum.

compute_cam_hessian(q)

Compute the matrix H(q) such that the rate of change of the CAM is

\[\dot{L}_G(q, \dot{q}) = J(q) \ddot{q} + \dot{q}^T H(q) \dot{q}\]
Parameters

q (array) – Vector of joint angles.

Returns

H – Hessian matrix for the centroidal angular-momentum.

Return type

array

compute_cam_jacobian()

Compute the jacobian matrix J(q) such that the CAM is given by:

\[L_G(q, \dot{q}) = J(q) \dot{q}\]
Returns

J_cam – Jacobian matrix mapping to the centroidal angular momentum.

Return type

array

compute_cam_rate(qdd)

Compute the time-derivative of the CAM.

Parameters

qdd (array) – Vector of joint accelerations.

Returns

cam – Coordinates of the angular momentum at the COM.

Return type

array

Note

This function is not optimized.

compute_com_acceleration(qdd)

Compute COM acceleration from joint accelerations \(\ddot{q}\).

Parameters

qdd (array) – Vector of DOF accelerations.

Returns

comdd – COM acceleration.

Return type

(3,) array

compute_com_hessian()

Compute the Hessian matrix H(q) of the center of mass G, such that the acceleration of G in the world frame is:

\[\ddot{p}_G = J(q) \ddot{q} + \dot{q}^T H(q) \dot{q}\]
compute_com_jacobian()

Compute the Jacobian matrix \(J_\mathrm{com}(q)\) of the center of mass G, such that the velocity of G in the world frame is:

\[\dot{p}_G = J_\mathrm{com}(q) \dot{q}\]
Returns

J_com – Jacobian of the center of mass.

Return type

array

compute_gravito_inertial_wrench(qdd, p)

Compute the gravito-inertial wrench at point P:

\[\begin{split}w_P = \left[\begin{array}{c} f^{gi} \\ \tau^{gi}_P \end{array}\right] = \left[\begin{array}{c} m (g - \ddot{p}_G) \\ (p_G - p_P) \times m (g - \ddot{p}_G) - \dot{L}_G \end{array}\right]\end{split}\]

with m the robot mass, g the gravity vector, G the center of mass, \(\ddot{p}_G\) the COM acceleration, and \(\dot{L}_G\) the rate of change of the centroidal angular momentum.

Parameters

  • qdd (array) – Vector of DOF accelerations.

  • p (array, shape=(3,)) – Application point of the gravito-inertial wrench.

Returns

w_P – Coordinates of the gravito-inertial wrench expressed at point P in the world frame.

Return type

array, shape=(6,)

compute_net_contact_wrench(qdd, p)

Compute the gravito-inertial wrench at point P:

\[\begin{split}w_P = \left[\begin{array}{c} f^{c} \\ \tau^{c}_P \end{array}\right] = \sum_i \left[\begin{array}{c} f_i \\ (p_i - p_P) \times f_i + \tau_i \end{array}\right]\end{split}\]

with the contact wrench \((f_i, \tau_i)\) is applied at the \(i^\mathrm{th}\) contact point located at \(p_i\).

Parameters

  • qdd (array) – Vector of DOF accelerations.

  • p (array, shape=(3,)) – Application point of the gravito-inertial wrench.

Returns

w_P – Coordinates of the gravito-inertial wrench expressed at point P in the world frame.

Return type

array, shape=(6,)

Notes

From the Newton-Euler equations of the system, the net contact wrench is opposite to the gravito-inertial wrench computed by pymanoid.robot.Humanoid.compute_gravito_inertial_wrench().

compute_zmp(qdd, origin=None, normal=None)

Compute the Zero-tilting Moment Point (ZMP).

Parameters

  • qdd (array) – Vector of joint accelerations.

  • origin (array, shape=(3,), optional) – Origin O of the ZMP plane. Defaults to the origin of the world frame.

  • normal (array, shape=(3,), optional) – Normal n of the ZMP plane. Defaults to the vertical.

Returns

zmp – ZMP of the gravito-inertial wrench in the plane (O, n).

Return type

array, shape=(3,)

Notes

See [Sardain04] for an excellent introduction to the concepts of ZMP and center of pressure. See [Caron17z] for the more general definition of ZMP support areas in arbitrary planes.

get_com_point_mass()

Get the center of mass as a PointMass instance.

Returns

com – Center of mass of the robot.

Return type

PointMass

get_dof_name_from_index(index)

Get DOF name from its index in the kinematic chain.

Parameters

index (integer) – DOF index.

Returns

name – DOF name.

Return type

string

hide_com()

Hide center of mass.

property n

Normal vector directing the z-axis of the free-flyer frame.

property p

Position of the free-flyer in the world frame.

Returns

p – Position coordinates in the world frame.

Return type

array, shape=(3,)

property pose

Pose of the free-flyer in the world frame.

Returns

pose – Pose in OpenRAVE format (qw, qx, qy, qz, x, y, z).

Return type

array, shape=(7,)

property quat

Quaternion of the free-flyer orientation in the world frame.

Returns

quat – Quaternion vector (w, x, y, z).

Return type

array, shape=(4,)

property rpy

Orientation of the free-flyer in the world frame.

Returns

rpy – Roll-pitch-yaw angles, corresponding to Euler sequence (1, 2, 3).

Return type

array, shape=(3,)

set_dof_values(q, dof_indices=None, clamp=False)

Set the joint values of the robot.

Parameters

  • q (array) – Vector of joint angle values (ordered by DOF indices).

  • dof_indices (list of integers, optional) – List of DOF indices to update.

  • clamp (bool, optional) – Correct q if it exceeds joint limits (not done by default).

set_dof_velocities(qd, dof_indices=None)

Set the joint velocities of the robot.

Parameters

  • q (array) – Vector of joint angular velocities (ordered by DOF indices).

  • dof_indices (list of integers, optional) – List of DOF indices to update.

set_pos(pos)

Set the position of the free-flyer, a.k.a. free-floating or base frame of the robot.

Parameters

pos (array, shape=(3,)) – Position coordinates in the world frame.

set_pose(pose)

Set the pose of the free-flyer, a.k.a. free-floating or base frame of the robot.

Parameters

pose (array, shape=(7,)) – Pose in OpenRAVE format (qw, qx, qy, qz, x, y, z).

set_quat(quat)

Set the orientation of the free-flyer, a.k.a. free-floating or base frame of the robot.

Parameters

quat (array, shape=(4,)) – Quaternion vector (w, x, y, z).

set_rpy(rpy)

Set the orientation of the free-flyer, a.k.a. free-floating or base frame of the robot.

Parameters

rpy (array, shape=(3,)) – Roll-pitch-yaw angles, corresponding to Euler sequence (1, 2, 3).

show_com()

Show a red ball at the location of the center of mass.

property t

Tangent vector directing the x-axis of the free-flyer frame.

class pymanoid.robot.Robot(path=None, xml=None)

Robot with a fixed base. This class wraps OpenRAVE’s Robot type.

Parameters

  • path (string) – Path to the COLLADA model of the robot.

  • xml (string, optional) – Environment description in OpenRAVE XML format.

compute_contact_hessian(contacts)

Compute the contact Hessian.

Parameters

contacts (pymanoid.ContactSet) – Contacts between the robot and its environment.

Returns

H_contact – Contact Hessian.

Return type

array

compute_contact_jacobian(contacts)

Compute the contact Jacobian.

Parameters

contacts (pymanoid.ContactSet) – Contacts between the robot and its environment.

Returns

J_contact – Contact Jacobian.

Return type

array

compute_inertia_matrix()

Compute the inertia matrix \(M(q)\) of the robot, that is, the matrix such that the equations of motion can be written:

\[M(q) \ddot{q} + \dot{q}^T C(q) \dot{q} + g(q) = S^T \tau + J_c(q)^T F\]

with:

  • \(q\) – vector of joint angles (DOF values)

  • \(\dot{q}\) – vector of joint velocities

  • \(\ddot{q}\) – vector of joint accelerations

  • \(C(q)\) – Coriolis tensor (derivative of M(q) w.r.t. q)

  • \(g(q)\) – gravity vector

  • \(S\) – selection matrix on actuated joints

  • \(\tau\) – vector of actuated joint torques

  • \(J_c(q)\) – matrix of stacked contact Jacobians

  • \(F\) – vector of stacked contact wrenches

Returns

M – Inertia matrix for the robot’s current joint angles.

Return type

array

Notes

This function applies the unit-vector method described by Walker & Orin in [Walker82]. It is not efficient, so if you are looking for performance, you should consider more recent libraries such as pinocchio or RBDyn.

compute_inverse_dynamics(qdd=None, external_torque=None)

Wrapper around OpenRAVE’s inverse dynamics function, which implements the Recursive Newton-Euler algorithm by Walker & Orin [Walker82]. It returns the three terms \(t_m\), \(t_c\) and \(t_g\) such that:

\[\begin{split}t_m & = M(q) \ddot{q} \\ t_c & = \dot{q}^T C(q) \dot{q} \\ t_g & = g(q)\end{split}\]

where the equations of motion are written:

\[t_m + t_c + t_g = S^T \tau + J_c(q)^T F\]
Parameters

  • qdd (array, optional) – Vector \(\ddot{q}\) of joint accelerations.

  • external_torque (array, optional) – Additional vector of external joint torques for the right-hand side of the equations of motion.

Returns

  • tm (array) – Torques due to inertial accelerations. Will be None if qdd is not provided.

  • tc (array) – Torques due to nonlinear (centrifugal and Coriolis) effects.

  • tg (array) – Torques due to gravity.

Compute the Hessian H(q) of a frame attached to a robot link, the acceleration of which is given by:

\[\begin{split}\left[\begin{array}{c} a_p \\ \dot{\omega} \end{array}\right] = J(q) \ddot{q} + \dot{q}^T H(q) \dot{q}\end{split}\]

where \(a_p\) is the linear acceleration of the point p (default is the origin of the link frame) and \(\dot{\omega}\) is the angular accelerations of the frame.

Parameters

  • link (integer or pymanoid.Link) – Link identifier: either a link index, or the Link object directly.

  • p (array) – Point coordinates in the world frame.

Compute the Jacobian J(q) of a frame attached to a given link, the velocity of this frame being given by:

\[\begin{split}\left[\begin{array}{c} v_p \\ \omega \end{array}\right] = J(q) \dot{q}\end{split}\]

where \(v_p\) is the linear velocity of the link at point p (default is the origin of the link frame) and \(\omega\) is the angular velocity of the link.

Parameters

  • link (integer or pymanoid.Link) – Link identifier: either a link index, or the Link object directly.

  • p (array) – Point coordinates in the world frame.

Compute the translation Hessian H(q) of a point p on link, i.e. the matrix such that the acceleration of p is given by:

\[a_p = J(q) \ddot{q} + \dot{q}^T H(q) \dot{q}.\]
Parameters

  • link (integer or pymanoid.Link) – Link identifier: either a link index, or the Link object directly.

  • p (array) – Point coordinates in the world frame.

Compute the position Jacobian of a point p on a given robot link.

Parameters

  • link (integer or pymanoid.Link) – Link identifier: either a link index, or the Link object directly.

  • p (array) – Point coordinates in the world frame.

Compute the pose Jacobian of a given link, i.e. the matrix J(q) such that:

\[\begin{split}\left[\begin{array}{c} \dot{\xi} \\ v_L \end{array}\right] = J(q) \dot{q},\end{split}\]

with \(\xi\) a quaternion for the link orientation and \(v_L = \dot{p}_L\) the velocity of the origin L of the link frame, so that the link pose is \([\xi p_L]\) and the left-hand side of the equation above is its time-derivative.

Parameters

link (integer or pymanoid.Link) – Link identifier: either a link index, or the Link object directly.

compute_static_gravity_torques(external_torque=None)

Compute static-equilibrium torques for the manipulator.

Parameters

external_torque (array) – Vector of external joint torques.

Returns

tg – Vector of static joint torques compensating gravity, and external_torque if applicable.

Return type

array

get_dof_limits(dof_indices=None)

Get the pair \((q_\mathrm{min}, q_\mathrm{max})\) of DOF limits.

Parameters

dof_indices (list of DOF indexes, optional) – Only compute limits for these indices.

Returns

  • q_min (array) – Vector of minimal DOF values.

  • q_max (array) – Vector of maximal DOF values.

Notes

This OpenRAVE function is wrapped because it is too slow in practice. On my machine:

In [1]: %timeit robot.get_dof_limits() 1000000 loops, best of 3: 237 ns per loop

In [2]: %timeit robot.rave.GetDOFLimits() 100000 loops, best of 3: 9.24 µs per loop

get_dof_values(dof_indices=None)

Get DOF values for a set of DOF indices.

Parameters

dof_indices (list of integers) – List of DOF indices.

Returns

q_indices – Vector of DOF values for these indices.

Return type

array

get_dof_velocities(dof_indices=None)

Get DOF velocities for a set of DOF indices.

Parameters

dof_indices (list of integers) – List of DOF indices.

Returns

qd_indices – Vector of DOF velocities for these indices.

Return type

array

Get robot link.

Parameters

name (string) – Link name in the robot model.

Returns

link – Link handle.

Return type

pymanoid.Body

hide()

Make the robot invisible.

property q

Vector of DOF values.

property qd

Vector of DOF velocities.

set_color(color)

Set the color of the robot.

Parameters

color (tuple or string) – RGB tuple, or color code in matplotlib convention.

set_dof_limits(q_min, q_max, dof_indices=None)

Set DOF limits of the robot.

Parameters

  • q_min (list or array) – Lower-bound on joint angle values, ordered by DOF indices.

  • q_max (list or array) – Upper-bound on joint angle values, ordered by DOF indices.

  • dof_indices (list, optional) – List of DOF indices to update.

set_dof_values(q, dof_indices=None, clamp=False)

Set DOF values of the robot.

Parameters

  • q (list or array) – Joint angle values, ordered by DOF indices.

  • dof_indices (list, optional) – List of DOF indices to update.

  • clamp (bool) – Correct joint angles when exceeding joint limits.

set_dof_velocities(qd, dof_indices=None)

Set the joint velocities of the robot.

Parameters

  • qd (list or array) – Joint angle velocities, ordered by DOF indices.

  • dof_indices (list, optional) – List of DOF indices to update.

set_transparency(transparency)

Update robot transparency.

Parameters

transparency (scalar) – Transparency value from 0 (opaque) to 1 (invisible).

show()

Make the robot visible.