-
Notifications
You must be signed in to change notification settings - Fork 49
/
spatial.html
45 lines (45 loc) · 36.6 KB
/
spatial.html
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
<!DOCTYPE html>
<html lang="en"><head><meta charset="UTF-8"/><meta name="viewport" content="width=device-width, initial-scale=1.0"/><title>Spatial vector algebra · RigidBodyDynamics.jl</title><link href="https://cdnjs.cloudflare.com/ajax/libs/normalize/4.2.0/normalize.min.css" rel="stylesheet" type="text/css"/><link href="https://fonts.googleapis.com/css?family=Lato|Roboto+Mono" rel="stylesheet" type="text/css"/><link href="https://cdnjs.cloudflare.com/ajax/libs/font-awesome/4.6.3/css/font-awesome.min.css" rel="stylesheet" type="text/css"/><link href="https://cdnjs.cloudflare.com/ajax/libs/highlight.js/9.12.0/styles/default.min.css" rel="stylesheet" type="text/css"/><script>documenterBaseURL="."</script><script src="https://cdnjs.cloudflare.com/ajax/libs/require.js/2.2.0/require.min.js" data-main="assets/documenter.js"></script><script src="siteinfo.js"></script><script src="../versions.js"></script><link href="assets/documenter.css" rel="stylesheet" type="text/css"/></head><body><nav class="toc"><h1>RigidBodyDynamics.jl</h1><select id="version-selector" onChange="window.location.href=this.value" style="visibility: hidden"></select><form class="search" id="search-form" action="search.html"><input id="search-query" name="q" type="text" placeholder="Search docs"/></form><ul><li><a class="toctext" href="index.html">Home</a></li><li><a class="toctext" href="quickstart.html">Quick start guide</a></li><li class="current"><a class="toctext" href="spatial.html">Spatial vector algebra</a><ul class="internal"><li><a class="toctext" href="#Index-1">Index</a></li><li><a class="toctext" href="#Types-1">Types</a></li><li><a class="toctext" href="#The-@framecheck-macro-1">The <code>@framecheck</code> macro</a></li><li><a class="toctext" href="#Functions-1">Functions</a></li></ul></li><li><a class="toctext" href="joints.html">Joints</a></li><li><a class="toctext" href="rigidbody.html">Rigid bodies</a></li><li><a class="toctext" href="mechanism.html">Mechanism</a></li><li><a class="toctext" href="mechanismstate.html">MechanismState</a></li><li><a class="toctext" href="algorithms.html">Kinematics/dynamics algorithms</a></li><li><a class="toctext" href="customcollections.html">Custom collection types</a></li><li><a class="toctext" href="caches.html">Cache types</a></li><li><a class="toctext" href="simulation.html">Simulation</a></li><li><a class="toctext" href="benchmarks.html">Benchmarks</a></li></ul></nav><article id="docs"><header><nav><ul><li><a href="spatial.html">Spatial vector algebra</a></li></ul><a class="edit-page" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/master/docs/src/spatial.md"><span class="fa"></span> Edit on GitHub</a></nav><hr/><div id="topbar"><span>Spatial vector algebra</span><a class="fa fa-bars" href="#"></a></div></header><h1><a class="nav-anchor" id="Spatial-vector-algebra-1" href="#Spatial-vector-algebra-1">Spatial vector algebra</a></h1><h2><a class="nav-anchor" id="Index-1" href="#Index-1">Index</a></h2><ul><li><a href="spatial.html#RigidBodyDynamics.Spatial.CartesianFrame3D"><code>RigidBodyDynamics.Spatial.CartesianFrame3D</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.FreeVector3D"><code>RigidBodyDynamics.Spatial.FreeVector3D</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.GeometricJacobian"><code>RigidBodyDynamics.Spatial.GeometricJacobian</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.Momentum"><code>RigidBodyDynamics.Spatial.Momentum</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.MomentumMatrix"><code>RigidBodyDynamics.Spatial.MomentumMatrix</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.Point3D"><code>RigidBodyDynamics.Spatial.Point3D</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.SpatialAcceleration"><code>RigidBodyDynamics.Spatial.SpatialAcceleration</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.SpatialInertia"><code>RigidBodyDynamics.Spatial.SpatialInertia</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.Transform3D"><code>RigidBodyDynamics.Spatial.Transform3D</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.Twist"><code>RigidBodyDynamics.Spatial.Twist</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.Wrench"><code>RigidBodyDynamics.Spatial.Wrench</code></a></li><li><a href="spatial.html#Base.LinAlg.dot-Tuple{RigidBodyDynamics.Spatial.Wrench,RigidBodyDynamics.Spatial.Twist}"><code>Base.LinAlg.dot</code></a></li><li><a href="spatial.html#Base.exp-Tuple{RigidBodyDynamics.Spatial.Twist}"><code>Base.exp</code></a></li><li><a href="spatial.html#Base.log-Tuple{RigidBodyDynamics.Spatial.Transform3D}"><code>Base.log</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.center_of_mass-Tuple{RigidBodyDynamics.Spatial.SpatialInertia}"><code>RigidBodyDynamics.Spatial.center_of_mass</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.colwise-Tuple{Any,AbstractArray{T,1} where T,AbstractArray{T,2} where T}"><code>RigidBodyDynamics.Spatial.colwise</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.colwise-Tuple{Any,StaticArrays.StaticArray,StaticArrays.StaticArray{Tuple{N},T,1} where T where N}"><code>RigidBodyDynamics.Spatial.colwise</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.colwise-Tuple{Any,StaticArrays.StaticArray{Tuple{N},T,1} where T where N,StaticArrays.StaticArray}"><code>RigidBodyDynamics.Spatial.colwise</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.kinetic_energy-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.Twist}"><code>RigidBodyDynamics.Spatial.kinetic_energy</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.log_with_time_derivative-Tuple{RigidBodyDynamics.Spatial.Transform3D,RigidBodyDynamics.Spatial.Twist}"><code>RigidBodyDynamics.Spatial.log_with_time_derivative</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.newton_euler-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.SpatialAcceleration,RigidBodyDynamics.Spatial.Twist}"><code>RigidBodyDynamics.Spatial.newton_euler</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.point_velocity-Tuple{RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Point3D}"><code>RigidBodyDynamics.Spatial.point_velocity</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.GeometricJacobian,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Momentum,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Wrench,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.SpatialAcceleration,RigidBodyDynamics.Spatial.Transform3D,RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Twist}"><code>RigidBodyDynamics.Spatial.transform</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.FreeVector3D,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Point3D,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a></li><li><a href="spatial.html#RigidBodyDynamics.Spatial.@framecheck"><code>RigidBodyDynamics.Spatial.@framecheck</code></a></li></ul><h2><a class="nav-anchor" id="Types-1" href="#Types-1">Types</a></h2><h3><a class="nav-anchor" id="Coordinate-frames-1" href="#Coordinate-frames-1">Coordinate frames</a></h3><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.CartesianFrame3D" href="#RigidBodyDynamics.Spatial.CartesianFrame3D"><code>RigidBodyDynamics.Spatial.CartesianFrame3D</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">bitstype 64 CartesianFrame3D</code></pre><p>A <code>CartesianFrame3D</code> identifies a three-dimensional Cartesian coordinate system.</p><p><code>CartesianFrame3D</code>s are typically used to annotate the frame in which certain quantities are expressed.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/frame.jl#L12">source</a></section><h3><a class="nav-anchor" id="Transforms-1" href="#Transforms-1">Transforms</a></h3><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.Transform3D" href="#RigidBodyDynamics.Spatial.Transform3D"><code>RigidBodyDynamics.Spatial.Transform3D</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct Transform3D{T}</code></pre><p>A homogeneous transformation matrix representing the transformation from one three-dimensional Cartesian coordinate system to another.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/transform3d.jl#L1">source</a></section><h3><a class="nav-anchor" id="Points,-free-vectors-1" href="#Points,-free-vectors-1">Points, free vectors</a></h3><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.Point3D" href="#RigidBodyDynamics.Spatial.Point3D"><code>RigidBodyDynamics.Spatial.Point3D</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct Point3D{V<:(AbstractArray{T,1} where T)}</code></pre><p>A <code>Point3D</code> represents a position in a given coordinate system.</p><p>A <code>Point3D</code> is a <a href="https://en.wikipedia.org/wiki/Euclidean_vector#Overview">bound vector</a>. Applying a <code>Transform3D</code> to a <code>Point3D</code> both rotates and translates the <code>Point3D</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/threevectors.jl#L45">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.FreeVector3D" href="#RigidBodyDynamics.Spatial.FreeVector3D"><code>RigidBodyDynamics.Spatial.FreeVector3D</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct FreeVector3D{V<:(AbstractArray{T,1} where T)}</code></pre><p>A <code>FreeVector3D</code> represents a <a href="https://en.wikipedia.org/wiki/Euclidean_vector#Overview">free vector</a>.</p><p>Examples of free vectors include displacements and velocities of points.</p><p>Applying a <code>Transform3D</code> to a <code>FreeVector3D</code> only rotates the <code>FreeVector3D</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/threevectors.jl#L56">source</a></section><h3><a class="nav-anchor" id="Inertias-1" href="#Inertias-1">Inertias</a></h3><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.SpatialInertia" href="#RigidBodyDynamics.Spatial.SpatialInertia"><code>RigidBodyDynamics.Spatial.SpatialInertia</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct SpatialInertia{T}</code></pre><p>A spatial inertia, or inertia matrix, represents the mass distribution of a rigid body.</p><p>A spatial inertia expressed in frame <span>$i$</span> is defined as:</p><div>\[I^i =
\int_{B}\rho\left(x\right)\left[\begin{array}{cc}
\hat{p}^{T}\left(x\right)\hat{p}\left(x\right) & \hat{p}\left(x\right)\\
\hat{p}^{T}\left(x\right) & I
\end{array}\right]dx=\left[\begin{array}{cc}
J & \hat{c}\\
\hat{c}^{T} & mI
\end{array}\right]\]</div><p>where <span>$\rho(x)$</span> is the density of point <span>$x$</span>, and <span>$p(x)$</span> are the coordinates of point <span>$x$</span> expressed in frame <span>$i$</span>. <span>$J$</span> is the mass moment of inertia, <span>$m$</span> is the total mass, and <span>$c$</span> is the 'cross part', center of mass position scaled by <span>$m$</span>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/motion_force_interaction.jl#L1">source</a></section><h3><a class="nav-anchor" id="Twists,-spatial-accelerations-1" href="#Twists,-spatial-accelerations-1">Twists, spatial accelerations</a></h3><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.Twist" href="#RigidBodyDynamics.Spatial.Twist"><code>RigidBodyDynamics.Spatial.Twist</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct Twist{T}</code></pre><p>A twist represents the relative angular and linear motion between two bodies.</p><p>The twist of frame <span>$j$</span> with respect to frame <span>$i$</span>, expressed in frame <span>$k$</span> is defined as</p><div>\[T_{j}^{k,i}=\left(\begin{array}{c}
\omega_{j}^{k,i}\\
v_{j}^{k,i}
\end{array}\right)\in\mathbb{R}^{6}\]</div><p>such that</p><div>\[\left[\begin{array}{cc}
\hat{\omega}_{j}^{k,i} & v_{j}^{k,i}\\
0 & 0
\end{array}\right]=H_{i}^{k}\dot{H}_{j}^{i}H_{k}^{j}\]</div><p>where <span>$H^{\beta}_{\alpha}$</span> is the homogeneous transform from frame <span>$\alpha$</span> to frame <span>$\beta$</span>, and <span>$\hat{x}$</span> is the <span>$3 \times 3$</span> skew symmetric matrix that satisfies <span>$\hat{x} y = x \times y$</span> for all <span>$y \in \mathbb{R}^3$</span>.</p><p>Here, <span>$\omega_{j}^{k,i}$</span> is the angular part and <span>$v_{j}^{k,i}$</span> is the linear part. Note that the linear part is not in general the same as the linear velocity of the origin of frame <span>$j$</span>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L69">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.SpatialAcceleration" href="#RigidBodyDynamics.Spatial.SpatialAcceleration"><code>RigidBodyDynamics.Spatial.SpatialAcceleration</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct SpatialAcceleration{T}</code></pre><p>A spatial acceleration is the time derivative of a twist.</p><p>See <a href="spatial.html#RigidBodyDynamics.Spatial.Twist"><code>Twist</code></a>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L106">source</a></section><h3><a class="nav-anchor" id="Momenta,-wrenches-1" href="#Momenta,-wrenches-1">Momenta, wrenches</a></h3><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.Momentum" href="#RigidBodyDynamics.Spatial.Momentum"><code>RigidBodyDynamics.Spatial.Momentum</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct Momentum{T}</code></pre><p>A <code>Momentum</code> is the product of a <code>SpatialInertia</code> and a <code>Twist</code>, i.e.</p><div>\[h^i =
\left(\begin{array}{c}
k^{i}\\
l^{i}
\end{array}\right) =
I^i T^{i, j}_k\]</div><p>where <span>$I^i$</span> is the spatial inertia of a given body expressed in frame <span>$i$</span>, and <span>$T^{i, j}_k$</span> is the twist of frame <span>$k$</span> (attached to the body) with respect to inertial frame <span>$j$</span>, expressed in frame <span>$i$</span>. <span>$k^i$</span> is the angular momentum and <span>$l^i$</span> is the linear momentum.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialforce.jl#L56">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.Wrench" href="#RigidBodyDynamics.Spatial.Wrench"><code>RigidBodyDynamics.Spatial.Wrench</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct Wrench{T}</code></pre><p>A wrench represents a system of forces.</p><p>The wrench <span>$w^i$</span> expressed in frame <span>$i$</span> is defined as</p><div>\[w^{i} =
\left(\begin{array}{c}
\tau^{i}\\
f^{i}
\end{array}\right) =
\sum_{j}\left(\begin{array}{c}
r_{j}^{i}\times f_{j}^{i}\\
f_{j}^{i}
\end{array}\right)\]</div><p>where the <span>$f_{j}^{i}$</span> are forces expressed in frame <span>$i$</span>, exerted at positions <span>$r_{j}^{i}$</span>. <span>$\tau^i$</span> is the total torque and <span>$f^i$</span> is the total force.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialforce.jl#L79">source</a></section><h3><a class="nav-anchor" id="Geometric-Jacobians-1" href="#Geometric-Jacobians-1">Geometric Jacobians</a></h3><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.GeometricJacobian" href="#RigidBodyDynamics.Spatial.GeometricJacobian"><code>RigidBodyDynamics.Spatial.GeometricJacobian</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct GeometricJacobian{A<:(AbstractArray{T,2} where T)}</code></pre><p>A geometric Jacobian (also known as basic, or spatial Jacobian) maps a vector of joint velocities to a twist.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L1">source</a></section><h3><a class="nav-anchor" id="Momentum-matrices-1" href="#Momentum-matrices-1">Momentum matrices</a></h3><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.MomentumMatrix" href="#RigidBodyDynamics.Spatial.MomentumMatrix"><code>RigidBodyDynamics.Spatial.MomentumMatrix</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">struct MomentumMatrix{A<:(AbstractArray{T,2} where T)}</code></pre><p>A momentum matrix maps a joint velocity vector to momentum.</p><p>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.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialforce.jl#L16">source</a></section><h2><a class="nav-anchor" id="The-@framecheck-macro-1" href="#The-@framecheck-macro-1">The <code>@framecheck</code> macro</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.@framecheck" href="#RigidBodyDynamics.Spatial.@framecheck"><code>RigidBodyDynamics.Spatial.@framecheck</code></a> — <span class="docstring-category">Macro</span>.</div><div><pre><code class="language-julia">@framecheck(f1, f2s)
</code></pre><p>Check that <code>CartesianFrame3D</code> <code>f1</code> is one of <code>f2s</code>.</p><p>Note that if <code>f2s</code> is a <code>CartesianFrame3D</code>, then <code>f1</code> and <code>f2s</code> are simply checked for equality.</p><p>Throws an <code>ArgumentError</code> if <code>f1</code> is not among <code>f2s</code> when bounds checks are enabled. <code>@framecheck</code> is a no-op when bounds checks are disabled.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/frame.jl#L53">source</a></section><h2><a class="nav-anchor" id="Functions-1" href="#Functions-1">Functions</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.center_of_mass-Tuple{RigidBodyDynamics.Spatial.SpatialInertia}" href="#RigidBodyDynamics.Spatial.center_of_mass-Tuple{RigidBodyDynamics.Spatial.SpatialInertia}"><code>RigidBodyDynamics.Spatial.center_of_mass</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">center_of_mass(inertia)
</code></pre><p>Return the center of mass of the <code>SpatialInertia</code> as a <a href="spatial.html#RigidBodyDynamics.Spatial.Point3D"><code>Point3D</code></a>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/motion_force_interaction.jl#L52">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.kinetic_energy-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.Twist}" href="#RigidBodyDynamics.Spatial.kinetic_energy-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.Twist}"><code>RigidBodyDynamics.Spatial.kinetic_energy</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">kinetic_energy(inertia, twist)
</code></pre><p>Compute the kinetic energy of a body with spatial inertia <span>$I$</span>, which has twist <span>$T$</span> with respect to an inertial frame.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/motion_force_interaction.jl#L226">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.log_with_time_derivative-Tuple{RigidBodyDynamics.Spatial.Transform3D,RigidBodyDynamics.Spatial.Twist}" href="#RigidBodyDynamics.Spatial.log_with_time_derivative-Tuple{RigidBodyDynamics.Spatial.Transform3D,RigidBodyDynamics.Spatial.Twist}"><code>RigidBodyDynamics.Spatial.log_with_time_derivative</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">log_with_time_derivative(t, twist)
</code></pre><p>Compute exponential coordinates as well as their time derivatives in one shot. This mainly exists because ForwardDiff won't work at the singularity of <code>log</code>. It is also ~50% faster than ForwardDiff in this case.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L230">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.newton_euler-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.SpatialAcceleration,RigidBodyDynamics.Spatial.Twist}" href="#RigidBodyDynamics.Spatial.newton_euler-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.SpatialAcceleration,RigidBodyDynamics.Spatial.Twist}"><code>RigidBodyDynamics.Spatial.newton_euler</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">newton_euler(inertia, spatial_accel, twist)
</code></pre><p>Apply the Newton-Euler equations to find the external wrench required to make a body with spatial inertia <span>$I$</span>, which has twist <span>$T$</span> with respect to an inertial frame, achieve spatial acceleration <span>$\dot{T}$</span>.</p><p>This wrench is also equal to the rate of change of momentum of the body.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/motion_force_interaction.jl#L168">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.point_velocity-Tuple{RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Point3D}" href="#RigidBodyDynamics.Spatial.point_velocity-Tuple{RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Point3D}"><code>RigidBodyDynamics.Spatial.point_velocity</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">point_velocity(twist, point)
</code></pre><p>Given a twist <span>$T_{j}^{k,i}$</span> of frame <span>$j$</span> with respect to frame <span>$i$</span>, expressed in frame <span>$k$</span>, and the location of a point fixed in frame <span>$j$</span>, also expressed in frame <span>$k$</span>, compute the velocity of the point relative to frame <span>$i$</span>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L305">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.FreeVector3D,RigidBodyDynamics.Spatial.Transform3D}" href="#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.FreeVector3D,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">transform(x, t)
</code></pre><p>Return <code>x</code> transformed to <code>CartesianFrame3D</code> <code>t.from</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/threevectors.jl#L3">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.GeometricJacobian,RigidBodyDynamics.Spatial.Transform3D}" href="#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.GeometricJacobian,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">transform(jac, tf)
</code></pre><p>Transform the <code>GeometricJacobian</code> to a different frame.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L44">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Momentum,RigidBodyDynamics.Spatial.Transform3D}" href="#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Momentum,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">transform(f, tf)
</code></pre><p>Transform the Momentum to a different frame.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialforce.jl#L106">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Point3D,RigidBodyDynamics.Spatial.Transform3D}" href="#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Point3D,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">transform(x, t)
</code></pre><p>Return <code>x</code> transformed to <code>CartesianFrame3D</code> <code>t.from</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/threevectors.jl#L3">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.SpatialAcceleration,RigidBodyDynamics.Spatial.Transform3D,RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Twist}" href="#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.SpatialAcceleration,RigidBodyDynamics.Spatial.Transform3D,RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Twist}"><code>RigidBodyDynamics.Spatial.transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">transform(accel, old_to_new, twist_of_current_wrt_new, twist_of_body_wrt_base)
</code></pre><p>Transform the <code>SpatialAcceleration</code> to a different frame.</p><p>The transformation rule is obtained by differentiating the transformation rule for twists.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L319">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.Transform3D}" href="#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.SpatialInertia,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">transform(inertia, t)
</code></pre><p>Transform the <code>SpatialInertia</code> to a different frame.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/motion_force_interaction.jl#L81">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Transform3D}" href="#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Twist,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">transform(twist, tf)
</code></pre><p>Transform the <code>Twist</code> to a different frame.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L179">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Wrench,RigidBodyDynamics.Spatial.Transform3D}" href="#RigidBodyDynamics.Spatial.transform-Tuple{RigidBodyDynamics.Spatial.Wrench,RigidBodyDynamics.Spatial.Transform3D}"><code>RigidBodyDynamics.Spatial.transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">transform(f, tf)
</code></pre><p>Transform the Wrench to a different frame.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialforce.jl#L106">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="Base.LinAlg.dot-Tuple{RigidBodyDynamics.Spatial.Wrench,RigidBodyDynamics.Spatial.Twist}" href="#Base.LinAlg.dot-Tuple{RigidBodyDynamics.Spatial.Wrench,RigidBodyDynamics.Spatial.Twist}"><code>Base.LinAlg.dot</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">dot(w, t)
</code></pre><p>Compute the mechanical power associated with a pairing of a wrench and a twist.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/motion_force_interaction.jl#L132">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="Base.exp-Tuple{RigidBodyDynamics.Spatial.Twist}" href="#Base.exp-Tuple{RigidBodyDynamics.Spatial.Twist}"><code>Base.exp</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">exp(twist)
</code></pre><p>Convert exponential coordinates to a homogeneous transform.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L272">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="Base.log-Tuple{RigidBodyDynamics.Spatial.Transform3D}" href="#Base.log-Tuple{RigidBodyDynamics.Spatial.Transform3D}"><code>Base.log</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">log(t)
</code></pre><p>Express a homogeneous transform in exponential coordinates centered around the identity.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/spatialmotion.jl#L219">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.colwise-Tuple{Any,AbstractArray{T,1} where T,AbstractArray{T,2} where T}" href="#RigidBodyDynamics.Spatial.colwise-Tuple{Any,AbstractArray{T,1} where T,AbstractArray{T,2} where T}"><code>RigidBodyDynamics.Spatial.colwise</code></a> — <span class="docstring-category">Method</span>.</div><div><p>colwise(f, vec, mat) Return a matrix <code>A</code> such that <code>A[:, i] == f(vec, mat[:, i])</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/util.jl#L24-L27">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.colwise-Tuple{Any,StaticArrays.StaticArray,StaticArrays.StaticArray{Tuple{N},T,1} where T where N}" href="#RigidBodyDynamics.Spatial.colwise-Tuple{Any,StaticArrays.StaticArray,StaticArrays.StaticArray{Tuple{N},T,1} where T where N}"><code>RigidBodyDynamics.Spatial.colwise</code></a> — <span class="docstring-category">Method</span>.</div><div><p>colwise(f, mat, vec) Return a matrix <code>A</code> such that <code>A[:, i] == f(mat[:, i], vec)</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/util.jl#L32-L35">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.colwise-Tuple{Any,StaticArrays.StaticArray{Tuple{N},T,1} where T where N,StaticArrays.StaticArray}" href="#RigidBodyDynamics.Spatial.colwise-Tuple{Any,StaticArrays.StaticArray{Tuple{N},T,1} where T where N,StaticArrays.StaticArray}"><code>RigidBodyDynamics.Spatial.colwise</code></a> — <span class="docstring-category">Method</span>.</div><div><p>colwise(f, vec, mat) Return a matrix <code>A</code> such that <code>A[:, i] == f(vec, mat[:, i])</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/spatial/util.jl#L3-L6">source</a></section><footer><hr/><a class="previous" href="quickstart.html"><span class="direction">Previous</span><span class="title">Quick start guide</span></a><a class="next" href="joints.html"><span class="direction">Next</span><span class="title">Joints</span></a></footer></article></body></html>