-
Notifications
You must be signed in to change notification settings - Fork 49
/
algorithms.html
35 lines (35 loc) · 62 KB
/
algorithms.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
<!DOCTYPE html>
<html lang="en"><head><meta charset="UTF-8"/><meta name="viewport" content="width=device-width, initial-scale=1.0"/><title>Kinematics/dynamics algorithms · 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><a class="toctext" href="spatial.html">Spatial vector algebra</a></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 class="current"><a class="toctext" href="algorithms.html">Kinematics/dynamics algorithms</a><ul class="internal"><li><a class="toctext" href="#Index-1">Index</a></li><li><a class="toctext" href="#The-DynamicsResult-type-1">The <code>DynamicsResult</code> type</a></li><li><a class="toctext" href="#Functions-1">Functions</a></li></ul></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="algorithms.html">Kinematics/dynamics algorithms</a></li></ul><a class="edit-page" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/master/docs/src/algorithms.md"><span class="fa"></span> Edit on GitHub</a></nav><hr/><div id="topbar"><span>Kinematics/dynamics algorithms</span><a class="fa fa-bars" href="#"></a></div></header><h1><a class="nav-anchor" id="Algorithms-1" href="#Algorithms-1">Algorithms</a></h1><h2><a class="nav-anchor" id="Index-1" href="#Index-1">Index</a></h2><ul><li><a href="algorithms.html#RigidBodyDynamics.DynamicsResult"><code>RigidBodyDynamics.DynamicsResult</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.Spatial.center_of_mass-Tuple{MechanismState,Any}"><code>RigidBodyDynamics.Spatial.center_of_mass</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.Spatial.center_of_mass-Tuple{MechanismState}"><code>RigidBodyDynamics.Spatial.center_of_mass</code></a></li><li><a href="algorithms.html#RigidBodyDynamics._point_jacobian!-Tuple{PointJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Point3D,Any}"><code>RigidBodyDynamics._point_jacobian!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.default_constraint_stabilization_gains-Union{Tuple{Type{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.default_constraint_stabilization_gains</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.dynamics!-Union{Tuple{X}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1}}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1},AbstractArray{T,1} where T}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1},AbstractArray{T,1} where T,AbstractDict{BodyID,#s16} where #s16<:Wrench}} where X"><code>RigidBodyDynamics.dynamics!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.dynamics!-Union{Tuple{X}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{T,1} where T}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{T,1} where T,AbstractDict{BodyID,#s15} where #s15<:Wrench}} where X"><code>RigidBodyDynamics.dynamics!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.dynamics_bias-Union{Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C}, Tuple{W}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,AbstractDict{BodyID,Wrench{W}}}} where W where M where X"><code>RigidBodyDynamics.dynamics_bias</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.dynamics_bias!-Union{Tuple{X}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s14} where #s14<:SpatialAcceleration,AbstractDict{BodyID,#s13} where #s13<:Wrench,MechanismState{X,M,C,JointCollection} where JointCollection where C where M}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s12} where #s12<:SpatialAcceleration,AbstractDict{BodyID,#s170} where #s170<:Wrench,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractDict{BodyID,#s171} where #s171<:Wrench}} where X"><code>RigidBodyDynamics.dynamics_bias!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.geometric_jacobian-Union{Tuple{C}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection,TreePath{RigidBody{M},Joint{M,JT} where JT<:JointType{M}}}} where C where M where X"><code>RigidBodyDynamics.geometric_jacobian</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath}"><code>RigidBodyDynamics.geometric_jacobian!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Any}"><code>RigidBodyDynamics.geometric_jacobian!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Transform3D}"><code>RigidBodyDynamics.geometric_jacobian!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.inverse_dynamics-Union{Tuple{W}, Tuple{V}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,SegmentedVector{JointID,V,KeyRange,P} where P<:AbstractArray{V,1} where KeyRange<:AbstractRange{JointID}}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,SegmentedVector{JointID,V,KeyRange,P} where P<:AbstractArray{V,1} where KeyRange<:AbstractRange{JointID},AbstractDict{BodyID,Wrench{W}}}} where W where V where M where X"><code>RigidBodyDynamics.inverse_dynamics</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.inverse_dynamics!-Union{Tuple{T}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,Wrench{T}},AbstractDict{BodyID,SpatialAcceleration{T}},MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,Wrench{T}},AbstractDict{BodyID,SpatialAcceleration{T}},MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s170} where #s170<:Wrench}} where T"><code>RigidBodyDynamics.inverse_dynamics!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.mass-Tuple{Mechanism}"><code>RigidBodyDynamics.mass</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.mass_matrix-Union{Tuple{MechanismState{X,M,C,JointCollection} where JointCollection}, Tuple{C}, Tuple{M}, Tuple{X}} where C where M where X"><code>RigidBodyDynamics.mass_matrix</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.mass_matrix!-Tuple{LinearAlgebra.Symmetric,MechanismState}"><code>RigidBodyDynamics.mass_matrix!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.momentum_matrix-Tuple{MechanismState}"><code>RigidBodyDynamics.momentum_matrix</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState,Transform3D}"><code>RigidBodyDynamics.momentum_matrix!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState}"><code>RigidBodyDynamics.momentum_matrix!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState,Any}"><code>RigidBodyDynamics.momentum_matrix!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.point_jacobian-Union{Tuple{C}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection,TreePath{RigidBody{M},Joint{M,JT} where JT<:JointType{M}},Point3D}} where C where M where X"><code>RigidBodyDynamics.point_jacobian</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.point_jacobian!-Tuple{PointJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Point3D}"><code>RigidBodyDynamics.point_jacobian!</code></a></li><li><a href="algorithms.html#RigidBodyDynamics.subtree_mass-Union{Tuple{T}, Tuple{RigidBody{T},Mechanism{T}}} where T"><code>RigidBodyDynamics.subtree_mass</code></a></li></ul><h2><a class="nav-anchor" id="The-DynamicsResult-type-1" href="#The-DynamicsResult-type-1">The <code>DynamicsResult</code> type</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.DynamicsResult" href="#RigidBodyDynamics.DynamicsResult"><code>RigidBodyDynamics.DynamicsResult</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">mutable struct DynamicsResult{T, M}</code></pre><p>Stores variables related to the dynamics of a <code>Mechanism</code>, e.g. the <code>Mechanism</code>'s mass matrix and joint acceleration vector.</p><p>Type parameters:</p><ul><li><code>T</code>: the scalar type of the dynamics-related variables.</li><li><code>M</code>: the scalar type of the <code>Mechanism</code>.</li></ul></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/dynamics_result.jl#L1">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{MechanismState,Any}" href="#RigidBodyDynamics.Spatial.center_of_mass-Tuple{MechanismState,Any}"><code>RigidBodyDynamics.Spatial.center_of_mass</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">center_of_mass(state, itr)
</code></pre><p>Compute the center of mass of an iterable subset of a <code>Mechanism</code>'s bodies in the given state. Ignores the root body of the mechanism.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L24">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Spatial.center_of_mass-Tuple{MechanismState}" href="#RigidBodyDynamics.Spatial.center_of_mass-Tuple{MechanismState}"><code>RigidBodyDynamics.Spatial.center_of_mass</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">center_of_mass(state)
</code></pre><p>Compute the center of mass of the whole <code>Mechanism</code> in the given state.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L51">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.dynamics!-Union{Tuple{X}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{T,1} where T}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{T,1} where T,AbstractDict{BodyID,#s15} where #s15<:Wrench}} where X" href="#RigidBodyDynamics.dynamics!-Union{Tuple{X}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{T,1} where T}, Tuple{DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{T,1} where T,AbstractDict{BodyID,#s15} where #s15<:Wrench}} where X"><code>RigidBodyDynamics.dynamics!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">dynamics!(result, state)
dynamics!(result, state, torques)
dynamics!(result, state, torques, externalwrenches; stabilization_gains)
</code></pre><p>Compute the joint acceleration vector <span>$\dot{v}$</span> and Lagrange multipliers <span>$\lambda$</span> that satisfy the joint-space equations of motion</p><div>\[M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau - K(q)^{T} \lambda\]</div><p>and the constraint equations</p><div>\[K(q) \dot{v} = -k\]</div><p>given joint configuration vector <span>$q$</span>, joint velocity vector <span>$v$</span>, and (optionally) joint torques <span>$\tau$</span> and external wrenches <span>$w_\text{ext}$</span>.</p><p>The <code>externalwrenches</code> argument can be used to specify additional wrenches that act on the <code>Mechanism</code>'s bodies.</p><p>The <code>stabilization_gains</code> 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:</p><ul><li><code>nothing</code> can be used to completely disable Baumgarte stabilization.</li><li>Gains can be specifed on a per-joint basis using any <code>AbstractDict{JointID, <:RigidBodyDynamics.PDControl.SE3PDGains}</code>, which maps the <code>JointID</code> for the non-tree joints of the mechanism to the gains for that joint.</li><li>As a special case of the second option, the same gains can be used for all joints by passing in a <code>RigidBodyDynamics.CustomCollections.ConstDict{JointID}</code>.</li></ul><p>The <a href="algorithms.html#RigidBodyDynamics.default_constraint_stabilization_gains-Union{Tuple{Type{T}}, Tuple{T}} where T"><code>default_constraint_stabilization_gains</code></a> function is called to produce the default gains, which use the last option.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L845-L859">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.dynamics!-Union{Tuple{X}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1}}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1},AbstractArray{T,1} where T}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1},AbstractArray{T,1} where T,AbstractDict{BodyID,#s16} where #s16<:Wrench}} where X" href="#RigidBodyDynamics.dynamics!-Union{Tuple{X}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1}}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1},AbstractArray{T,1} where T}, Tuple{Union{DenseArray{X,1}, ReinterpretArray{X,1,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, ReshapedArray{X,1,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray}, SubArray{X,1,A,I,L} where L where I<:Tuple{Vararg{Union{Int64, AbstractRange{Int64}, AbstractCartesianIndex},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, ReshapedArray{T,N,A,MI} where MI<:Tuple{Vararg{SignedMultiplicativeInverse{Int64},N} where N} where A<:Union{ReinterpretArray{T,N,S,A} where S where A<:Union{SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, SubArray{T,N,A,I,true} where I<:Tuple{AbstractUnitRange,Vararg{Any,N} where N} where A<:DenseArray where N where T, DenseArray} where N where T, DenseArray}},DynamicsResult,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractArray{X,1},AbstractArray{T,1} where T,AbstractDict{BodyID,#s16} where #s16<:Wrench}} where X"><code>RigidBodyDynamics.dynamics!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">dynamics!(ẋ, result, state, x)
dynamics!(ẋ, result, state, x, torques)
dynamics!(ẋ, result, state, x, torques, externalwrenches; stabilization_gains)
</code></pre><p>Convenience function for use with standard ODE integrators that takes a <code>Vector</code> argument</p><div>\[x = \left(\begin{array}{c}
q\\
v
\end{array}\right)\]</div><p>and returns a <code>Vector</code> <span>$\dot{x}$</span>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L887">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.dynamics_bias!-Union{Tuple{X}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s14} where #s14<:SpatialAcceleration,AbstractDict{BodyID,#s13} where #s13<:Wrench,MechanismState{X,M,C,JointCollection} where JointCollection where C where M}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s12} where #s12<:SpatialAcceleration,AbstractDict{BodyID,#s170} where #s170<:Wrench,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractDict{BodyID,#s171} where #s171<:Wrench}} where X" href="#RigidBodyDynamics.dynamics_bias!-Union{Tuple{X}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s14} where #s14<:SpatialAcceleration,AbstractDict{BodyID,#s13} where #s13<:Wrench,MechanismState{X,M,C,JointCollection} where JointCollection where C where M}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s12} where #s12<:SpatialAcceleration,AbstractDict{BodyID,#s170} where #s170<:Wrench,MechanismState{X,M,C,JointCollection} where JointCollection where C where M,AbstractDict{BodyID,#s171} where #s171<:Wrench}} where X"><code>RigidBodyDynamics.dynamics_bias!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">dynamics_bias!(torques, biasaccelerations, wrenches, state)
dynamics_bias!(torques, biasaccelerations, wrenches, state, externalwrenches)
</code></pre><p>Compute the 'dynamics bias term', i.e. the term</p><div>\[c(q, v, w_\text{ext})\]</div><p>in the unconstrained joint-space equations of motion</p><div>\[M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau\]</div><p>given joint configuration vector <span>$q$</span>, joint velocity vector <span>$v$</span>, joint acceleration vector <span>$\dot{v}$</span> and (optionally) external wrenches <span>$w_\text{ext}$</span>.</p><p>The <code>externalwrenches</code> argument can be used to specify additional wrenches that act on the <code>Mechanism</code>'s bodies.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L495-L510">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.dynamics_bias-Union{Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C}, Tuple{W}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,AbstractDict{BodyID,Wrench{W}}}} where W where M where X" href="#RigidBodyDynamics.dynamics_bias-Union{Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C}, Tuple{W}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,AbstractDict{BodyID,Wrench{W}}}} where W where M where X"><code>RigidBodyDynamics.dynamics_bias</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">dynamics_bias(state)
dynamics_bias(state, externalwrenches)
</code></pre><p>Compute the 'dynamics bias term', i.e. the term</p><div>\[c(q, v, w_\text{ext})\]</div><p>in the unconstrained joint-space equations of motion</p><div>\[M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau\]</div><p>given joint configuration vector <span>$q$</span>, joint velocity vector <span>$v$</span>, joint acceleration vector <span>$\dot{v}$</span> and (optionally) external wrenches <span>$w_\text{ext}$</span>.</p><p>The <code>externalwrenches</code> argument can be used to specify additional wrenches that act on the <code>Mechanism</code>'s bodies.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L518-L533">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Any}" href="#RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Any}"><code>RigidBodyDynamics.geometric_jacobian!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">geometric_jacobian!(jac, state, path, transformfun)
</code></pre><p>Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the <code>Mechanism</code>'s spanning tree, (a collection of <code>Joint</code>s and traversal directions) in the given state.</p><p>A geometric Jacobian maps the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> 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).</p><p>See also <a href="mechanism.html#RigidBodyDynamics.path-Tuple{Mechanism,RigidBody,RigidBody}"><code>path</code></a>, <a href="spatial.html#RigidBodyDynamics.Spatial.GeometricJacobian"><code>GeometricJacobian</code></a>, <a href="spatial.html#RigidBodyDynamics.Spatial.Twist"><code>Twist</code></a>.</p><p><code>transformfun</code> is a callable that may be used to transform the individual motion subspaces of each of the joints to the frame in which <code>out</code> is expressed.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L70-L81">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Transform3D}" href="#RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Transform3D}"><code>RigidBodyDynamics.geometric_jacobian!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">geometric_jacobian!(out, state, path, root_to_desired)
</code></pre><p>Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the <code>Mechanism</code>'s spanning tree, (a collection of <code>Joint</code>s and traversal directions) in the given state.</p><p>A geometric Jacobian maps the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> 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).</p><p>See also <a href="mechanism.html#RigidBodyDynamics.path-Tuple{Mechanism,RigidBody,RigidBody}"><code>path</code></a>, <a href="spatial.html#RigidBodyDynamics.Spatial.GeometricJacobian"><code>GeometricJacobian</code></a>, <a href="spatial.html#RigidBodyDynamics.Spatial.Twist"><code>Twist</code></a>.</p><p><code>root_to_desired</code> is the transform from the <code>Mechanism</code>'s root frame to the frame in which <code>out</code> is expressed.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L101-L112">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath}" href="#RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath}"><code>RigidBodyDynamics.geometric_jacobian!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">geometric_jacobian!(out, state, path)
</code></pre><p>Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the <code>Mechanism</code>'s spanning tree, (a collection of <code>Joint</code>s and traversal directions) in the given state.</p><p>A geometric Jacobian maps the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> 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).</p><p>See also <a href="mechanism.html#RigidBodyDynamics.path-Tuple{Mechanism,RigidBody,RigidBody}"><code>path</code></a>, <a href="spatial.html#RigidBodyDynamics.Spatial.GeometricJacobian"><code>GeometricJacobian</code></a>, <a href="spatial.html#RigidBodyDynamics.Spatial.Twist"><code>Twist</code></a>.</p><p>See <a href="algorithms.html#RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Any}"><code>geometric_jacobian!(out, state, path, root_to_desired)</code></a>. Uses <code>state</code> to compute the transform from the <code>Mechanism</code>'s root frame to the frame in which <code>out</code> is expressed.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L115-L126">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.geometric_jacobian-Union{Tuple{C}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection,TreePath{RigidBody{M},Joint{M,JT} where JT<:JointType{M}}}} where C where M where X" href="#RigidBodyDynamics.geometric_jacobian-Union{Tuple{C}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection,TreePath{RigidBody{M},Joint{M,JT} where JT<:JointType{M}}}} where C where M where X"><code>RigidBodyDynamics.geometric_jacobian</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">geometric_jacobian(state, path)
</code></pre><p>Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the <code>Mechanism</code>'s spanning tree, (a collection of <code>Joint</code>s and traversal directions) in the given state.</p><p>A geometric Jacobian maps the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> 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).</p><p>See also <a href="mechanism.html#RigidBodyDynamics.path-Tuple{Mechanism,RigidBody,RigidBody}"><code>path</code></a>, <a href="spatial.html#RigidBodyDynamics.Spatial.GeometricJacobian"><code>GeometricJacobian</code></a>, <a href="spatial.html#RigidBodyDynamics.Spatial.Twist"><code>Twist</code></a>.</p><p>The Jacobian is computed in the <code>Mechanism</code>'s root frame.</p><p>See <a href="algorithms.html#RigidBodyDynamics.geometric_jacobian!-Tuple{GeometricJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Any}"><code>geometric_jacobian!(out, state, path)</code></a>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L134-L145">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.inverse_dynamics!-Union{Tuple{T}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,Wrench{T}},AbstractDict{BodyID,SpatialAcceleration{T}},MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,Wrench{T}},AbstractDict{BodyID,SpatialAcceleration{T}},MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s170} where #s170<:Wrench}} where T" href="#RigidBodyDynamics.inverse_dynamics!-Union{Tuple{T}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,Wrench{T}},AbstractDict{BodyID,SpatialAcceleration{T}},MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}, Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,Wrench{T}},AbstractDict{BodyID,SpatialAcceleration{T}},MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,AbstractDict{BodyID,#s170} where #s170<:Wrench}} where T"><code>RigidBodyDynamics.inverse_dynamics!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">inverse_dynamics!(torquesout, jointwrenchesout, accelerations, state, v̇)
inverse_dynamics!(torquesout, jointwrenchesout, accelerations, state, v̇, externalwrenches)
</code></pre><p>Do inverse dynamics, i.e. compute <span>$\tau$</span> in the unconstrained joint-space equations of motion</p><div>\[M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau\]</div><p>given joint configuration vector <span>$q$</span>, joint velocity vector <span>$v$</span>, joint acceleration vector <span>$\dot{v}$</span> and (optionally) external wrenches <span>$w_\text{ext}$</span>.</p><p>The <code>externalwrenches</code> argument can be used to specify additional wrenches that act on the <code>Mechanism</code>'s bodies.</p><p>This method implements the recursive Newton-Euler algorithm.</p><p>Currently doesn't support <code>Mechanism</code>s with cycles.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L553-L569">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.inverse_dynamics-Union{Tuple{W}, Tuple{V}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,SegmentedVector{JointID,V,KeyRange,P} where P<:AbstractArray{V,1} where KeyRange<:AbstractRange{JointID}}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,SegmentedVector{JointID,V,KeyRange,P} where P<:AbstractArray{V,1} where KeyRange<:AbstractRange{JointID},AbstractDict{BodyID,Wrench{W}}}} where W where V where M where X" href="#RigidBodyDynamics.inverse_dynamics-Union{Tuple{W}, Tuple{V}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,SegmentedVector{JointID,V,KeyRange,P} where P<:AbstractArray{V,1} where KeyRange<:AbstractRange{JointID}}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection where C,SegmentedVector{JointID,V,KeyRange,P} where P<:AbstractArray{V,1} where KeyRange<:AbstractRange{JointID},AbstractDict{BodyID,Wrench{W}}}} where W where V where M where X"><code>RigidBodyDynamics.inverse_dynamics</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">inverse_dynamics(state, v̇)
inverse_dynamics(state, v̇, externalwrenches)
</code></pre><p>Do inverse dynamics, i.e. compute <span>$\tau$</span> in the unconstrained joint-space equations of motion</p><div>\[M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau\]</div><p>given joint configuration vector <span>$q$</span>, joint velocity vector <span>$v$</span>, joint acceleration vector <span>$\dot{v}$</span> and (optionally) external wrenches <span>$w_\text{ext}$</span>.</p><p>The <code>externalwrenches</code> argument can be used to specify additional wrenches that act on the <code>Mechanism</code>'s bodies.</p><p>This method implements the recursive Newton-Euler algorithm.</p><p>Currently doesn't support <code>Mechanism</code>s with cycles.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L573-L589">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.mass-Tuple{Mechanism}" href="#RigidBodyDynamics.mass-Tuple{Mechanism}"><code>RigidBodyDynamics.mass</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">mass(m)
</code></pre><p>Return the total mass of the <code>Mechanism</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L17">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.mass_matrix!-Tuple{LinearAlgebra.Symmetric,MechanismState}" href="#RigidBodyDynamics.mass_matrix!-Tuple{LinearAlgebra.Symmetric,MechanismState}"><code>RigidBodyDynamics.mass_matrix!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">mass_matrix!(M, state)
</code></pre><p>Compute the joint-space mass matrix (also known as the inertia matrix) of the <code>Mechanism</code> in the given state, i.e., the matrix <span>$M(q)$</span> in the unconstrained joint-space equations of motion</p><div>\[M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau\]</div><p>This method implements the composite rigid body algorithm.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p><p>The <code>out</code> argument must be an <span>$n_v \times n_v$</span> lower triangular <code>Symmetric</code> matrix, where <span>$n_v$</span> is the dimension of the <code>Mechanism</code>'s joint velocity vector <span>$v$</span>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L237-L246">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.mass_matrix-Union{Tuple{MechanismState{X,M,C,JointCollection} where JointCollection}, Tuple{C}, Tuple{M}, Tuple{X}} where C where M where X" href="#RigidBodyDynamics.mass_matrix-Union{Tuple{MechanismState{X,M,C,JointCollection} where JointCollection}, Tuple{C}, Tuple{M}, Tuple{X}} where C where M where X"><code>RigidBodyDynamics.mass_matrix</code></a> — <span class="docstring-category">Method</span>.</div><div><div><p>Compute the joint-space mass matrix (also known as the inertia matrix) of the <code>Mechanism</code> in the given state, i.e., the matrix <span>$M(q)$</span> in the unconstrained joint-space equations of motion</p><div>\[M(q) \dot{v} + c(q, v, w_\text{ext}) = \tau\]</div><p>This method implements the composite rigid body algorithm.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L283-L292">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState,Any}" href="#RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState,Any}"><code>RigidBodyDynamics.momentum_matrix!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">momentum_matrix!(mat, state, transformfun)
</code></pre><p>Compute the momentum matrix <span>$A(q)$</span> of the <code>Mechanism</code> in the given state.</p><p>The momentum matrix maps the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> to its total momentum.</p><p>See also <a href="spatial.html#RigidBodyDynamics.Spatial.MomentumMatrix"><code>MomentumMatrix</code></a>.</p><p>The <code>out</code> argument must be a mutable <code>MomentumMatrix</code> with as many columns as the dimension of the <code>Mechanism</code>'s joint velocity vector <span>$v$</span>.</p><p><code>transformfun</code> 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 <code>out</code> is expressed.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L309-L320">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState,Transform3D}" href="#RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState,Transform3D}"><code>RigidBodyDynamics.momentum_matrix!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">momentum_matrix!(mat, state, root_to_desired)
</code></pre><p>Compute the momentum matrix <span>$A(q)$</span> of the <code>Mechanism</code> in the given state.</p><p>The momentum matrix maps the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> to its total momentum.</p><p>See also <a href="spatial.html#RigidBodyDynamics.Spatial.MomentumMatrix"><code>MomentumMatrix</code></a>.</p><p>The <code>out</code> argument must be a mutable <code>MomentumMatrix</code> with as many columns as the dimension of the <code>Mechanism</code>'s joint velocity vector <span>$v$</span>.</p><p><code>root_to_desired</code> is the transform from the <code>Mechanism</code>'s root frame to the frame in which <code>out</code> is expressed.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L337-L348">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState}" href="#RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState}"><code>RigidBodyDynamics.momentum_matrix!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">momentum_matrix!(out, state)
</code></pre><p>Compute the momentum matrix <span>$A(q)$</span> of the <code>Mechanism</code> in the given state.</p><p>The momentum matrix maps the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> to its total momentum.</p><p>See also <a href="spatial.html#RigidBodyDynamics.Spatial.MomentumMatrix"><code>MomentumMatrix</code></a>.</p><p>The <code>out</code> argument must be a mutable <code>MomentumMatrix</code> with as many columns as the dimension of the <code>Mechanism</code>'s joint velocity vector <span>$v$</span>.</p><p>See <a href="algorithms.html#RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState,Any}"><code>momentum_matrix!(out, state, root_to_desired)</code></a>. Uses <code>state</code> to compute the transform from the <code>Mechanism</code>'s root frame to the frame in which <code>out</code> is expressed.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L351-L362">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.momentum_matrix-Tuple{MechanismState}" href="#RigidBodyDynamics.momentum_matrix-Tuple{MechanismState}"><code>RigidBodyDynamics.momentum_matrix</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">momentum_matrix(state)
</code></pre><p>Compute the momentum matrix <span>$A(q)$</span> of the <code>Mechanism</code> in the given state.</p><p>The momentum matrix maps the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> to its total momentum.</p><p>See also <a href="spatial.html#RigidBodyDynamics.Spatial.MomentumMatrix"><code>MomentumMatrix</code></a>.</p><p>See <a href="algorithms.html#RigidBodyDynamics.momentum_matrix!-Tuple{MomentumMatrix,MechanismState,Any}"><code>momentum_matrix!(out, state)</code></a>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L370-L378">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.point_jacobian!-Tuple{PointJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Point3D}" href="#RigidBodyDynamics.point_jacobian!-Tuple{PointJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Point3D}"><code>RigidBodyDynamics.point_jacobian!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">point_jacobian!(out, state, path, point)
</code></pre><p>Compute the Jacobian mapping the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> 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).</p><p>Uses <code>state</code> to compute the transform from the <code>Mechanism</code>'s root frame to the frame in which <code>out</code> is expressed if necessary.</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L191-L196">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.point_jacobian-Union{Tuple{C}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection,TreePath{RigidBody{M},Joint{M,JT} where JT<:JointType{M}},Point3D}} where C where M where X" href="#RigidBodyDynamics.point_jacobian-Union{Tuple{C}, Tuple{M}, Tuple{X}, Tuple{MechanismState{X,M,C,JointCollection} where JointCollection,TreePath{RigidBody{M},Joint{M,JT} where JT<:JointType{M}},Point3D}} where C where M where X"><code>RigidBodyDynamics.point_jacobian</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">point_jacobian(state, path, point)
</code></pre><p>Compute the Jacobian mapping the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> 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).</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L213-L218">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics._point_jacobian!-Tuple{PointJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Point3D,Any}" href="#RigidBodyDynamics._point_jacobian!-Tuple{PointJacobian,MechanismState,RigidBodyDynamics.Graphs.TreePath,Point3D,Any}"><code>RigidBodyDynamics._point_jacobian!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">_point_jacobian!(Jp, state, path, point, transformfun)
</code></pre><p>Compute the Jacobian mapping the <code>Mechanism</code>'s joint velocity vector <span>$v$</span> 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).</p><p>This method does its computation in place, performing no dynamic memory allocation.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L161-L166">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.default_constraint_stabilization_gains-Union{Tuple{Type{T}}, Tuple{T}} where T" href="#RigidBodyDynamics.default_constraint_stabilization_gains-Union{Tuple{Type{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.default_constraint_stabilization_gains</code></a> — <span class="docstring-category">Method</span>.</div><div><div><p>Return the default Baumgarte constraint stabilization gains. These gains result in critical damping, and correspond to <span>$T_{stab} = 0.1$</span> in Featherstone (2008), section 8.3.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L624">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.subtree_mass-Union{Tuple{T}, Tuple{RigidBody{T},Mechanism{T}}} where T" href="#RigidBodyDynamics.subtree_mass-Union{Tuple{T}, Tuple{RigidBody{T},Mechanism{T}}} where T"><code>RigidBodyDynamics.subtree_mass</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">subtree_mass(base, mechanism)
</code></pre><p>Return the mass of a subtree of a <code>Mechanism</code>, rooted at <code>base</code> (including the mass of <code>base</code>).</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_algorithms.jl#L3">source</a></section><footer><hr/><a class="previous" href="mechanismstate.html"><span class="direction">Previous</span><span class="title">MechanismState</span></a><a class="next" href="customcollections.html"><span class="direction">Next</span><span class="title">Custom collection types</span></a></footer></article></body></html>