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.axis_angle_from_quat()

axisAngleFromQuat( (object)quat) -> object :

RaveVector < T > axisAngleFromQuat(const RaveVector < T > & quat)

Converts a quaternion into the axis-angle representation.

Parameters
quat -
quaternion, (s,vx,vy,vz)
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.
R

Rotation matrix R from local to world coordinates.

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}\).

__del__()

Add body removal to garbage collection step (effective).

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

index

OpenRAVE index of the body.

Notes

This index is notably used to compute jacobians and hessians.

n

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

name

Body name.

normal

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

p

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

pitch

Pitch angle of the body orientation.

pos

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

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.

quat

Quaternion of the rigid body orientation.

remove()

Remove body from OpenRAVE environment.

roll

Roll angle of the body orientation.

rotation_matrix

Rotation matrix R from local to world coordinates.

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.

name : string
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.

t

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

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

x-coordinate in the world frame.

y

y-coordinate in the world frame.

yaw

Yaw angle of the body orientation.

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

index

Index used in Jacobian and Hessian computations.

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].
pd

Point velocity.

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

Point velocity along x-axis.

xdd

Point acceleration along x-axis.

yd

Point velocity along y-axis.

ydd

Point acceleration along y-axis.

zd

Point velocity along z-axis.

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

R

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

Returns:R – Rotation matrix.
Return type:array, shape=(3, 3)
T

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

Returns:R – Rotation matrix.
Return type:array, shape=(3, 3)
b

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

cam

Centroidal angular momentum.

com

Position of the center of mass in the world frame.

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.

n

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

p

Position of the free-flyer in the world frame.

Returns:p – Position coordinates in the world frame.
Return type:array, shape=(3,)
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,)
quat

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

Returns:quat – Quaternion vector (w, x, y, z).
Return type:array, shape=(4,)
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.

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.

q

Vector of DOF values.

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.