Spatial vector algebra

bitstype 64 CartesianFrame3D

A CartesianFrame3D identifies a three-dimensional Cartesian coordinate system.

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

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

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

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.

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

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

struct SpatialAcceleration{T}

A spatial acceleration is the time derivative of a twist.

See Twist.

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.

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.

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.

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.

@framecheck(f1, f2)

Check that CartesianFrame3Ds f1 and f2 are identical (when bounds checks are enabled).

Throws an ArgumentError if f1 is not identical to f2 when bounds checks are enabled. @framecheck is a no-op when bounds checks are disabled.

center_of_mass(inertia)

Return the center of mass of the SpatialInertia as a Point3D.

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.

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.

num_cols(jac)

Return the number of columns of the GeometricJacobian.

point_velocity(twist, point)

Given a 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$.

transform(jac, transform)

Transform the GeometricJacobian to a different frame.

transform(f, transform)

Transform the Momentum to a different frame.

transform(accel, oldToNew, twistOfCurrentWrtNew, twistOfBodyWrtBase)

Transform the SpatialAcceleration to a different frame.

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

transform(inertia, t)

Transform the SpatialInertia to a different frame.

transform(twist, transform)

Transform the Twist to a different frame.

transform(f, transform)

Transform the Wrench to a different frame.

invtransform(x, t)

Apply the inverse transform, i.e. return x, originally expressed in CartesianFrame3D t.from, transformed to t.to.

invtransform(x, t)

Apply the inverse transform, i.e. return x, originally expressed in CartesianFrame3D t.from, transformed to t.to.

transform(x, t)

Return x transformed to CartesianFrame3D t.from.

transform(x, t)

Return x transformed to CartesianFrame3D t.from.

dot(w, t)

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

exp(twist)

Convert exponential coordinates to a homogeneous transform.

log(t)

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

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.