-
Notifications
You must be signed in to change notification settings - Fork 49
/
joints.html
27 lines (27 loc) · 40.6 KB
/
joints.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
<!DOCTYPE html>
<html lang="en"><head><meta charset="UTF-8"/><meta name="viewport" content="width=device-width, initial-scale=1.0"/><title>Joints · 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 class="current"><a class="toctext" href="joints.html">Joints</a><ul class="internal"><li><a class="toctext" href="#Index-1">Index</a></li><li><a class="toctext" href="#The-Joint-type-1">The <code>Joint</code> type</a></li><li><a class="toctext" href="#Functions-1">Functions</a></li><li><a class="toctext" href="#JointTypes-1"><code>JointType</code>s</a></li></ul></li><li><a class="toctext" href="rigidbody.html">Rigid bodies</a></li><li><a class="toctext" href="mechanism.html">Mechanism</a></li><li><a class="toctext" href="mechanismstate.html">MechanismState</a></li><li><a class="toctext" href="algorithms.html">Kinematics/dynamics algorithms</a></li><li><a class="toctext" href="customcollections.html">Custom collection types</a></li><li><a class="toctext" href="caches.html">Cache types</a></li><li><a class="toctext" href="simulation.html">Simulation</a></li><li><a class="toctext" href="benchmarks.html">Benchmarks</a></li></ul></nav><article id="docs"><header><nav><ul><li><a href="joints.html">Joints</a></li></ul><a class="edit-page" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/master/docs/src/joints.md"><span class="fa"></span> Edit on GitHub</a></nav><hr/><div id="topbar"><span>Joints</span><a class="fa fa-bars" href="#"></a></div></header><h1><a class="nav-anchor" id="Joints-1" href="#Joints-1">Joints</a></h1><h2><a class="nav-anchor" id="Index-1" href="#Index-1">Index</a></h2><ul><li><a href="joints.html#RigidBodyDynamics.Fixed"><code>RigidBodyDynamics.Fixed</code></a></li><li><a href="joints.html#RigidBodyDynamics.Joint"><code>RigidBodyDynamics.Joint</code></a></li><li><a href="joints.html#RigidBodyDynamics.Planar"><code>RigidBodyDynamics.Planar</code></a></li><li><a href="joints.html#RigidBodyDynamics.Prismatic"><code>RigidBodyDynamics.Prismatic</code></a></li><li><a href="joints.html#RigidBodyDynamics.QuaternionFloating"><code>RigidBodyDynamics.QuaternionFloating</code></a></li><li><a href="joints.html#RigidBodyDynamics.QuaternionSpherical"><code>RigidBodyDynamics.QuaternionSpherical</code></a></li><li><a href="joints.html#RigidBodyDynamics.Revolute"><code>RigidBodyDynamics.Revolute</code></a></li><li><a href="joints.html#RigidBodyDynamics.bias_acceleration-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.bias_acceleration</code></a></li><li><a href="joints.html#RigidBodyDynamics.configuration_derivative_to_velocity!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.configuration_derivative_to_velocity!</code></a></li><li><a href="joints.html#RigidBodyDynamics.configuration_derivative_to_velocity_adjoint!-Tuple{Any,Joint,AbstractArray{T,1} where T,Any}"><code>RigidBodyDynamics.configuration_derivative_to_velocity_adjoint!</code></a></li><li><a href="joints.html#RigidBodyDynamics.configuration_derivative_to_velocity_jacobian-Tuple{Joint,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.configuration_derivative_to_velocity_jacobian</code></a></li><li><a href="joints.html#RigidBodyDynamics.constraint_wrench_subspace-Tuple{Joint,Transform3D}"><code>RigidBodyDynamics.constraint_wrench_subspace</code></a></li><li><a href="joints.html#RigidBodyDynamics.effort_bounds-Tuple{Joint}"><code>RigidBodyDynamics.effort_bounds</code></a></li><li><a href="joints.html#RigidBodyDynamics.global_coordinates!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.global_coordinates!</code></a></li><li><a href="joints.html#RigidBodyDynamics.has_fixed_subspaces-Tuple{Joint}"><code>RigidBodyDynamics.has_fixed_subspaces</code></a></li><li><a href="joints.html#RigidBodyDynamics.isfloating-Tuple{Joint}"><code>RigidBodyDynamics.isfloating</code></a></li><li><a href="joints.html#RigidBodyDynamics.joint_spatial_acceleration-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.joint_spatial_acceleration</code></a></li><li><a href="joints.html#RigidBodyDynamics.joint_torque!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,Wrench}"><code>RigidBodyDynamics.joint_torque!</code></a></li><li><a href="joints.html#RigidBodyDynamics.joint_transform-Tuple{Joint,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.joint_transform</code></a></li><li><a href="joints.html#RigidBodyDynamics.joint_twist-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.joint_twist</code></a></li><li><a href="joints.html#RigidBodyDynamics.local_coordinates!-Tuple{AbstractArray{T,1} where T,AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.local_coordinates!</code></a></li><li><a href="joints.html#RigidBodyDynamics.motion_subspace-Tuple{Joint,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.motion_subspace</code></a></li><li><a href="joints.html#RigidBodyDynamics.normalize_configuration!-Tuple{AbstractArray{T,1} where T,Joint}"><code>RigidBodyDynamics.normalize_configuration!</code></a></li><li><a href="joints.html#RigidBodyDynamics.num_constraints-Tuple{Joint}"><code>RigidBodyDynamics.num_constraints</code></a></li><li><a href="joints.html#RigidBodyDynamics.num_positions-Tuple{Joint}"><code>RigidBodyDynamics.num_positions</code></a></li><li><a href="joints.html#RigidBodyDynamics.num_velocities-Tuple{Joint}"><code>RigidBodyDynamics.num_velocities</code></a></li><li><a href="joints.html#RigidBodyDynamics.position_bounds-Tuple{Joint}"><code>RigidBodyDynamics.position_bounds</code></a></li><li><a href="joints.html#RigidBodyDynamics.rand_configuration!-Tuple{AbstractArray{T,1} where T,Joint}"><code>RigidBodyDynamics.rand_configuration!</code></a></li><li><a href="joints.html#RigidBodyDynamics.velocity_bounds-Tuple{Joint}"><code>RigidBodyDynamics.velocity_bounds</code></a></li><li><a href="joints.html#RigidBodyDynamics.velocity_to_configuration_derivative!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.velocity_to_configuration_derivative!</code></a></li><li><a href="joints.html#RigidBodyDynamics.velocity_to_configuration_derivative_jacobian-Tuple{Joint,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.velocity_to_configuration_derivative_jacobian</code></a></li><li><a href="joints.html#RigidBodyDynamics.zero_configuration!-Tuple{AbstractArray{T,1} where T,Joint}"><code>RigidBodyDynamics.zero_configuration!</code></a></li></ul><h2><a class="nav-anchor" id="The-Joint-type-1" href="#The-Joint-type-1">The <code>Joint</code> type</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Joint" href="#RigidBodyDynamics.Joint"><code>RigidBodyDynamics.Joint</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct Joint{T, JT<:JointType{T}}</code></pre><p>A joint represents a kinematic restriction of the relative twist between two rigid bodies to a linear subspace of dimension <span>$k$</span>.</p><p>A joint has a direction. The rigid body before the joint is called the joint's predecessor, and the rigid body after the joint is its successor.</p><p>The state related to the joint is parameterized by two sets of variables, namely</p><ul><li>a vector <span>$q \in \mathcal{Q}$</span>, parameterizing the relative homogeneous transform.</li><li>a vector <span>$v \in \mathbb{R}^k$</span>, parameterizing the relative twist.</li></ul><p>The twist of the successor with respect to the predecessor is a linear function of <span>$v$</span>.</p><p>For some joint types (notably those using a redundant representation of relative orientation, such as a unit quaternion), <span>$\dot{q}$</span>, the time derivative of <span>$q$</span>, may not be the same as <span>$v$</span>. However, an invertible linear transformation exists between <span>$\dot{q}$</span> and <span>$v$</span>.</p><p>See also:</p><ul><li>Definition 2.9 in Duindam, "Port-Based Modeling and Control for Efficient Bipedal Walking Robots", 2006.</li><li>Section 4.4 of Featherstone, "Rigid Body Dynamics Algorithms", 2008.</li></ul></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L50">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.bias_acceleration-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.bias_acceleration-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.bias_acceleration</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">bias_acceleration(joint, q, v)
</code></pre><p>Return the acceleration of the joint's successor with respect to its predecessor in configuration <span>$q$</span> and at velocity <span>$v$</span>, when the joint acceleration <span>$\dot{v}$</span> is zero.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L263">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.configuration_derivative_to_velocity!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.configuration_derivative_to_velocity!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.configuration_derivative_to_velocity!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">configuration_derivative_to_velocity!(v, joint, q, q̇)
</code></pre><p>Compute joint velocity vector <span>$v$</span> given the joint configuration vector <span>$q$</span> and its time derivative <span>$\dot{q}$</span> (in place).</p><p>Note that this mapping is linear.</p><p>See also <a href="joints.html#RigidBodyDynamics.velocity_to_configuration_derivative!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>velocity_to_configuration_derivative!</code></a>, the inverse mapping.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L284">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.configuration_derivative_to_velocity_adjoint!-Tuple{Any,Joint,AbstractArray{T,1} where T,Any}" href="#RigidBodyDynamics.configuration_derivative_to_velocity_adjoint!-Tuple{Any,Joint,AbstractArray{T,1} where T,Any}"><code>RigidBodyDynamics.configuration_derivative_to_velocity_adjoint!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">configuration_derivative_to_velocity_adjoint!(fq, joint, q, fv)
</code></pre><p>Given a linear function</p><div>\[f(v) = \langle f_v, v \rangle\]</div><p>where <span>$v$</span> is the joint velocity vector, return a vector <span>$f_q$</span> such that</p><div>\[\langle f_v, v \rangle = \langle f_q, \dot{q}(v) \rangle.\]</div><p>Note: since <span>$v$</span> is a linear function of <span>$\dot{q}$</span> (see <a href="joints.html#RigidBodyDynamics.configuration_derivative_to_velocity!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>configuration_derivative_to_velocity!</code></a>), we can write <span>$v = J_{\dot{q} \rightarrow v} \dot{q}$</span>, so</p><div>\[\langle f_v, v \rangle = \langle f_v, J_{\dot{q} \rightarrow v} \dot{q} \rangle = \langle J_{\dot{q} \rightarrow v}^{*} f_v, \dot{q} \rangle\]</div><p>so <span>$f_q = J_{\dot{q} \rightarrow v}^{*} f_v$</span>.</p><p>To compute <span>$J_{\dot{q} \rightarrow v}$</span> see <a href="joints.html#RigidBodyDynamics.configuration_derivative_to_velocity_jacobian-Tuple{Joint,AbstractArray{T,1} where T}"><code>configuration_derivative_to_velocity_jacobian</code></a>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L301">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.constraint_wrench_subspace-Tuple{Joint,Transform3D}" href="#RigidBodyDynamics.constraint_wrench_subspace-Tuple{Joint,Transform3D}"><code>RigidBodyDynamics.constraint_wrench_subspace</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">constraint_wrench_subspace(joint, joint_transform)
</code></pre><p>Return a basis for the constraint wrench subspace of the joint, where <code>joint_transform</code> is the transform from the frame after the joint to the frame before the joint.</p><p>The constraint wrench subspace is a <span>$6 \times (6 - k)$</span> matrix, where <span>$k$</span> is the dimension of the velocity vector <span>$v$</span>, that maps a vector of Lagrange multipliers <span>$\lambda$</span> to the constraint wrench exerted across the joint onto its successor.</p><p>The constraint wrench subspace is orthogonal to the motion subspace.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L243">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.effort_bounds-Tuple{Joint}" href="#RigidBodyDynamics.effort_bounds-Tuple{Joint}"><code>RigidBodyDynamics.effort_bounds</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">effort_bounds(joint)
</code></pre><p>Return a <code>Vector{Bounds{T}}</code> giving the upper and lower bounds of the effort for <code>joint</code></p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L130">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.global_coordinates!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.global_coordinates!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.global_coordinates!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">global_coordinates!(q, joint, q0, ϕ)
</code></pre><p>Compute the global parameterization of the joint's configuration, <span>$q$</span>, given a 'base' orientation <span>$q_0$</span> and a vector of local coordinates <span>$ϕ$</span> centered around <span>$q_0$</span>.</p><p>See also <a href="joints.html#RigidBodyDynamics.local_coordinates!-Tuple{AbstractArray{T,1} where T,AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>local_coordinates!</code></a>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L469">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.has_fixed_subspaces-Tuple{Joint}" href="#RigidBodyDynamics.has_fixed_subspaces-Tuple{Joint}"><code>RigidBodyDynamics.has_fixed_subspaces</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">has_fixed_subspaces(joint)
</code></pre><p>Whether the joint's motion subspace and constraint wrench subspace depend on <span>$q$</span>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L276">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.isfloating-Tuple{Joint}" href="#RigidBodyDynamics.isfloating-Tuple{Joint}"><code>RigidBodyDynamics.isfloating</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">isfloating(joint)
</code></pre><p>Whether the joint is a floating joint, i.e., whether it imposes no constraints on the relative motions of its successor and predecessor bodies.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L485">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.joint_transform-Tuple{Joint,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.joint_transform-Tuple{Joint,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.joint_transform</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">joint_transform(joint, q)
</code></pre><p>Return a <code>Transform3D</code> representing the homogeneous transform from the frame after the joint to the frame before the joint for 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/joint.jl#L217">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.local_coordinates!-Tuple{AbstractArray{T,1} where T,AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.local_coordinates!-Tuple{AbstractArray{T,1} where T,AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.local_coordinates!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">local_coordinates!(ϕ, ϕ̇, joint, q0, q, v)
</code></pre><p>Compute a vector of local coordinates <span>$\phi$</span> around configuration <span>$q_0$</span> corresponding to configuration <span>$q$</span> (in place). Also compute the time derivative <span>$\dot{\phi}$</span> of <span>$\phi$</span> given the joint velocity vector <span>$v$</span>.</p><p>The local coordinate vector <span>$\phi$</span> must be zero if and only if <span>$q = q_0$</span>.</p><p>For revolute or prismatic joint types, the local coordinates can just be <span>$\phi = q - q_0$</span>, but for joint types with configuration vectors that are restricted to a manifold (e.g. when unit quaternions are used to represent orientation), elementwise subtraction may not make sense. For such joints, exponential coordinates could be used as the local coordinate vector <span>$\phi$</span>.</p><p>See also <a href="joints.html#RigidBodyDynamics.global_coordinates!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>global_coordinates!</code></a>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L442">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.motion_subspace-Tuple{Joint,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.motion_subspace-Tuple{Joint,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.motion_subspace</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">motion_subspace(joint, q)
</code></pre><p>Return a basis for the motion subspace of the joint in configuration <span>$q$</span>.</p><p>The motion subspace basis is a <span>$6 \times k$</span> matrix, where <span>$k$</span> is the dimension of the velocity vector <span>$v$</span>, that maps <span>$v$</span> to the twist of the joint's successor with respect to its predecessor. The returned motion subspace is expressed in the frame after the joint, which is attached to the joint's successor.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L228">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.normalize_configuration!-Tuple{AbstractArray{T,1} where T,Joint}" href="#RigidBodyDynamics.normalize_configuration!-Tuple{AbstractArray{T,1} where T,Joint}"><code>RigidBodyDynamics.normalize_configuration!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">normalize_configuration!(q, joint)
</code></pre><p>Renormalize the configuration vector <span>$q$</span> associated with <code>joint</code> so that it lies on the joint's configuration manifold.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L493">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_constraints-Tuple{Joint}" href="#RigidBodyDynamics.num_constraints-Tuple{Joint}"><code>RigidBodyDynamics.num_constraints</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">num_constraints(joint)
</code></pre><p>Return the number of constraints imposed on the relative twist between the joint's predecessor and successor</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L210">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_positions-Tuple{Joint}" href="#RigidBodyDynamics.num_positions-Tuple{Joint}"><code>RigidBodyDynamics.num_positions</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">num_positions(joint)
</code></pre><p>Return the length of the configuration vector of <code>joint</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L196">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.num_velocities-Tuple{Joint}" href="#RigidBodyDynamics.num_velocities-Tuple{Joint}"><code>RigidBodyDynamics.num_velocities</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">num_velocities(joint)
</code></pre><p>Return the length of the velocity vector of <code>joint</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L203">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.position_bounds-Tuple{Joint}" href="#RigidBodyDynamics.position_bounds-Tuple{Joint}"><code>RigidBodyDynamics.position_bounds</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">position_bounds(joint)
</code></pre><p>Return a <code>Vector{Bounds{T}}</code> giving the upper and lower bounds of the configuration for <code>joint</code></p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L114">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.rand_configuration!-Tuple{AbstractArray{T,1} where T,Joint}" href="#RigidBodyDynamics.rand_configuration!-Tuple{AbstractArray{T,1} where T,Joint}"><code>RigidBodyDynamics.rand_configuration!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">rand_configuration!(q, joint)
</code></pre><p>Set <span>$q$</span> to a random configuration. The distribution used depends on the joint type.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L390">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.velocity_bounds-Tuple{Joint}" href="#RigidBodyDynamics.velocity_bounds-Tuple{Joint}"><code>RigidBodyDynamics.velocity_bounds</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">velocity_bounds(joint)
</code></pre><p>Return a <code>Vector{Bounds{T}}</code> giving the upper and lower bounds of the velocity for <code>joint</code></p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L122">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.velocity_to_configuration_derivative!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.velocity_to_configuration_derivative!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.velocity_to_configuration_derivative!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">velocity_to_configuration_derivative!(q̇, joint, q, v)
</code></pre><p>Compute the time derivative <span>$\dot{q}$</span> of the joint configuration vector <span>$q$</span> given <span>$q$</span> and the joint velocity vector <span>$v$</span> (in place).</p><p>Note that this mapping is linear.</p><p>See also <a href="joints.html#RigidBodyDynamics.configuration_derivative_to_velocity!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>configuration_derivative_to_velocity!</code></a>, the inverse mapping.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L331">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.zero_configuration!-Tuple{AbstractArray{T,1} where T,Joint}" href="#RigidBodyDynamics.zero_configuration!-Tuple{AbstractArray{T,1} where T,Joint}"><code>RigidBodyDynamics.zero_configuration!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">zero_configuration!(q, joint)
</code></pre><p>Set <span>$q$</span> to the 'zero' configuration, corresponding to an identity joint transform.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L379">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.configuration_derivative_to_velocity_jacobian-Tuple{Joint,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.configuration_derivative_to_velocity_jacobian-Tuple{Joint,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.configuration_derivative_to_velocity_jacobian</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">configuration_derivative_to_velocity_jacobian(joint, q)
</code></pre><p>Compute the jacobian <span>$J_{\dot{q} \rightarrow v}$</span> which maps joint configuration derivative to velocity for the given joint:</p><div>\[v = J_{\dot{q} \rightarrow v} \dot{q}\]</div></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L364">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.joint_spatial_acceleration-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.joint_spatial_acceleration-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.joint_spatial_acceleration</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">joint_spatial_acceleration(joint, q, v, vd)
</code></pre><p>Return the spatial acceleration of <code>joint</code>'s successor with respect to its predecessor, expressed in the frame after the joint.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L415">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.joint_torque!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,Wrench}" href="#RigidBodyDynamics.joint_torque!-Tuple{AbstractArray{T,1} where T,Joint,AbstractArray{T,1} where T,Wrench}"><code>RigidBodyDynamics.joint_torque!</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">joint_torque!(τ, joint, q, joint_wrench)
</code></pre><p>Given the wrench exerted across the joint on the joint's successor, compute the vector of joint torques <span>$\tau$</span> (in place), in configuration <code>q</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L428">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.joint_twist-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.joint_twist-Tuple{Joint,AbstractArray{T,1} where T,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.joint_twist</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">joint_twist(joint, q, v)
</code></pre><p>Return the twist of <code>joint</code>'s successor with respect to its predecessor, expressed in the frame after the joint.</p><p>Note that this is the same as <code>Twist(motion_subspace(joint, q), v)</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L401">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.velocity_to_configuration_derivative_jacobian-Tuple{Joint,AbstractArray{T,1} where T}" href="#RigidBodyDynamics.velocity_to_configuration_derivative_jacobian-Tuple{Joint,AbstractArray{T,1} where T}"><code>RigidBodyDynamics.velocity_to_configuration_derivative_jacobian</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">velocity_to_configuration_derivative_jacobian(joint, q)
</code></pre><p>Compute the jacobian <span>$J_{v \rightarrow \dot{q}}$</span> which maps joint velocity to configuration derivative for the given joint:</p><div>\[\dot{q} = J_{v \rightarrow \dot{q}} v\]</div></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint.jl#L349">source</a></section><h2><a class="nav-anchor" id="JointTypes-1" href="#JointTypes-1"><code>JointType</code>s</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.QuaternionFloating" href="#RigidBodyDynamics.QuaternionFloating"><code>RigidBodyDynamics.QuaternionFloating</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct QuaternionFloating{T} <: JointType{T}</code></pre><p>A floating joint type that uses a unit quaternion representation for orientation.</p><p>Floating joints are 6-degree-of-freedom joints that are in a sense degenerate, as they impose no constraints on the relative motion between two bodies.</p><p>The full, 7-dimensional configuration vector of a <code>QuaternionFloating</code> joint type consists of a unit quaternion representing the orientation that rotates vectors from the frame 'directly after' the joint to the frame 'directly before' it, and a 3D position vector representing the origin of the frame after the joint in the frame before the joint.</p><p>The 6-dimensional velocity vector of a <code>QuaternionFloating</code> joint is the twist of the frame after the joint with respect to the frame before it, expressed in the frame after the joint.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint_types.jl#L1">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Revolute" href="#RigidBodyDynamics.Revolute"><code>RigidBodyDynamics.Revolute</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct Revolute{T} <: RigidBodyDynamics.OneDegreeOfFreedomFixedAxis{T}</code></pre><p>A <code>Revolute</code> joint type allows rotation about a fixed axis.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint_types.jl#L364">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Prismatic" href="#RigidBodyDynamics.Prismatic"><code>RigidBodyDynamics.Prismatic</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct Prismatic{T} <: RigidBodyDynamics.OneDegreeOfFreedomFixedAxis{T}</code></pre><p>A <code>Prismatic</code> joint type allows translation along a fixed axis.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint_types.jl#L294">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Fixed" href="#RigidBodyDynamics.Fixed"><code>RigidBodyDynamics.Fixed</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct Fixed{T} <: JointType{T}</code></pre><p>The <code>Fixed</code> joint type is a degenerate joint type, in the sense that it allows no motion between its predecessor and successor rigid bodies.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint_types.jl#L434">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.Planar" href="#RigidBodyDynamics.Planar"><code>RigidBodyDynamics.Planar</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct Planar{T} <: JointType{T}</code></pre><p>The <code>Planar</code> joint type allows translation along two orthogonal vectors, referred to as <span>$x$</span> and <span>$y$</span>, as well as rotation about an axis <span>$z = x \times y$</span>.</p><p>The components of the 3-dimensional configuration vector <span>$q$</span> associated with a <code>Planar</code> joint are the <span>$x$</span>- and <span>$y$</span>-coordinates of the translation, and the angle of rotation <span>$\theta$</span> about <span>$z$</span>, in that order.</p><p>The components of the 3-dimension velocity vector <span>$v$</span> associated with a <code>Planar</code> joint are the <span>$x$</span>- and <span>$y$</span>-coordinates of the linear part of the joint twist, expressed in the frame after the joint, followed by the <span>$z$</span>-component of the angular part of this joint twist.</p><div class="admonition warning"><div class="admonition-title">Warning</div><div class="admonition-text"><p>For the <code>Planar</code> joint type, <span>$v \neq \dot{q}$</span>! Although the angular parts of <span>$v$</span> and <span>$\dot{q}$</span> are the same, their linear parts differ. The linear part of <span>$v$</span> is the linear part of <span>$\dot{q}$</span>, rotated to the frame after the joint. This parameterization was chosen to allow the translational component of the joint transform to be independent of the rotation angle <span>$\theta$</span> (i.e., the rotation is applied <strong>after</strong> the translation), while still retaining a constant motion subspace expressed in the frame after the joint.</p></div></div></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint_types.jl#L504">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.QuaternionSpherical" href="#RigidBodyDynamics.QuaternionSpherical"><code>RigidBodyDynamics.QuaternionSpherical</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct QuaternionSpherical{T} <: JointType{T}</code></pre><p>The <code>QuaternionSpherical</code> joint type allows rotation in any direction. It is an implementation of a ball-and-socket joint.</p><p>The 4-dimensional configuration vector <span>$q$</span> associated with a <code>QuaternionSpherical</code> joint is the unit quaternion that describes the orientation of the frame after the joint with respect to the frame before the joint. In other words, it is the quaternion that can be used to rotate vectors from the frame after the joint to the frame before the joint.</p><p>The 3-dimensional velocity vector <span>$v$</span> associated with a <code>QuaternionSpherical</code> joint is the angular velocity of the frame after the joint with respect to the frame before the joint, expressed in the frame after the joint (body frame).</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/joint_types.jl#L658">source</a></section><footer><hr/><a class="previous" href="spatial.html"><span class="direction">Previous</span><span class="title">Spatial vector algebra</span></a><a class="next" href="rigidbody.html"><span class="direction">Next</span><span class="title">Rigid bodies</span></a></footer></article></body></html>