# Algorithms

## The DynamicsResult type

RigidBodyDynamics.DynamicsResultType
mutable struct DynamicsResult{T, M}

Stores variables related to the dynamics of a Mechanism, e.g. the Mechanism's mass matrix and joint acceleration vector.

Type parameters:

• T: the scalar type of the dynamics-related variables.
• M: the scalar type of the Mechanism.
source

## Functions

RigidBodyDynamics._point_jacobian!Method
_point_jacobian!(Jp, state, path, point, transformfun)


Compute the Jacobian mapping the Mechanism's joint velocity vector $v$ to the velocity of a point fixed to the target of the joint path (the body succeeding the last joint in the path) with respect to the source of the joint path (the body preceding the first joint in the path).

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.dynamics!Method
dynamics!(result, state)
dynamics!(result, state, torques)
dynamics!(result, state, torques, externalwrenches; stabilization_gains)


Compute the joint acceleration vector $\dot{v}$ and Lagrange multipliers $\lambda$ that satisfy the joint-space equations of motion

$$$M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau - K(q)^{T} \lambda$$$

and the constraint equations

$$$K(q) \dot{v} = -k$$$

given joint configuration vector $q$, joint velocity vector $v$, and (optionally) joint torques $\tau$ and external wrenches $w_\text{ext}$.

The externalwrenches argument can be used to specify additional wrenches that act on the Mechanism's bodies.

The stabilization_gains keyword argument can be used to set PD gains for Baumgarte stabilization, which can be used to prevent separation of non-tree (loop) joints. See Featherstone (2008), section 8.3 for more information. There are several options for specifying gains:

• nothing can be used to completely disable Baumgarte stabilization.
• Gains can be specifed on a per-joint basis using any AbstractDict{JointID, <:RigidBodyDynamics.PDControl.SE3PDGains}, which maps the JointID for the non-tree joints of the mechanism to the gains for that joint.
• As a special case of the second option, the same gains can be used for all joints by passing in a RigidBodyDynamics.CustomCollections.ConstDict{JointID}.

The default_constraint_stabilization_gains function is called to produce the default gains, which use the last option.

source
RigidBodyDynamics.dynamics!Method
dynamics!(ẋ, result, state, x)
dynamics!(ẋ, result, state, x, torques)
dynamics!(ẋ, result, state, x, torques, externalwrenches; stabilization_gains)


Convenience function for use with standard ODE integrators that takes a Vector argument

$$$x = \left(\begin{array}{c} q\\ v \end{array}\right)$$$

and returns a Vector $\dot{x}$.

source
RigidBodyDynamics.dynamics_bias!Method
dynamics_bias!(torques, biasaccelerations, wrenches, state)
dynamics_bias!(torques, biasaccelerations, wrenches, state, externalwrenches)


Compute the 'dynamics bias term', i.e. the term

$$$c(q, v, w_\text{ext})$$$

in the unconstrained joint-space equations of motion

$$$M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau$$$

given joint configuration vector $q$, joint velocity vector $v$, joint acceleration vector $\dot{v}$ and (optionally) external wrenches $w_\text{ext}$.

The externalwrenches argument can be used to specify additional wrenches that act on the Mechanism's bodies.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.dynamics_biasMethod
dynamics_bias(state)
dynamics_bias(state, externalwrenches)


Compute the 'dynamics bias term', i.e. the term

$$$c(q, v, w_\text{ext})$$$

in the unconstrained joint-space equations of motion

$$$M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau$$$

given joint configuration vector $q$, joint velocity vector $v$, joint acceleration vector $\dot{v}$ and (optionally) external wrenches $w_\text{ext}$.

The externalwrenches argument can be used to specify additional wrenches that act on the Mechanism's bodies.

source
RigidBodyDynamics.geometric_jacobian!Method
geometric_jacobian!(jac, state, path, transformfun)


Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the Mechanism's spanning tree, (a collection of Joints and traversal directions) in the given state.

A geometric Jacobian maps the Mechanism's joint velocity vector $v$ to the twist of the target of the joint path (the body succeeding the last joint in the path) with respect to the source of the joint path (the body preceding the first joint in the path).

transformfun is a callable that may be used to transform the individual motion subspaces of each of the joints to the frame in which out is expressed.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.geometric_jacobian!Method
geometric_jacobian!(out, state, path, root_to_desired)


Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the Mechanism's spanning tree, (a collection of Joints and traversal directions) in the given state.

A geometric Jacobian maps the Mechanism's joint velocity vector $v$ to the twist of the target of the joint path (the body succeeding the last joint in the path) with respect to the source of the joint path (the body preceding the first joint in the path).

root_to_desired is the transform from the Mechanism's root frame to the frame in which out is expressed.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.geometric_jacobian!Method
geometric_jacobian!(out, state, path)


Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the Mechanism's spanning tree, (a collection of Joints and traversal directions) in the given state.

A geometric Jacobian maps the Mechanism's joint velocity vector $v$ to the twist of the target of the joint path (the body succeeding the last joint in the path) with respect to the source of the joint path (the body preceding the first joint in the path).

See geometric_jacobian!(out, state, path, root_to_desired). Uses state to compute the transform from the Mechanism's root frame to the frame in which out is expressed.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.geometric_jacobianMethod
geometric_jacobian(state, path)


Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the Mechanism's spanning tree, (a collection of Joints and traversal directions) in the given state.

A geometric Jacobian maps the Mechanism's joint velocity vector $v$ to the twist of the target of the joint path (the body succeeding the last joint in the path) with respect to the source of the joint path (the body preceding the first joint in the path).

The Jacobian is computed in the Mechanism's root frame.

source
RigidBodyDynamics.inverse_dynamics!Method
inverse_dynamics!(torquesout, jointwrenchesout, accelerations, state, v̇)
inverse_dynamics!(torquesout, jointwrenchesout, accelerations, state, v̇, externalwrenches)


Do inverse dynamics, i.e. compute $\tau$ in the unconstrained joint-space equations of motion

$$$M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau$$$

given joint configuration vector $q$, joint velocity vector $v$, joint acceleration vector $\dot{v}$ and (optionally) external wrenches $w_\text{ext}$.

The externalwrenches argument can be used to specify additional wrenches that act on the Mechanism's bodies.

This method implements the recursive Newton-Euler algorithm.

Currently doesn't support Mechanisms with cycles.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.inverse_dynamicsMethod
inverse_dynamics(state, v̇)
inverse_dynamics(state, v̇, externalwrenches)


Do inverse dynamics, i.e. compute $\tau$ in the unconstrained joint-space equations of motion

$$$M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau$$$

given joint configuration vector $q$, joint velocity vector $v$, joint acceleration vector $\dot{v}$ and (optionally) external wrenches $w_\text{ext}$.

The externalwrenches argument can be used to specify additional wrenches that act on the Mechanism's bodies.

This method implements the recursive Newton-Euler algorithm.

Currently doesn't support Mechanisms with cycles.

source
RigidBodyDynamics.mass_matrix!Method
mass_matrix!(M, state)


Compute the joint-space mass matrix (also known as the inertia matrix) of the Mechanism in the given state, i.e., the matrix $M(q)$ in the unconstrained joint-space equations of motion

$$$M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau$$$

This method implements the composite rigid body algorithm.

This method does its computation in place, performing no dynamic memory allocation.

The out argument must be an $n_v \times n_v$ lower triangular Symmetric matrix, where $n_v$ is the dimension of the Mechanism's joint velocity vector $v$.

source
RigidBodyDynamics.mass_matrixMethod

Compute the joint-space mass matrix (also known as the inertia matrix) of the Mechanism in the given state, i.e., the matrix $M(q)$ in the unconstrained joint-space equations of motion

$$$M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau$$$

This method implements the composite rigid body algorithm.

source
RigidBodyDynamics.momentum_matrix!Method
momentum_matrix!(mat, state, transformfun)


Compute the momentum matrix $A(q)$ of the Mechanism in the given state.

The momentum matrix maps the Mechanism's joint velocity vector $v$ to its total momentum.

See also MomentumMatrix.

The out argument must be a mutable MomentumMatrix with as many columns as the dimension of the Mechanism's joint velocity vector $v$.

transformfun is a callable that may be used to transform the individual momentum matrix blocks associated with each of the joints to the frame in which out is expressed.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.momentum_matrix!Method
momentum_matrix!(mat, state, root_to_desired)


Compute the momentum matrix $A(q)$ of the Mechanism in the given state.

The momentum matrix maps the Mechanism's joint velocity vector $v$ to its total momentum.

See also MomentumMatrix.

The out argument must be a mutable MomentumMatrix with as many columns as the dimension of the Mechanism's joint velocity vector $v$.

root_to_desired is the transform from the Mechanism's root frame to the frame in which out is expressed.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.momentum_matrix!Method
momentum_matrix!(out, state)


Compute the momentum matrix $A(q)$ of the Mechanism in the given state.

The momentum matrix maps the Mechanism's joint velocity vector $v$ to its total momentum.

See also MomentumMatrix.

The out argument must be a mutable MomentumMatrix with as many columns as the dimension of the Mechanism's joint velocity vector $v$.

See momentum_matrix!(out, state, root_to_desired). Uses state to compute the transform from the Mechanism's root frame to the frame in which out is expressed.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.point_jacobian!Method
point_jacobian!(out, state, path, point)


Compute the Jacobian mapping the Mechanism's joint velocity vector $v$ to the velocity of a point fixed to the target of the joint path (the body succeeding the last joint in the path) with respect to the source of the joint path (the body preceding the first joint in the path).

Uses state to compute the transform from the Mechanism's root frame to the frame in which out is expressed if necessary.

This method does its computation in place, performing no dynamic memory allocation.

source
RigidBodyDynamics.point_jacobianMethod
point_jacobian(state, path, point)


Compute the Jacobian mapping the Mechanism's joint velocity vector $v$ to the velocity of a point fixed to the target of the joint path (the body succeeding the last joint in the path) with respect to the source of the joint path (the body preceding the first joint in the path).

source
RigidBodyDynamics.subtree_massMethod
subtree_mass(base, mechanism)


Return the mass of a subtree of a Mechanism, rooted at base (including the mass of base).

source