-
Notifications
You must be signed in to change notification settings - Fork 49
/
mechanismstate.html
53 lines (53 loc) · 45.2 KB
/
mechanismstate.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
46
47
48
49
50
51
52
53
<!DOCTYPE html>
<html lang="en"><head><meta charset="UTF-8"/><meta name="viewport" content="width=device-width, initial-scale=1.0"/><title>MechanismState · 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 class="current"><a class="toctext" href="mechanismstate.html">MechanismState</a><ul class="internal"><li><a class="toctext" href="#Index-1">Index</a></li><li><a class="toctext" href="#The-MechanismState-type-1">The <code>MechanismState</code> type</a></li><li><a class="toctext" href="#Functions-1">Functions</a></li></ul></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="mechanismstate.html">MechanismState</a></li></ul><a class="edit-page" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/master/docs/src/mechanismstate.md"><span class="fa"></span> Edit on GitHub</a></nav><hr/><div id="topbar"><span>MechanismState</span><a class="fa fa-bars" href="#"></a></div></header><h1><a class="nav-anchor" id="MechanismState-1" href="#MechanismState-1">MechanismState</a></h1><h2><a class="nav-anchor" id="Index-1" href="#Index-1">Index</a></h2><ul><li><a href="mechanismstate.html#RigidBodyDynamics.MechanismState"><code>RigidBodyDynamics.MechanismState</code></a></li><li><a href="mechanismstate.html#Base.copyto!-Tuple{MechanismState,MechanismState}"><code>Base.copyto!</code></a></li><li><a href="mechanismstate.html#Base.copyto!-Tuple{MechanismState,AbstractArray{T,1} where T}"><code>Base.copyto!</code></a></li><li><a href="mechanismstate.html#Base.copyto!-Tuple{AbstractArray{T,1} where T,MechanismState}"><code>Base.copyto!</code></a></li><li><a href="mechanismstate.html#Random.rand!-Tuple{MechanismState}"><code>Random.rand!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.additional_state-Tuple{MechanismState}"><code>RigidBodyDynamics.additional_state</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.bias_acceleration"><code>RigidBodyDynamics.bias_acceleration</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.bias_acceleration"><code>RigidBodyDynamics.bias_acceleration</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.configuration-Tuple{MechanismState}"><code>RigidBodyDynamics.configuration</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.configuration-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.configuration</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.configuration_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.configuration_range</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.constraint_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.constraint_range</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.crb_inertia"><code>RigidBodyDynamics.crb_inertia</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.global_coordinates!-Tuple{MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}"><code>RigidBodyDynamics.global_coordinates!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.gravitational_potential_energy"><code>RigidBodyDynamics.gravitational_potential_energy</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.joint_transform"><code>RigidBodyDynamics.joint_transform</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.local_coordinates!-Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}"><code>RigidBodyDynamics.local_coordinates!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.normalize_configuration!-Tuple{MechanismState}"><code>RigidBodyDynamics.normalize_configuration!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.num_additional_states-Tuple{MechanismState}"><code>RigidBodyDynamics.num_additional_states</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.num_positions-Tuple{MechanismState}"><code>RigidBodyDynamics.num_positions</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.num_velocities-Tuple{MechanismState}"><code>RigidBodyDynamics.num_velocities</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.rand_configuration!-Tuple{MechanismState}"><code>RigidBodyDynamics.rand_configuration!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.rand_velocity!-Tuple{MechanismState}"><code>RigidBodyDynamics.rand_velocity!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.relative_transform-Tuple{MechanismState,CartesianFrame3D,CartesianFrame3D}"><code>RigidBodyDynamics.relative_transform</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.relative_twist-Tuple{MechanismState,CartesianFrame3D,CartesianFrame3D}"><code>RigidBodyDynamics.relative_twist</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.relative_twist-Tuple{MechanismState,Union{BodyID, #s17} where #s17<:RigidBody,Union{BodyID, #s16} where #s16<:RigidBody}"><code>RigidBodyDynamics.relative_twist</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.reset_contact_state!-Tuple{MechanismState}"><code>RigidBodyDynamics.reset_contact_state!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.set_additional_state!-Tuple{MechanismState,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.set_additional_state!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.set_configuration!-Tuple{MechanismState,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.set_configuration!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.set_configuration!-Tuple{MechanismState,Joint,Any}"><code>RigidBodyDynamics.set_configuration!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.set_velocity!-Tuple{MechanismState,Joint,Any}"><code>RigidBodyDynamics.set_velocity!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.set_velocity!-Tuple{MechanismState,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.set_velocity!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.setdirty!-Tuple{MechanismState}"><code>RigidBodyDynamics.setdirty!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.spatial_inertia"><code>RigidBodyDynamics.spatial_inertia</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.transform_to_root"><code>RigidBodyDynamics.transform_to_root</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.twist"><code>RigidBodyDynamics.twist</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.twist_wrt_world"><code>RigidBodyDynamics.twist_wrt_world</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.velocity-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.velocity</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.velocity-Tuple{MechanismState}"><code>RigidBodyDynamics.velocity</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.velocity_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.velocity_range</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.zero!-Tuple{MechanismState}"><code>RigidBodyDynamics.zero!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.zero_configuration!-Tuple{MechanismState}"><code>RigidBodyDynamics.zero_configuration!</code></a></li><li><a href="mechanismstate.html#RigidBodyDynamics.zero_velocity!-Tuple{MechanismState}"><code>RigidBodyDynamics.zero_velocity!</code></a></li></ul><h2><a class="nav-anchor" id="The-MechanismState-type-1" href="#The-MechanismState-type-1">The <code>MechanismState</code> type</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.MechanismState" href="#RigidBodyDynamics.MechanismState"><code>RigidBodyDynamics.MechanismState</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct MechanismState{X, M, C, JointCollection}</code></pre><p>A <code>MechanismState</code> stores state information for an entire <code>Mechanism</code>. It contains the joint configuration and velocity vectors <span>$q$</span> and <span>$v$</span>, and a vector of additional states <span>$s$</span>. In addition, it stores cache variables that depend on <span>$q$</span> and <span>$v$</span> and are aimed at preventing double work.</p><p>Type parameters:</p><ul><li><code>X</code>: the scalar type of the <span>$q$</span>, <span>$v$</span>, and <span>$s$</span> vectors.</li><li><code>M</code>: the scalar type of the <code>Mechanism</code></li><li><code>C</code>: the scalar type of the cache variables (<code>== promote_type(X, M)</code>)</li><li><code>JointCollection</code>: the type of the <code>treejoints</code> and <code>nontreejoints</code> members (a <code>TypeSortedCollection</code> subtype)</li></ul></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L21">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.additional_state-Tuple{MechanismState}" href="#RigidBodyDynamics.additional_state-Tuple{MechanismState}"><code>RigidBodyDynamics.additional_state</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">additional_state(state)
</code></pre><p>Return the vector of additional states <span>$s$</span>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L365">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.bias_acceleration" href="#RigidBodyDynamics.bias_acceleration"><code>RigidBodyDynamics.bias_acceleration</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">bias_acceleration(state, joint)
bias_acceleration(state, joint, safe)
</code></pre><p>Return the bias acceleration across the given joint, i.e. the spatial acceleration of <code>frame_after(joint)</code> with respect to <code>frame_before(joint)</code>, expressed in the root frame of the mechanism when all joint accelerations are zero.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L557">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.bias_acceleration" href="#RigidBodyDynamics.bias_acceleration"><code>RigidBodyDynamics.bias_acceleration</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">bias_acceleration(state, body)
bias_acceleration(state, body, safe)
</code></pre><p>Return the bias acceleration of the given body with respect to the world, i.e. the spatial acceleration of <code>default_frame(body)</code> with respect to the root frame of the mechanism, expressed in the root frame, when all joint accelerations are zero.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L591">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.configuration-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}" href="#RigidBodyDynamics.configuration-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.configuration</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">configuration(state, joint)
</code></pre><p>Return the part of the configuration vector <span>$q$</span> associated with <code>joint</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L221">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.configuration-Tuple{MechanismState}" href="#RigidBodyDynamics.configuration-Tuple{MechanismState}"><code>RigidBodyDynamics.configuration</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">configuration(state)
</code></pre><p>Return the configuration vector <span>$q$</span>.</p><p>Note that this returns a reference to the underlying data in <code>state</code>. The user is responsible for calling <a href="mechanismstate.html#RigidBodyDynamics.setdirty!-Tuple{MechanismState}"><code>setdirty!</code></a> after modifying this vector to ensure that dependent cache variables are invalidated.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L343">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.configuration_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}" href="#RigidBodyDynamics.configuration_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.configuration_range</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">configuration_range(state, joint)
</code></pre><p>Return the range of indices into the joint configuration vector <span>$q$</span> corresponding to joint <code>joint</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L512">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.crb_inertia" href="#RigidBodyDynamics.crb_inertia"><code>RigidBodyDynamics.crb_inertia</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">crb_inertia(state, body)
crb_inertia(state, body, safe)
</code></pre><p>Return the composite rigid body inertia <code>body</code> expressed in the root frame of the mechanism.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L615">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.global_coordinates!-Tuple{MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}" href="#RigidBodyDynamics.global_coordinates!-Tuple{MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}"><code>RigidBodyDynamics.global_coordinates!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">global_coordinates!(state, q0, ϕ)
</code></pre><p>Convert local coordinates <span>$\phi$</span> centered around <span>$q_0$</span> to (global) configuration vector <span>$q$</span>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L1016">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.gravitational_potential_energy" href="#RigidBodyDynamics.gravitational_potential_energy"><code>RigidBodyDynamics.gravitational_potential_energy</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">gravitational_potential_energy(state, body)
gravitational_potential_energy(state, body, safe)
</code></pre><p>Return the gravitational potential energy in the given state, computed as the negation of the dot product of the gravitational force and the center of mass expressed in the <code>Mechanism</code>'s root frame.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L832">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.joint_transform" href="#RigidBodyDynamics.joint_transform"><code>RigidBodyDynamics.joint_transform</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">joint_transform(state, joint)
joint_transform(state, joint, safe)
</code></pre><p>Return the joint transform for the given joint, i.e. the transform from <code>frame_after(joint)</code> to <code>frame_before(joint)</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L535">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.local_coordinates!-Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}" href="#RigidBodyDynamics.local_coordinates!-Tuple{SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T,MechanismState,SegmentedVector{JointID,T,KeyRange,P} where P<:AbstractArray{T,1} where KeyRange<:AbstractRange{JointID} where T}"><code>RigidBodyDynamics.local_coordinates!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">local_coordinates!(ϕ, ϕd, state, q0)
</code></pre><p>Compute local coordinates <span>$\phi$</span> centered around (global) configuration vector <span>$q_0$</span>, as well as their time derivatives <span>$\dot{\phi}$</span>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L1000">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.normalize_configuration!-Tuple{MechanismState}" href="#RigidBodyDynamics.normalize_configuration!-Tuple{MechanismState}"><code>RigidBodyDynamics.normalize_configuration!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">normalize_configuration!(state)
</code></pre><p>Project the configuration vector <span>$q$</span> onto the configuration manifold.</p><p>For example:</p><ul><li>for a part of <span>$q$</span> corresponding to a revolute joint, this method is a no-op;</li><li>for a part of <span>$q$</span> corresponding to a spherical joint that uses a unit quaternion</li></ul><p>to parameterize the orientation of its successor with respect to its predecessor, <code>normalize_configuration!</code> will renormalize the quaternion so that it is indeed of unit length.</p><div class="admonition warning"><div class="admonition-title">Warning</div><div class="admonition-text"></div></div><p>This method does not ensure that the configuration or velocity satisfy joint configuration or velocity limits/bounds.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L490">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_additional_states-Tuple{MechanismState}" href="#RigidBodyDynamics.num_additional_states-Tuple{MechanismState}"><code>RigidBodyDynamics.num_additional_states</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">num_additional_states(state)
</code></pre><p>Return the length of the vector of additional states <span>$s$</span> (currently used for stateful contact models).</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L209">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_positions-Tuple{MechanismState}" href="#RigidBodyDynamics.num_positions-Tuple{MechanismState}"><code>RigidBodyDynamics.num_positions</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">num_positions(state)
</code></pre><p>Return the length of the joint configuration vector <span>$q$</span>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L195">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_velocities-Tuple{MechanismState}" href="#RigidBodyDynamics.num_velocities-Tuple{MechanismState}"><code>RigidBodyDynamics.num_velocities</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">num_velocities(state)
</code></pre><p>Return the length of the 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_state.jl#L202">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.rand_configuration!-Tuple{MechanismState}" href="#RigidBodyDynamics.rand_configuration!-Tuple{MechanismState}"><code>RigidBodyDynamics.rand_configuration!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">rand_configuration!(state)
</code></pre><p>Randomize the configuration vector <span>$q$</span>. The distribution depends on the particular joint types present in the associated <code>Mechanism</code>. The resulting <span>$q$</span> is guaranteed to be on the <code>Mechanism</code>'s configuration manifold. Invalidates cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L307">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.rand_velocity!-Tuple{MechanismState}" href="#RigidBodyDynamics.rand_velocity!-Tuple{MechanismState}"><code>RigidBodyDynamics.rand_velocity!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">rand_velocity!(state)
</code></pre><p>Randomize the velocity vector <span>$v$</span>. Invalidates cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L323">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.relative_transform-Tuple{MechanismState,CartesianFrame3D,CartesianFrame3D}" href="#RigidBodyDynamics.relative_transform-Tuple{MechanismState,CartesianFrame3D,CartesianFrame3D}"><code>RigidBodyDynamics.relative_transform</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">relative_transform(state, from, to)
</code></pre><p>Return the homogeneous transform from <code>from</code> to <code>to</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L948">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.relative_twist-Tuple{MechanismState,CartesianFrame3D,CartesianFrame3D}" href="#RigidBodyDynamics.relative_twist-Tuple{MechanismState,CartesianFrame3D,CartesianFrame3D}"><code>RigidBodyDynamics.relative_twist</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">relative_twist(state, body_frame, base_frame)
</code></pre><p>Return the twist of <code>body_frame</code> with respect to <code>base_frame</code>, expressed in the <code>Mechanism</code>'s root frame.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L969">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.relative_twist-Tuple{MechanismState,Union{BodyID, #s17} where #s17<:RigidBody,Union{BodyID, #s16} where #s16<:RigidBody}" href="#RigidBodyDynamics.relative_twist-Tuple{MechanismState,Union{BodyID, #s17} where #s17<:RigidBody,Union{BodyID, #s16} where #s16<:RigidBody}"><code>RigidBodyDynamics.relative_twist</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">relative_twist(state, body, base)
</code></pre><p>Return the twist of <code>body</code> with respect to <code>base</code>, expressed in the <code>Mechanism</code>'s root frame.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L958">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.set_additional_state!-Tuple{MechanismState,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.set_additional_state!-Tuple{MechanismState,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.set_additional_state!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">set_additional_state!(state, s)
</code></pre><p>Set the vector of additional states <span>$s$</span>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L422">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.set_configuration!-Tuple{MechanismState,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.set_configuration!-Tuple{MechanismState,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.set_configuration!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">set_configuration!(state, q)
</code></pre><p>Set the configuration vector <span>$q$</span>. Invalidates cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L402">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.set_configuration!-Tuple{MechanismState,Joint,Any}" href="#RigidBodyDynamics.set_configuration!-Tuple{MechanismState,Joint,Any}"><code>RigidBodyDynamics.set_configuration!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">set_configuration!(state, joint, config)
</code></pre><p>Set the part of the configuration vector associated with <code>joint</code>. Invalidates cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L378">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.set_velocity!-Tuple{MechanismState,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.set_velocity!-Tuple{MechanismState,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.set_velocity!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">set_velocity!(state, v)
</code></pre><p>Set the velocity vector <span>$v$</span>. Invalidates cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L412">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.set_velocity!-Tuple{MechanismState,Joint,Any}" href="#RigidBodyDynamics.set_velocity!-Tuple{MechanismState,Joint,Any}"><code>RigidBodyDynamics.set_velocity!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">set_velocity!(state, joint, vel)
</code></pre><p>Set the part of the velocity vector associated with <code>joint</code>. Invalidates cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L390">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.setdirty!-Tuple{MechanismState}" href="#RigidBodyDynamics.setdirty!-Tuple{MechanismState}"><code>RigidBodyDynamics.setdirty!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">setdirty!(state)
</code></pre><p>Invalidate all cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L235">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.spatial_inertia" href="#RigidBodyDynamics.spatial_inertia"><code>RigidBodyDynamics.spatial_inertia</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">spatial_inertia(state, body)
spatial_inertia(state, body, safe)
</code></pre><p>Return the spatial inertia of <code>body</code> expressed in the root frame of the mechanism.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L604">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.transform_to_root" href="#RigidBodyDynamics.transform_to_root"><code>RigidBodyDynamics.transform_to_root</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">transform_to_root(state, body)
transform_to_root(state, body, safe)
</code></pre><p>Return the transform from <code>default_frame(body)</code> to the root frame of the mechanism.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L569">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.twist_wrt_world" href="#RigidBodyDynamics.twist_wrt_world"><code>RigidBodyDynamics.twist_wrt_world</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">twist_wrt_world(state, body)
twist_wrt_world(state, body, safe)
</code></pre><p>Return the twist of <code>default_frame(body)</code> with respect to the root frame of the mechanism, expressed in the root frame.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L580">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.velocity-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}" href="#RigidBodyDynamics.velocity-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.velocity</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">velocity(state, joint)
</code></pre><p>Return the part of the velocity vector <span>$v$</span> associated with <code>joint</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L228">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.velocity-Tuple{MechanismState}" href="#RigidBodyDynamics.velocity-Tuple{MechanismState}"><code>RigidBodyDynamics.velocity</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">velocity(state)
</code></pre><p>Return the velocity vector <span>$v$</span>.</p><p>Note that this function returns a read-write reference to a field in <code>state</code>. The user is responsible for calling <a href="mechanismstate.html#RigidBodyDynamics.setdirty!-Tuple{MechanismState}"><code>setdirty!</code></a> after modifying this vector to ensure that dependent cache variables are invalidated.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L354">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.velocity_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}" href="#RigidBodyDynamics.velocity_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.velocity_range</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">velocity_range(state, joint)
</code></pre><p>Return the range of indices into the joint velocity vector <span>$v$</span> corresponding to joint <code>joint</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L519">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.zero!-Tuple{MechanismState}" href="#RigidBodyDynamics.zero!-Tuple{MechanismState}"><code>RigidBodyDynamics.zero!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">zero!(state)
</code></pre><p>Zero both the configuration and velocity. Invalidates cache variables.</p><p>See <a href="joints.html#RigidBodyDynamics.zero_configuration!-Tuple{AbstractArray{T,1} where T,Joint}"><code>zero_configuration!</code></a>, <a href="mechanismstate.html#RigidBodyDynamics.zero_velocity!-Tuple{MechanismState}"><code>zero_velocity!</code></a>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L298">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.zero_configuration!-Tuple{MechanismState}" href="#RigidBodyDynamics.zero_configuration!-Tuple{MechanismState}"><code>RigidBodyDynamics.zero_configuration!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">zero_configuration!(state)
</code></pre><p>'Zero' the configuration vector <span>$q$</span>. Invalidates cache variables.</p><p>Note that when the <code>Mechanism</code> contains e.g. quaternion-parameterized joints, <span>$q$</span> may not actually be set to all zeros; the quaternion part of the configuration vector would be set to identity. The contract is that each of the joint transforms should be an identity transform.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L269">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.zero_velocity!-Tuple{MechanismState}" href="#RigidBodyDynamics.zero_velocity!-Tuple{MechanismState}"><code>RigidBodyDynamics.zero_velocity!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">zero_velocity!(state)
</code></pre><p>Zero the velocity vector <span>$v$</span>. Invalidates cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L287">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="Base.copyto!-Tuple{AbstractArray{T,1} where T,MechanismState}" href="#Base.copyto!-Tuple{AbstractArray{T,1} where T,MechanismState}"><code>Base.copyto!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">copyto!(dest, src)
</code></pre><p>Copy state information in state <code>dest</code> to vector <code>src</code> (ordered <code>[q; v; s]</code>).</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L464">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="Base.copyto!-Tuple{MechanismState,AbstractArray{T,1} where T}" href="#Base.copyto!-Tuple{MechanismState,AbstractArray{T,1} where T}"><code>Base.copyto!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">copyto!(dest, src)
</code></pre><p>Copy state information in vector <code>src</code> (ordered <code>[q; v; s]</code>) to state <code>dest</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L447">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="Base.copyto!-Tuple{MechanismState,MechanismState}" href="#Base.copyto!-Tuple{MechanismState,MechanismState}"><code>Base.copyto!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">copyto!(dest, src)
</code></pre><p>Copy (minimal representation of) state <code>src</code> to state <code>dest</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L432">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="Random.rand!-Tuple{MechanismState}" href="#Random.rand!-Tuple{MechanismState}"><code>Random.rand!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">rand!(state)
</code></pre><p>Randomize both the configuration and velocity. Invalidates cache variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L335">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.constraint_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}" href="#RigidBodyDynamics.constraint_range-Tuple{MechanismState,Union{JointID, #s125} where #s125<:Joint}"><code>RigidBodyDynamics.constraint_range</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">constraint_range(state, joint)
</code></pre><p>Return the range of row indices into the constraint Jacobian corresponding to joint <code>joint</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L526">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.reset_contact_state!-Tuple{MechanismState}" href="#RigidBodyDynamics.reset_contact_state!-Tuple{MechanismState}"><code>RigidBodyDynamics.reset_contact_state!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">reset_contact_state!(state)
</code></pre><p>Reset all contact state variables.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L254">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.twist" href="#RigidBodyDynamics.twist"><code>RigidBodyDynamics.twist</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">twist(state, joint)
twist(state, joint, safe)
</code></pre><p>Return the joint twist for the given joint, i.e. the twist of <code>frame_after(joint)</code> with respect to <code>frame_before(joint)</code>, expressed in the root frame of the mechanism.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/mechanism_state.jl#L546">source</a></section><footer><hr/><a class="previous" href="mechanism.html"><span class="direction">Previous</span><span class="title">Mechanism</span></a><a class="next" href="algorithms.html"><span class="direction">Next</span><span class="title">Kinematics/dynamics algorithms</span></a></footer></article></body></html>