RBDReference index#

A Python reference implementation of rigid body dynamics algorithms.

This package is designed to enable rapid prototyping and testing of new algorithms and algorithmic optimizations. If your favorite rigid body dynamics algorithm is not yet implemented please submit a PR with the implementation. We’ll then try to get a GPU, FPGA, and/or accelerator implementation designed as soon as possible.

Usage and API:#

This package relies on an already parsed robot object from our URDFParser package.

RBDReference = RBDReference(robot)
outputs = RBDReference.ALGORITHM(inputs)

Currently implemented algorithms include the: + Recursive Newton Euler Algorithm (RNEA): (c,v,a,f) = rbdReference.rnea(q, qd, qdd = None, GRAVITY = -9.81) + The Gradient of the RNEA: dc_du = rnea_grad(q, qd, qdd = None, GRAVITY = -9.81) where dc_du = np.hstack((dc_dq,dc_dqd)) + The Direct Inverse of the Mass Matrix Algorithm: Minv = rbdReference.minv(q, output_dense = True) + The Composite Rigid Body Algorithm: M = rbdReference.crba(q,qd)

We also include functions that break these algorithms down into there different passes and by their output types (dq vs dqd) to enable easier testing of downstream GPU, FPGA, and accelerator implementations.

Testing Algorithms on URDFs#

Run the following in terminal from forked repo here at base of the GRiD working directory in the floating base branch. Run any URDF tests by replacing the desired urdf with the terminal scripts below.

iiwa14.urdf testing python printReferenceValues.py iiwa.urdf -f

This runs the print printReferenceValues.py script located A2R-Lab/GRiD. This will output values for RNEA, Minv, CRBA, RNEA_grad, and more.

Once can test algorithm outputs using Matlab Spatial V2 using the following sequence: 1. Run python compare_rnea_fb_spatial.py INSERT_URDF.urdf -f which will print the exact q, qd, qdd inputs for Spatial V2 floating base algorithms. (Matlab expects floating base joint to input quaternion xyzw and xyz location of Fb joint)

2. After generating compatible inputs with Matlab, create a floating base URDF.m file by running python generate_spatial_model.py INSERT_URDF.urdf -f.

3. Place the newly generated URDF.m file in the working directory of spatial_v2_extended.

4. Run the script after ensuring spatial directories are sourced correctly.

5. Suggested testing includes RNEA (ID), HandC, Hinverse, and more.

Dependencies and Installation#

The only external dependency is numpy, which can be automatically installed by running:

pip3 install -r requirements.txt

This package also depends on our URDFParser package.

API Reference#

The following sections provide an overview of each function, including its purpose, parameters, return values, and example usage.

Functions#

__init__(robotObj)#

Description#

Initializes the RBDReference class, associating it with a given robot object.

Parameters#

robotObjobject
The robot object to associate with this reference. It should represent a multibody system.

Returns#

NoneNone
This function does not return a value.

Example#

from your_module import RBDReference

robot_obj = SomeRobotObject()
rbd_ref = RBDReference(robot_obj)

cross_operator(v)#

Description#

Computes the cross product operator for a given vector v.

Parameters#

vndarray
A vector representing the spatial velocity (or any other relevant quantity) to compute the cross product operator.

Returns#

resultndarray
A 6x6 matrix representing the cross product operator of the input vector.

Example#

from your_module import RBDReference

v = np.array([1, 2, 3, 4, 5, 6])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.cross_operator(v)
print(result)

dual_cross_operator(v)#

Description#

Computes the dual cross product operator for a given vector v.

Parameters#

vndarray
A vector representing the spatial velocity (or any other relevant quantity) to compute the dual cross product operator.

Returns#

resultndarray
A 6x6 matrix representing the dual cross product operator of the input vector.

Example#

from your_module import RBDReference

v = np.array([1, 2, 3, 4, 5, 6])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.dual_cross_operator(v)
print(result)

icrf(v)#

Description#

Computes the inverse cross product matrix for a vector v.

Parameters#

vndarray
A vector representing the spatial velocity (or any other relevant quantity).

Returns#

resultndarray
A 6x6 matrix representing the inverse cross product matrix for the input vector.

Example#

from your_module import RBDReference

v = np.array([1, 2, 3, 4, 5, 6])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.icrf(v)
print(result)

factor_functions(I, v, number=3)#

Description#

Computes a factor function for the system, which is used in various multibody dynamics algorithms.

Parameters#

Indarray
The inertia matrix for the system.
vndarray
The velocity vector for the system.
numberint, optional
A number that selects which formula to use (default is 3).

Returns#

resultndarray
The resulting factor matrix for the system.

Example#

from your_module import RBDReference

I = np.eye(6)
v = np.array([1, 2, 3, 4, 5, 6])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.factor_functions(I, v)
print(result)

_mxS(S, vec, alpha=1.0)#

Description#

Computes the spatial cross product between vectors S and vec.

Parameters#

Sndarray
The first vector (spatial motion vector).
vecndarray
The second vector (spatial force or other relevant vector).
alphafloat, optional
A scaling factor for the operation (default is 1.0).

Returns#

resultndarray
The result of the spatial cross product.

Example#

from your_module import RBDReference

S = np.array([1, 2, 3, 4, 5, 6])
vec = np.array([6, 5, 4, 3, 2, 1])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref._mxS(S, vec)
print(result)

mxS(S, vec)#

Description#

Computes the spatial cross product for a given set of vectors S and vec.

Parameters#

Sndarray
A vector representing the spatial motion.
vecndarray
A vector representing the spatial velocity or force.

Returns#

resultndarray
The resulting spatial cross product.

Example#

from your_module import RBDReference

S = np.array([1, 2, 3, 4, 5, 6])
vec = np.array([6, 5, 4, 3, 2, 1])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.mxS(S, vec)
print(result)

fxv(fxVec, timesVec)#

Description#

Computes the result of multiplying two vectors fxVec and timesVec using spatial operations.

Parameters#

fxVecndarray
The first vector to be used in the operation.
timesVecndarray
The second vector to be used in the operation.

Returns#

resultndarray
The resulting vector after applying the spatial cross product.

Example#

from your_module import RBDReference

fxVec = np.array([1, 2, 3, 4, 5, 6])
timesVec = np.array([6, 5, 4, 3, 2, 1])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.fxv(fxVec, timesVec)
print(result)

fxS(S, vec, alpha=1.0)#

Description#

Computes the spatial cross product between a matrix S and a vector vec.

Parameters#

Sndarray
The matrix to apply the spatial cross product on.
vecndarray
The vector to apply the spatial cross product with.
alphafloat, optional
A scaling factor for the operation (default is 1.0).

Returns#

resultndarray
The resulting spatial cross product.

Example#

from your_module import RBDReference

S = np.array([[1, 0], [0, 1]])
vec = np.array([1, 2, 3, 4, 5, 6])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.fxS(S, vec)
print(result)

vxIv(vec, Imat)#

Description#

Computes the result of multiplying a vector vec by an inertia matrix Imat using spatial operations.

Parameters#

vecndarray
A vector to be multiplied by the inertia matrix.
Imatndarray
The inertia matrix to multiply the vector with.

Returns#

resultndarray
The resulting vector after the multiplication.

Example#

from your_module import RBDReference

vec = np.array([1, 2, 3, 4, 5, 6])
Imat = np.eye(6)
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.vxIv(vec, Imat)
print(result)

apply_external_forces(self, q, f_in, f_ext)#

Description#

Subtracts external forces from the input forces, applying the external forces to the system. This method takes the structure of either a 6/3xNB matrix or a shortened planar vector with length == NB, where f[i] corresponds to the force applied to body i.

Parameters#

f_inndarray
The initial forces applied to links.
f_extndarray
The external forces to subtract.

Returns#

f_outndarray
The updated forces after applying the external forces.

Example#

from your_module import RBDReference

q = np.array([1, 2, 3])
f_in = np.array([1, 1, 1, 0, 0, 0])
f_ext = np.array([0, 0, 0, 0, 0, 0])
rbd_ref = RBDReference(robot_obj)
result = rbd_ref.apply_external_forces(q, f_in, f_ext)
print(result)

rnea_fpass(self, q, qd, qdd=None, GRAVITY=-9.81)#

Description#

Performs the forward pass of the Recursive Newton-Euler Algorithm (RNEA) for computing spatial velocities, accelerations, and forces.

Parameters#

qndarray
The joint positions of the robot.
qdndarray
The joint velocities.
qddndarray, optional
The joint accelerations.
GRAVITYfloat, optional
The gravitational constant. Default is -9.81.

Returns#

vndarray
The spatial velocities.
andarray
The spatial accelerations.
fndarray
The spatial forces.

Example#

from your_module import RBDReference

q = np.array([1, 2, 3])
qd = np.array([0, 1, 0])
rbd_ref = RBDReference(robot_obj)
v, a, f = rbd_ref.rnea_fpass(q, qd)
print(v, a, f)

rnea_bpass(self, q, f)#

Description#

Performs the backward pass of the Recursive Newton-Euler Algorithm (RNEA), computing joint forces.

Parameters#

qndarray
The joint positions.
fndarray
The spatial forces.

Returns#

cndarray
The computed joint forces.
fndarray
The updated spatial forces.

Example#

from your_module import RBDReference

q = np.array([1, 2, 3])
f = np.array([0, 0, 0, 0, 0, 0])
rbd_ref = RBDReference(robot_obj)
c, f = rbd_ref.rnea_bpass(q, f)
print(c, f)

rnea(self, q, qd, qdd=None, GRAVITY=-9.81, f_ext=None)#

Description#

Computes the Recursive Newton-Euler Algorithm (RNEA) by performing both the forward and backward passes.

Parameters#

qndarray
The joint positions of the robot.
qdndarray
The joint velocities.
qddndarray, optional
The joint accelerations.
GRAVITYfloat, optional
The gravitational constant. Default is -9.81.
f_extndarray, optional
The external forces to apply.

Returns#

cndarray
The computed joint forces.
vndarray
The spatial velocities.
andarray
The spatial accelerations.
fndarray
The spatial forces.

Example#

from your_module import RBDReference

q = np.array([1, 2, 3])
qd = np.array([0, 1, 0])
rbd_ref = RBDReference(robot_obj)
c, v, a, f = rbd_ref.rnea(q, qd)
print(c, v, a, f)

rnea_grad_fpass_dq(self, q, qd, v, a, GRAVITY=-9.81)#

Description#

Computes the forward pass gradient of the Recursive Newton-Euler Algorithm (RNEA) with respect to joint positions.

Parameters#

qndarray
The joint positions.
qdndarray
The joint velocities.
vndarray
The spatial velocities.
andarray
The spatial accelerations.
GRAVITYfloat, optional
The gravitational constant. Default is -9.81.

Returns#

dv_dqndarray
The derivative of spatial velocities with respect to joint positions.
da_dqndarray
The derivative of spatial accelerations with respect to joint positions.
df_dqndarray
The derivative of spatial forces with respect to joint positions.

Example#

from your_module import RBDReference

q = np.array([1, 2, 3])
qd = np.array([0, 1, 0])
v = np.array([0, 0, 0])
a = np.array([0, 0, 0])
rbd_ref = RBDReference(robot_obj)
dv_dq, da_dq, df_dq = rbd_ref.rnea_grad_fpass_dq(q, qd, v, a)
print(dv_dq, da_dq, df_dq)

rnea_grad_fpass_dqd(self, q, qd, v)#

Description#

Computes the forward pass gradient of the Recursive Newton-Euler Algorithm (RNEA) with respect to joint velocities.

Parameters#

qndarray
The joint positions.
qdndarray
The joint velocities.
vndarray
The spatial velocities.

Returns#

dv_dqdndarray
The gradient of spatial velocities with respect to joint velocities.
da_dqdndarray
The gradient of spatial accelerations with respect to joint velocities.
df_dqdndarray
The gradient of spatial forces with respect to joint velocities.

Example#

from your_module import RBDReference

q = np.array([1, 2, 3])
qd = np.array([0, 1, 0])
v = np.array([0, 0, 0])
rbd_ref = RBDReference(robot_obj)
dv_dqd, da_dqd, df_dqd = rbd_ref.rnea_grad_fpass_dqd(q, qd, v)
print(dv_dqd, da_dqd, df_dqd)

rnea_grad_bpass_dq(self, q, f, df_dq)#

Description#

Computes the backward pass gradient of the Recursive Newton-Euler Algorithm (RNEA) with respect to joint positions.

Parameters#

qndarray
The joint positions.
fndarray
The spatial forces.
df_dqndarray
The gradient of joint forces with respect to joint positions.

Returns#

dc_dqndarray
The gradient of RNEA with respect to joint positions.

Example#

from your_module import RBDReference

q = np.array([1, 2, 3])
f = np.array([0, 0, 0, 0, 0, 0])
df_dq = np.array([0, 0, 0])
rbd_ref = RBDReference(robot_obj)
dc_dq = rbd_ref.rnea_grad_bpass_dq(q, f,

Additional Resources#

For more information on how to use this package, please see: - Sphinx Documentation