-
Notifications
You must be signed in to change notification settings - Fork 49
/
mechanism.html
35 lines (35 loc) · 40.1 KB
/
mechanism.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>Mechanism · 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 class="current"><a class="toctext" href="mechanism.html">Mechanism</a><ul class="internal"><li><a class="toctext" href="#Index-1">Index</a></li><li><a class="toctext" href="#The-Mechanism-type-1">The <code>Mechanism</code> type</a></li><li><a class="toctext" href="#mechanism_create-1">Creating and modifying <code>Mechanism</code>s</a></li><li><a class="toctext" href="#Basic-functionality-1">Basic functionality</a></li></ul></li><li><a class="toctext" href="mechanismstate.html">MechanismState</a></li><li><a class="toctext" href="algorithms.html">Kinematics/dynamics algorithms</a></li><li><a class="toctext" href="customcollections.html">Custom collection types</a></li><li><a class="toctext" href="caches.html">Cache types</a></li><li><a class="toctext" href="simulation.html">Simulation</a></li><li><a class="toctext" href="benchmarks.html">Benchmarks</a></li></ul></nav><article id="docs"><header><nav><ul><li><a href="mechanism.html">Mechanism</a></li></ul><a class="edit-page" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/master/docs/src/mechanism.md"><span class="fa"></span> Edit on GitHub</a></nav><hr/><div id="topbar"><span>Mechanism</span><a class="fa fa-bars" href="#"></a></div></header><h1><a class="nav-anchor" id="Mechanisms-1" href="#Mechanisms-1">Mechanisms</a></h1><h2><a class="nav-anchor" id="Index-1" href="#Index-1">Index</a></h2><ul><li><a href="mechanism.html#RigidBodyDynamics.Mechanism"><code>RigidBodyDynamics.Mechanism</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.attach!-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.Mechanism{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.attach!</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.attach!-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.Joint{T,JT} where JT<:RigidBodyDynamics.JointType{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.attach!</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.bodies-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.bodies</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.body_fixed_frame_definition-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D}"><code>RigidBodyDynamics.body_fixed_frame_definition</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.body_fixed_frame_to_body-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D}"><code>RigidBodyDynamics.body_fixed_frame_to_body</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.fixed_transform-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D,RigidBodyDynamics.Spatial.CartesianFrame3D}"><code>RigidBodyDynamics.fixed_transform</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.in_joints-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.in_joints</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.joint_to_parent-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.joint_to_parent</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.joints-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.joints</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.joints_to_children-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.joints_to_children</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.maximal_coordinates-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.maximal_coordinates</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.non_tree_joints-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.non_tree_joints</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.num_additional_states-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.num_additional_states</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.num_constraints-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.num_constraints</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.num_positions-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.num_positions</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.num_velocities-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.num_velocities</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.out_joints-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.out_joints</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.parse_urdf"><code>RigidBodyDynamics.parse_urdf</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.path-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.RigidBody,RigidBodyDynamics.RigidBody}"><code>RigidBodyDynamics.path</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.predecessor-Tuple{RigidBodyDynamics.Joint,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.predecessor</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.rand_chain_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T"><code>RigidBodyDynamics.rand_chain_mechanism</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.rand_floating_tree_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T"><code>RigidBodyDynamics.rand_floating_tree_mechanism</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.rand_tree_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T"><code>RigidBodyDynamics.rand_tree_mechanism</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.rand_tree_mechanism-Union{Tuple{Type{T},Function,Vararg{Type{#s8} where #s8<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T"><code>RigidBodyDynamics.rand_tree_mechanism</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.rebuild_spanning_tree!-Union{Tuple{M}, Tuple{RigidBodyDynamics.Mechanism{M},Associative}, Tuple{RigidBodyDynamics.Mechanism{M}}} where M"><code>RigidBodyDynamics.rebuild_spanning_tree!</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.remove_fixed_tree_joints!-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.remove_fixed_tree_joints!</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.remove_joint!-Union{Tuple{M}, Tuple{RigidBodyDynamics.Mechanism{M},RigidBodyDynamics.Joint{M,JT} where JT<:RigidBodyDynamics.JointType{M}}} where M"><code>RigidBodyDynamics.remove_joint!</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.root_body-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.root_body</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.root_frame-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.root_frame</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.submechanism-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.submechanism</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.successor-Tuple{RigidBodyDynamics.Joint,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.successor</code></a></li><li><a href="mechanism.html#RigidBodyDynamics.tree_joints-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.tree_joints</code></a></li></ul><h2><a class="nav-anchor" id="The-Mechanism-type-1" href="#The-Mechanism-type-1">The <code>Mechanism</code> type</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Mechanism" href="#RigidBodyDynamics.Mechanism"><code>RigidBodyDynamics.Mechanism</code></a> — <span class="docstring-category">Type</span>.</div><div><pre><code class="language-julia">type Mechanism{T}</code></pre><p>A <code>Mechanism</code> represents an interconnection of rigid bodies and joints. <code>Mechanism</code>s store the joint layout and inertia parameters, but no state-dependent information.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L1">source</a></section><h2><a class="nav-anchor" id="mechanism_create-1" href="#mechanism_create-1">Creating and modifying <code>Mechanism</code>s</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.parse_urdf" href="#RigidBodyDynamics.parse_urdf"><code>RigidBodyDynamics.parse_urdf</code></a> — <span class="docstring-category">Function</span>.</div><div><pre><code class="language-julia">parse_urdf(scalartype, filename)
</code></pre><p>Create a <code>Mechanism</code> by parsing a <a href="http://wiki.ros.org/urdf">URDF</a> file.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/parse_urdf.jl#L127">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.attach!-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.Mechanism{T}}, Tuple{T}} where T" href="#RigidBodyDynamics.attach!-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.Mechanism{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.attach!</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">attach!(mechanism, parentbody, childmechanism; child_root_pose)
</code></pre><p>Attach a copy of <code>childmechanism</code> to <code>mechanism</code>. Return mappings from the bodies and joints of the <code>childmechanism</code> to the bodies and joints that were added to <code>mechanism</code>.</p><p>Essentially replaces the root body of a copy of <code>childmechanism</code> with <code>parentbody</code> (which belongs to <code>mechanism</code>).</p><p>Note: gravitational acceleration for childmechanism is ignored.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L63">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.attach!-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.Joint{T,JT} where JT<:RigidBodyDynamics.JointType{T}}, Tuple{T}} where T" href="#RigidBodyDynamics.attach!-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.RigidBody{T},RigidBodyDynamics.Joint{T,JT} where JT<:RigidBodyDynamics.JointType{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.attach!</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">attach!(mechanism, predecessor, successor, joint; joint_pose, successor_pose)
</code></pre><p>Attach <code>successor</code> to <code>predecessor</code> using <code>joint</code>.</p><p>See <a href="joints.html#RigidBodyDynamics.Joint"><code>Joint</code></a> for definitions of the terms successor and predecessor.</p><p>The <code>Transform3D</code>s <code>joint_pose</code> and <code>successor_pose</code> define where <code>joint</code> is attached to each body. <code>joint_pose</code> should define <code>frame_before(joint)</code> with respect to any frame fixed to <code>predecessor</code>, and likewise <code>successor_pose</code> should define any frame fixed to <code>successor</code> with respect to <code>frame_after(joint)</code>.</p><p><code>predecessor</code> is required to already be among the bodies of the <code>Mechanism</code>.</p><p>If <code>successor</code> is not yet a part of the <code>Mechanism</code>, it will be added to the <code>Mechanism</code>. Otherwise, the <code>joint</code> will be treated as a non-tree edge in the <code>Mechanism</code>, effectively creating a loop constraint that will be enforced using Lagrange multipliers (as opposed to using recursive algorithms).</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L1">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.maximal_coordinates-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.maximal_coordinates-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.maximal_coordinates</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">maximal_coordinates(mechanism)
</code></pre><p>Return a dynamically equivalent <code>Mechanism</code>, but with a flat tree structure with all bodies attached to the root body with a quaternion floating joint, and with the 'tree edge' joints of the input <code>Mechanism</code> transformed into non-tree edge joints (a constraint enforced using Lagrange multipliers in <code>dynamics!</code>). In addition, return:</p><ul><li><p>a mapping from bodies in the maximal-coordinate <code>Mechanism</code> to their floating joints.</p></li><li><p>a mapping from bodies in the input <code>Mechanism</code> to bodies in the returned <code>Mechanism</code></p></li><li><p>a mapping from joints in the input <code>Mechanism</code> to joints in the returned <code>Mechanism</code></p></li></ul></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L257">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.rand_chain_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T" href="#RigidBodyDynamics.rand_chain_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T"><code>RigidBodyDynamics.rand_chain_mechanism</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">rand_chain_mechanism(?, jointtypes)
</code></pre><p>Create a random chain <code>Mechanism</code> with the given joint types.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L329">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.rand_floating_tree_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T" href="#RigidBodyDynamics.rand_floating_tree_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T"><code>RigidBodyDynamics.rand_floating_tree_mechanism</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">rand_floating_tree_mechanism(?, nonfloatingjointtypes)
</code></pre><p>Create a random tree <code>Mechanism</code>, with a quaternion floating joint as the first joint (between the root body and the first non-root body).</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L345">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.rand_tree_mechanism-Union{Tuple{Type{T},Function,Vararg{Type{#s8} where #s8<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T" href="#RigidBodyDynamics.rand_tree_mechanism-Union{Tuple{Type{T},Function,Vararg{Type{#s8} where #s8<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T"><code>RigidBodyDynamics.rand_tree_mechanism</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">rand_tree_mechanism(?, parentselector, jointtypes)
</code></pre><p>Create a random tree <code>Mechanism</code> with the given joint types. Each new body is attached to a parent selected using the <code>parentselector</code> function.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L308">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.rand_tree_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T" href="#RigidBodyDynamics.rand_tree_mechanism-Union{Tuple{Type{T},Vararg{Type{#s22} where #s22<:RigidBodyDynamics.JointType{T},N} where N}, Tuple{T}} where T"><code>RigidBodyDynamics.rand_tree_mechanism</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">rand_tree_mechanism(?, jointtypes)
</code></pre><p>Create a random tree <code>Mechanism</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L337">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.remove_fixed_tree_joints!-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.remove_fixed_tree_joints!-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.remove_fixed_tree_joints!</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">remove_fixed_tree_joints!(mechanism)
</code></pre><p>Remove any fixed joints present as tree edges in <code>mechanism</code> by merging the rigid bodies that these fixed joints join together into bodies with equivalent inertial properties. Return the fixed joints that were removed.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L194">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.remove_joint!-Union{Tuple{M}, Tuple{RigidBodyDynamics.Mechanism{M},RigidBodyDynamics.Joint{M,JT} where JT<:RigidBodyDynamics.JointType{M}}} where M" href="#RigidBodyDynamics.remove_joint!-Union{Tuple{M}, Tuple{RigidBodyDynamics.Mechanism{M},RigidBodyDynamics.Joint{M,JT} where JT<:RigidBodyDynamics.JointType{M}}} where M"><code>RigidBodyDynamics.remove_joint!</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">remove_joint!(mechanism, joint; flipped_joint_map, spanning_tree_next_edge)
</code></pre><p>Remove a joint from the mechanism. Rebuilds the spanning tree if the joint is part of the current spanning tree.</p><p>Optionally, the <code>flipped_joint_map</code> keyword argument can be used to pass in an associative container that will be populated with a mapping from original joints to flipped joints, if removing <code>joint</code> requires rebuilding the spanning tree of <code>mechanism</code> and the polarity of some joints needed to be changed in the process.</p><p>Also optionally, <code>spanning_tree_next_edge</code> can be used to select which joints should become part of the new spanning tree, if rebuilding the spanning tree is required.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L157">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.submechanism-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T}}, Tuple{T}} where T" href="#RigidBodyDynamics.submechanism-Union{Tuple{RigidBodyDynamics.Mechanism{T},RigidBodyDynamics.RigidBody{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.submechanism</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">submechanism(mechanism, submechanismroot)
</code></pre><p>Create a new <code>Mechanism</code> from the subtree of <code>mechanism</code> rooted at <code>submechanismroot</code>.</p><p>Also return mappings from the bodies and joints of the input mechanism to the bodies and joints of the submechanism.</p><p>Any non-tree joint in <code>mechanism</code> will appear in the returned <code>Mechanism</code> if and only if both its successor and its predecessor are part of the subtree.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L99">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.rebuild_spanning_tree!-Union{Tuple{M}, Tuple{RigidBodyDynamics.Mechanism{M},Associative}, Tuple{RigidBodyDynamics.Mechanism{M}}} where M" href="#RigidBodyDynamics.rebuild_spanning_tree!-Union{Tuple{M}, Tuple{RigidBodyDynamics.Mechanism{M},Associative}, Tuple{RigidBodyDynamics.Mechanism{M}}} where M"><code>RigidBodyDynamics.rebuild_spanning_tree!</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">rebuild_spanning_tree!(mechanism)
rebuild_spanning_tree!(mechanism, flipped_joint_map; next_edge)
</code></pre><p>Reconstruct the mechanism's spanning tree.</p><p>Optionally, the <code>flipped_joint_map</code> keyword argument can be used to pass in an associative container that will be populated with a mapping from original joints to flipped joints, if the rebuilding process required the polarity of some joints to be flipped.</p><p>Also optionally, <code>next_edge</code> can be used to select which joints should become part of the new spanning tree.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism_modification.jl#L137">source</a></section><h2><a class="nav-anchor" id="Basic-functionality-1" href="#Basic-functionality-1">Basic functionality</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.bodies-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.bodies-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.bodies</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">bodies(mechanism)
</code></pre><p>Return the <code>RigidBody</code>s that are part of the <code>Mechanism</code> as an iterable collection.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L54">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.body_fixed_frame_to_body-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D}" href="#RigidBodyDynamics.body_fixed_frame_to_body-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D}"><code>RigidBodyDynamics.body_fixed_frame_to_body</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">body_fixed_frame_to_body(mechanism, frame)
</code></pre><p>Return the <code>RigidBody</code> to which <code>frame</code> is attached.</p><p>Note: this function is linear in the number of bodies and is not meant to be called in tight loops.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L146">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.fixed_transform-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D,RigidBodyDynamics.Spatial.CartesianFrame3D}" href="#RigidBodyDynamics.fixed_transform-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D,RigidBodyDynamics.Spatial.CartesianFrame3D}"><code>RigidBodyDynamics.fixed_transform</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">fixed_transform(mechanism, from, to)
</code></pre><p>Return the transform from <code>CartesianFrame3D</code> <code>from</code> to <code>to</code>, both of which are rigidly attached to the same <code>RigidBody</code>.</p><p>Note: this function is linear in the number of bodies and is not meant to be called in tight loops.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L178">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.in_joints-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.in_joints-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.in_joints</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">in_joints(body, mechanism)
</code></pre><p>Return the joints that have <code>body</code> as their <a href="mechanism.html#RigidBodyDynamics.successor-Tuple{RigidBodyDynamics.Joint,RigidBodyDynamics.Mechanism}"><code>successor</code></a>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L218">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.joint_to_parent-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.joint_to_parent-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.joint_to_parent</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">joint_to_parent(body, mechanism)
</code></pre><p>Return the joint that is part of the mechanism's kinematic tree and has <code>body</code> as its successor.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L233">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.joints-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.joints-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.joints</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">joints(mechanism)
</code></pre><p>Return the <code>Joint</code>s that are part of the <code>Mechanism</code> as an iterable collection.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L33">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.joints_to_children-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.joints_to_children-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.joints_to_children</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">joints_to_children(body, mechanism)
</code></pre><p>Return the joints that are part of the mechanism's kinematic tree and have <code>body</code> as their predecessor.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L225">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_additional_states-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.num_additional_states-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.num_additional_states</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">num_additional_states(mechanism)
</code></pre><p>Return the dimension of the vector of additional states <span>$s$</span> (used for stateful contact models).</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L133">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_constraints-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.num_constraints-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.num_constraints</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">num_constraints(mechanism)
</code></pre><p>Return the number of constraints imposed by the mechanism's non-tree joints (i.e., the number of rows of the constraint Jacobian).</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L126">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_positions-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.num_positions-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.num_positions</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">num_positions(mechanism)
</code></pre><p>Return the dimension of the joint configuration vector <span>$q$</span>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L112">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_velocities-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.num_velocities-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.num_velocities</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">num_velocities(mechanism)
</code></pre><p>Return the dimension of the joint velocity vector <span>$v$</span>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L119">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.out_joints-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.out_joints-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.out_joints</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">out_joints(body, mechanism)
</code></pre><p>Return the joints that have <code>body</code> as their <a href="mechanism.html#RigidBodyDynamics.predecessor-Tuple{RigidBodyDynamics.Joint,RigidBodyDynamics.Mechanism}"><code>predecessor</code></a>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L211">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.path-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.RigidBody,RigidBodyDynamics.RigidBody}" href="#RigidBodyDynamics.path-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.RigidBody,RigidBodyDynamics.RigidBody}"><code>RigidBodyDynamics.path</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">path(mechanism, from, to)
</code></pre><p>Return the path from rigid body <code>from</code> to <code>to</code> along edges of the <code>Mechanism</code>'s kinematic tree.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L75">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.predecessor-Tuple{RigidBodyDynamics.Joint,RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.predecessor-Tuple{RigidBodyDynamics.Joint,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.predecessor</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">predecessor(joint, mechanism)
</code></pre><p>Return the body 'before' the joint, i.e. the 'tail' of the joint interpreted as an arrow in the <code>Mechanism</code>'s kinematic graph.</p><p>See <a href="joints.html#RigidBodyDynamics.Joint"><code>Joint</code></a>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L191">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.root_body-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.root_body-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.root_body</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">root_body(mechanism)
</code></pre><p>Return the root (stationary 'world') body of the <code>Mechanism</code>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L61">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.root_frame-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.root_frame-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.root_frame</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">root_frame(mechanism)
</code></pre><p>Return the default frame of the root body.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L68">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.successor-Tuple{RigidBodyDynamics.Joint,RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.successor-Tuple{RigidBodyDynamics.Joint,RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.successor</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">successor(joint, mechanism)
</code></pre><p>Return the body 'after' the joint, i.e. the 'head' of the joint interpreted as an arrow in the <code>Mechanism</code>'s kinematic graph.</p><p>See <a href="joints.html#RigidBodyDynamics.Joint"><code>Joint</code></a>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L201">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.tree_joints-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.tree_joints-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.tree_joints</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">tree_joints(mechanism)
</code></pre><p>Return the <code>Joint</code>s that are part of the <code>Mechanism</code>'s spanning tree as an iterable collection.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L40">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.body_fixed_frame_definition-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D}" href="#RigidBodyDynamics.body_fixed_frame_definition-Tuple{RigidBodyDynamics.Mechanism,RigidBodyDynamics.Spatial.CartesianFrame3D}"><code>RigidBodyDynamics.body_fixed_frame_definition</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">body_fixed_frame_definition(mechanism, frame)
</code></pre><p>Return the definition of body-fixed frame <code>frame</code>, i.e., the <code>Transform3D</code> from <code>frame</code> to the default frame of the body to which it is attached.</p><p>Note: this function is linear in the number of bodies and is not meant to be called in tight loops.</p><p>See also <a href="rigidbody.html#RigidBodyDynamics.default_frame-Tuple{RigidBodyDynamics.RigidBody}"><code>default_frame</code></a>, <a href="rigidbody.html#RigidBodyDynamics.frame_definition-Tuple{RigidBodyDynamics.RigidBody,RigidBodyDynamics.Spatial.CartesianFrame3D}"><code>frame_definition</code></a>.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L163">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.non_tree_joints-Tuple{RigidBodyDynamics.Mechanism}" href="#RigidBodyDynamics.non_tree_joints-Tuple{RigidBodyDynamics.Mechanism}"><code>RigidBodyDynamics.non_tree_joints</code></a> — <span class="docstring-category">Method</span>.</div><div><pre><code class="language-julia">non_tree_joints(mechanism)
</code></pre><p>Return the <code>Joint</code>s that are not part of the <code>Mechanism</code>'s spanning tree as an iterable collection.</p></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/611aaf052afe52969b1f4ce8ee81129969ee8864/src/mechanism.jl#L47">source</a></section><footer><hr/><a class="previous" href="rigidbody.html"><span class="direction">Previous</span><span class="title">Rigid bodies</span></a><a class="next" href="mechanismstate.html"><span class="direction">Next</span><span class="title">MechanismState</span></a></footer></article></body></html>