# 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. Tp – Result after applying the transformation. (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. C – Cross-product matrix of x. (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. Ri – Rotation matrix after integration. (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. T_f – Affine transform after integration. (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. Omega – Integral of omegad for dt starting from omega. (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. pose – Pose vector [qw qx qy qz x y z] of the transformation matrix. (7,) array
pymanoid.transformations.quat_from_rotation_matrix(R)

Quaternion from rotation matrix.

Parameters: R ((3, 3) array) – Rotation matrix. quat – Quaternion in [w x y z] format. (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]. quat – Quaternion in [w x y z] format. (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. R – Rotation matrix. (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]. R – Rotation matrix. (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. rpy – Array of roll-pitch-yaw angles, in [rad]. (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. rpy – Array of roll-pitch-yaw angles, in [rad]. (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]. T – Homogeneous transformation matrix of the pose vector. (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. T_inv – Inverse of T. (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. 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]. contact – Contact located at manipulator pose. 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. array, shape=(3, 3)
T

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

Returns: R – Rotation matrix. 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. am – Angular momentum of the robot at p. 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. H – Hessian tensor of the angular momentum at the application point. 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. J_am – Jacobian giving the angular momentum of the robot at P. 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. H – Hessian matrix for the centroidal angular-momentum. 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. array
compute_cam_rate(qdd)

Compute the time-derivative of the CAM.

Parameters: qdd (array) – Vector of joint accelerations. cam – Coordinates of the angular momentum at the COM. 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. comdd – COM acceleration. (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. 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. w_P – Coordinates of the gravito-inertial wrench expressed at point P in the world frame. 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. w_P – Coordinates of the gravito-inertial wrench expressed at point P in the world frame. 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. zmp – ZMP of the gravito-inertial wrench in the plane (O, n). 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. PointMass
get_dof_name_from_index(index)

Get DOF name from its index in the kinematic chain.

Parameters: index (integer) – DOF index. name – DOF name. 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. 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). array, shape=(7,)
quat

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

Returns: quat – Quaternion vector (w, x, y, z). 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). 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. H_contact – Contact Hessian. array
compute_contact_jacobian(contacts)

Compute the contact Jacobian.

Parameters: contacts (pymanoid.ContactSet) – Contacts between the robot and its environment. J_contact – Contact Jacobian. 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. 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. 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.

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.

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

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

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.

compute_static_gravity_torques(external_torque=None)

Compute static-equilibrium torques for the manipulator.

Parameters: external_torque (array) – Vector of external joint torques. tg – Vector of static joint torques compensating gravity, and external_torque if applicable. 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. 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. q_indices – Vector of DOF values for these indices. 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. qd_indices – Vector of DOF velocities for these indices. array

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.