# Spatial vector algebra

## Index

`RigidBodyDynamics.Spatial.CartesianFrame3D`

`RigidBodyDynamics.Spatial.CartesianFrame3D`

`RigidBodyDynamics.Spatial.CartesianFrame3D`

`RigidBodyDynamics.Spatial.FreeVector3D`

`RigidBodyDynamics.Spatial.GeometricJacobian`

`RigidBodyDynamics.Spatial.Momentum`

`RigidBodyDynamics.Spatial.MomentumMatrix`

`RigidBodyDynamics.Spatial.Point3D`

`RigidBodyDynamics.Spatial.SpatialAcceleration`

`RigidBodyDynamics.Spatial.SpatialInertia`

`RigidBodyDynamics.Spatial.Transform3D`

`RigidBodyDynamics.Spatial.Twist`

`RigidBodyDynamics.Spatial.Wrench`

`Base.exp`

`Base.log`

`LinearAlgebra.dot`

`RigidBodyDynamics.Spatial.center_of_mass`

`RigidBodyDynamics.Spatial.colwise`

`RigidBodyDynamics.Spatial.kinetic_energy`

`RigidBodyDynamics.Spatial.log_with_time_derivative`

`RigidBodyDynamics.Spatial.newton_euler`

`RigidBodyDynamics.Spatial.point_acceleration`

`RigidBodyDynamics.Spatial.point_velocity`

`RigidBodyDynamics.Spatial.transform`

`RigidBodyDynamics.Spatial.transform`

`RigidBodyDynamics.Spatial.transform`

`RigidBodyDynamics.Spatial.transform`

`RigidBodyDynamics.Spatial.transform`

`RigidBodyDynamics.Spatial.transform`

`RigidBodyDynamics.Spatial.transform`

`RigidBodyDynamics.Spatial.transform`

`RigidBodyDynamics.Spatial.@framecheck`

## Types

### Coordinate frames

`struct CartesianFrame3D`

A `CartesianFrame3D`

identifies a three-dimensional Cartesian coordinate system.

`CartesianFrame3D`

s are typically used to annotate the frame in which certain quantities are expressed.

```
CartesianFrame3D(name)
```

Create a `CartesianFrame3D`

with the given name.

```
CartesianFrame3D()
```

Create an anonymous `CartesianFrame3D`

.

### Transforms

`struct Transform3D{T}`

A homogeneous transformation matrix representing the transformation from one three-dimensional Cartesian coordinate system to another.

### Points, free vectors

`struct Point3D{V<:(AbstractArray{T,1} where T)}`

A `Point3D`

represents a position in a given coordinate system.

A `Point3D`

is a bound vector. Applying a `Transform3D`

to a `Point3D`

both rotates and translates the `Point3D`

.

`struct FreeVector3D{V<:(AbstractArray{T,1} where T)}`

A `FreeVector3D`

represents a free vector.

Examples of free vectors include displacements and velocities of points.

Applying a `Transform3D`

to a `FreeVector3D`

only rotates the `FreeVector3D`

.

### Inertias

`struct SpatialInertia{T}`

A spatial inertia, or inertia matrix, represents the mass distribution of a rigid body.

A spatial inertia expressed in frame $i$ is defined as:

where $\rho(x)$ is the density of point $x$, and $p(x)$ are the coordinates of point $x$ expressed in frame $i$. $J$ is the mass moment of inertia, $m$ is the total mass, and $c$ is the 'cross part', center of mass position scaled by $m$.

The `moment`

field of a `SpatialInertia`

is the moment of inertia **about the origin of its frame**, not about the center of mass.

### Twists, spatial accelerations

`RigidBodyDynamics.Spatial.Twist`

— Type.`struct Twist{T}`

A twist represents the relative angular and linear motion between two bodies.

The twist of frame $j$ with respect to frame $i$, expressed in frame $k$ is defined as

such that

where $H^{\beta}_{\alpha}$ is the homogeneous transform from frame $\alpha$ to frame $\beta$, and $\hat{x}$ is the $3 \times 3$ skew symmetric matrix that satisfies $\hat{x} y = x \times y$ for all $y \in \mathbb{R}^3$.

Here, $\omega_{j}^{k,i}$ is the angular part and $v_{j}^{k,i}$ is the linear part. Note that the linear part is not in general the same as the linear velocity of the origin of frame $j$.

### Momenta, wrenches

`struct Momentum{T}`

A `Momentum`

is the product of a `SpatialInertia`

and a `Twist`

, i.e.

where $I^i$ is the spatial inertia of a given body expressed in frame $i$, and $T^{i, j}_k$ is the twist of frame $k$ (attached to the body) with respect to inertial frame $j$, expressed in frame $i$. $k^i$ is the angular momentum and $l^i$ is the linear momentum.

`RigidBodyDynamics.Spatial.Wrench`

— Type.`struct Wrench{T}`

A wrench represents a system of forces.

The wrench $w^i$ expressed in frame $i$ is defined as

where the $f_{j}^{i}$ are forces expressed in frame $i$, exerted at positions $r_{j}^{i}$. $\tau^i$ is the total torque and $f^i$ is the total force.

### Geometric Jacobians

`struct GeometricJacobian{A<:(AbstractArray{T,2} where T)}`

A geometric Jacobian (also known as basic, or spatial Jacobian) maps a vector of joint velocities to a twist.

### Momentum matrices

`struct MomentumMatrix{A<:(AbstractArray{T,2} where T)}`

A momentum matrix maps a joint velocity vector to momentum.

This is a slight generalization of the centroidal momentum matrix (Orin, Goswami, "Centroidal momentum matrix of a humanoid robot: Structure and properties.") in that the matrix (and hence the corresponding total momentum) need not be expressed in a centroidal frame.

## The `@framecheck`

macro

Check that `CartesianFrame3D`

`f1`

is one of `f2s`

.

Note that if `f2s`

is a `CartesianFrame3D`

, then `f1`

and `f2s`

are simply checked for equality.

Throws an `ArgumentError`

if `f1`

is not among `f2s`

when bounds checks are enabled. `@framecheck`

is a no-op when bounds checks are disabled.

## Functions

`RigidBodyDynamics.Spatial.center_of_mass`

— Method.```
center_of_mass(inertia)
```

Return the center of mass of the `SpatialInertia`

as a `Point3D`

.

`RigidBodyDynamics.Spatial.kinetic_energy`

— Method.```
kinetic_energy(inertia, twist)
```

Compute the kinetic energy of a body with spatial inertia $I$, which has twist $T$ with respect to an inertial frame.

```
log_with_time_derivative(t, twist)
```

Compute exponential coordinates as well as their time derivatives in one shot. This mainly exists because ForwardDiff won't work at the singularity of `log`

. It is also ~50% faster than ForwardDiff in this case.

`RigidBodyDynamics.Spatial.newton_euler`

— Method.```
newton_euler(inertia, spatial_accel, twist)
```

Apply the Newton-Euler equations to find the external wrench required to make a body with spatial inertia $I$, which has twist $T$ with respect to an inertial frame, achieve spatial acceleration $\dot{T}$.

This wrench is also equal to the rate of change of momentum of the body.

```
point_acceleration(twist, accel, point)
```

Given the twist $dot{T}_{j}^{k,i}$ of frame $j$ with respect to frame $i$, expressed in frame $k$ and its time derivative (a spatial acceleration), as well as the location of a point fixed in frame $j$, also expressed in frame $k$, compute the acceleration of the point relative to frame $i$.

`RigidBodyDynamics.Spatial.point_velocity`

— Method.```
point_velocity(twist, point)
```

Given the twist $T_{j}^{k,i}$ of frame $j$ with respect to frame $i$, expressed in frame $k$, and the location of a point fixed in frame $j$, also expressed in frame $k$, compute the velocity of the point relative to frame $i$.

`RigidBodyDynamics.Spatial.transform`

— Method.```
transform(x, t)
```

Return `x`

transformed to `CartesianFrame3D`

`t.from`

.

`RigidBodyDynamics.Spatial.transform`

— Method.```
transform(jac, tf)
```

Transform the `GeometricJacobian`

to a different frame.

`RigidBodyDynamics.Spatial.transform`

— Method.```
transform(f, tf)
```

Transform the Momentum to a different frame.

`RigidBodyDynamics.Spatial.transform`

— Method.```
transform(x, t)
```

Return `x`

transformed to `CartesianFrame3D`

`t.from`

.

`RigidBodyDynamics.Spatial.transform`

— Method.```
transform(accel, old_to_new, twist_of_current_wrt_new, twist_of_body_wrt_base)
```

Transform the `SpatialAcceleration`

to a different frame.

The transformation rule is obtained by differentiating the transformation rule for twists.

`RigidBodyDynamics.Spatial.transform`

— Method.```
transform(inertia, t)
```

Transform the `SpatialInertia`

to a different frame.

`RigidBodyDynamics.Spatial.transform`

— Method.```
transform(twist, tf)
```

Transform the `Twist`

to a different frame.

`RigidBodyDynamics.Spatial.transform`

— Method.```
transform(f, tf)
```

Transform the Wrench to a different frame.

`Base.exp`

— Method.```
exp(twist)
```

Convert exponential coordinates to a homogeneous transform.

`Base.log`

— Method.```
log(t)
```

Express a homogeneous transform in exponential coordinates centered around the identity.

`LinearAlgebra.dot`

— Method.```
dot(w, t)
```

Compute the mechanical power associated with a pairing of a wrench and a twist.

`RigidBodyDynamics.Spatial.colwise`

— Function.Equivalent to one of

```
mapslices(x -> f(a, x), B, dims=1)
mapslices(x -> f(x, b), A, dims=1)
```

but optimized for statically-sized matrices.