-
Notifications
You must be signed in to change notification settings - Fork 49
/
simulation.html
8 lines (8 loc) · 14.9 KB
/
simulation.html
1
2
3
4
5
6
7
8
<!DOCTYPE html>
<html lang="en"><head><meta charset="UTF-8"/><meta name="viewport" content="width=device-width, initial-scale=1.0"/><title>Simulation · RigidBodyDynamics.jl</title><link href="https://cdnjs.cloudflare.com/ajax/libs/normalize/4.2.0/normalize.min.css" rel="stylesheet" type="text/css"/><link href="https://fonts.googleapis.com/css?family=Lato|Roboto+Mono" rel="stylesheet" type="text/css"/><link href="https://cdnjs.cloudflare.com/ajax/libs/font-awesome/4.6.3/css/font-awesome.min.css" rel="stylesheet" type="text/css"/><link href="https://cdnjs.cloudflare.com/ajax/libs/highlight.js/9.12.0/styles/default.min.css" rel="stylesheet" type="text/css"/><script>documenterBaseURL="."</script><script src="https://cdnjs.cloudflare.com/ajax/libs/require.js/2.2.0/require.min.js" data-main="assets/documenter.js"></script><script src="siteinfo.js"></script><script src="../versions.js"></script><link href="assets/documenter.css" rel="stylesheet" type="text/css"/></head><body><nav class="toc"><h1>RigidBodyDynamics.jl</h1><select id="version-selector" onChange="window.location.href=this.value" style="visibility: hidden"></select><form class="search" id="search-form" action="search.html"><input id="search-query" name="q" type="text" placeholder="Search docs"/></form><ul><li><a class="toctext" href="index.html">Home</a></li><li><a class="toctext" href="quickstart.html">Quick start guide</a></li><li><a class="toctext" href="spatial.html">Spatial vector algebra</a></li><li><a class="toctext" href="joints.html">Joints</a></li><li><a class="toctext" href="rigidbody.html">Rigid bodies</a></li><li><a class="toctext" href="mechanism.html">Mechanism</a></li><li><a class="toctext" href="mechanismstate.html">MechanismState</a></li><li><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 class="current"><a class="toctext" href="simulation.html">Simulation</a><ul class="internal"><li><a class="toctext" href="#Index-1">Index</a></li><li><a class="toctext" href="#Basic-simulation-1">Basic simulation</a></li><li><a class="toctext" href="#Lower-level-ODE-integration-interface-1">Lower level ODE integration interface</a></li></ul></li><li><a class="toctext" href="benchmarks.html">Benchmarks</a></li></ul></nav><article id="docs"><header><nav><ul><li><a href="simulation.html">Simulation</a></li></ul><a class="edit-page" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/master/docs/src/simulation.md"><span class="fa"></span> Edit on GitHub</a></nav><hr/><div id="topbar"><span>Simulation</span><a class="fa fa-bars" href="#"></a></div></header><h1><a class="nav-anchor" id="Simulation-1" href="#Simulation-1">Simulation</a></h1><h2><a class="nav-anchor" id="Index-1" href="#Index-1">Index</a></h2><ul><li><a href="simulation.html#RigidBodyDynamics.OdeIntegrators.ButcherTableau"><code>RigidBodyDynamics.OdeIntegrators.ButcherTableau</code></a></li><li><a href="simulation.html#RigidBodyDynamics.OdeIntegrators.ExpandingStorage"><code>RigidBodyDynamics.OdeIntegrators.ExpandingStorage</code></a></li><li><a href="simulation.html#RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator"><code>RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator</code></a></li><li><a href="simulation.html#RigidBodyDynamics.OdeIntegrators.OdeResultsSink"><code>RigidBodyDynamics.OdeIntegrators.OdeResultsSink</code></a></li><li><a href="simulation.html#RigidBodyDynamics.OdeIntegrators.RingBufferStorage"><code>RigidBodyDynamics.OdeIntegrators.RingBufferStorage</code></a></li><li><a href="simulation.html#RigidBodyDynamics.OdeIntegrators.integrate-Tuple{MuntheKaasIntegrator,Any,Any}"><code>RigidBodyDynamics.OdeIntegrators.integrate</code></a></li><li><a href="simulation.html#RigidBodyDynamics.OdeIntegrators.runge_kutta_4-Union{Tuple{Type{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.OdeIntegrators.runge_kutta_4</code></a></li><li><a href="simulation.html#RigidBodyDynamics.OdeIntegrators.step-Tuple{MuntheKaasIntegrator,Real,Real}"><code>RigidBodyDynamics.OdeIntegrators.step</code></a></li><li><a href="simulation.html#RigidBodyDynamics.simulate"><code>RigidBodyDynamics.simulate</code></a></li></ul><h2><a class="nav-anchor" id="Basic-simulation-1" href="#Basic-simulation-1">Basic simulation</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.simulate" href="#RigidBodyDynamics.simulate"><code>RigidBodyDynamics.simulate</code></a> — <span class="docstring-category">Function</span>.</div><div><div><pre><code class="language-julia">simulate(state0, final_time)
simulate(state0, final_time, control!; Δt, stabilization_gains)
</code></pre><p>Basic <code>Mechanism</code> simulation: integrate the state from time <span>$0$</span> to <code>final_time</code> starting from the initial state <code>state0</code>. Return a <code>Vector</code> of times, as well as <code>Vector</code>s of configuration vectors and velocity vectors at these times.</p><p>Optionally, a function (or other callable) can be passed in as the third argument (<code>control!</code>). <code>control!</code> will be called at each time step of the simulation and allows you to specify joint torques given the time and the state of the <code>Mechanism</code>. It should look like this:</p><pre><code class="language-julia">function control!(torques::AbstractVector, t, state::MechanismState)
rand!(torques) # for example
end</code></pre><p>The integration time step can be specified using the <code>Δt</code> keyword argument (defaults to <code>1e-4</code>).</p><p>The <code>stabilization_gains</code> keyword argument can be used to set PD gains for Baumgarte stabilization, which can be used to prevent separation of non-tree (loop) joints. See Featherstone (2008), section 8.3 for more information. There are several options for specifying gains:</p><ul><li><code>nothing</code> can be used to completely disable Baumgarte stabilization.</li><li>Gains can be specifed on a per-joint basis using any <code>AbstractDict{JointID, <:RigidBodyDynamics.PDControl.SE3PDGains}</code>, which maps the <code>JointID</code> for the non-tree joints of the mechanism to the gains for that joint.</li><li>As a special case of the second option, the same gains can be used for all joints by passing in a <code>RigidBodyDynamics.CustomCollections.ConstDict{JointID}</code>.</li></ul><p>The <a href="algorithms.html#RigidBodyDynamics.default_constraint_stabilization_gains-Union{Tuple{Type{T}}, Tuple{T}} where T"><code>default_constraint_stabilization_gains</code></a> function is called to produce the default gains, which use the last option.</p><p>Uses <code>MuntheKaasIntegrator</code>. See <a href="simulation.html#RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator"><code>RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator</code></a> for a lower level interface with more options.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/simulate.jl#L12-L26">source</a></section><h2><a class="nav-anchor" id="Lower-level-ODE-integration-interface-1" href="#Lower-level-ODE-integration-interface-1">Lower level ODE integration interface</a></h2><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator" href="#RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator"><code>RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator</code></a> — <span class="docstring-category">Type</span>.</div><div><div><p>A Lie-group-aware ODE integrator.</p><p><code>MuntheKaasIntegrator</code> is used to properly integrate the dynamics of globally parameterized rigid joints (Duindam, Port-Based Modeling and Control for Efficient Bipedal Walking Robots, 2006, Definition 2.9). Global parameterizations of e.g. <span>$SO(3)$</span> are needed to avoid singularities, but this leads to the problem that the tangent space no longer has the same dimension as the ambient space of the global parameterization. A Munthe-Kaas integrator solves this problem by converting back and forth between local and global coordinates at every integration time step.</p><p>The idea is to do the dynamics and compute the stages of the integration scheme in terms of local coordinates centered around the global parameterization of the configuration at the end of the previous time step (e.g. exponential coordinates), combine the stages into a new set of local coordinates as usual for Runge-Kutta methods, and then convert the local coordinates back to global coordinates.</p><p>From <a href="https://hal.archives-ouvertes.fr/hal-01328729">Iserles et al., 'Lie-group methods' (2000)</a>.</p><p>Another useful reference is <a href="http://www.ent.mrt.ac.lk/iml/paperbase/TRO%20Collection/TRO/2005/october/7.pdf">Park and Chung, 'Geometric Integration on Euclidean Group with Application to Articulated Multibody Systems' (2005)</a>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/ode_integrators.jl#L169-L189">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.OdeIntegrators.ButcherTableau" href="#RigidBodyDynamics.OdeIntegrators.ButcherTableau"><code>RigidBodyDynamics.OdeIntegrators.ButcherTableau</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">struct ButcherTableau{N, T, L}</code></pre><p>A <a href="https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods#Explicit_Runge.E2.80.93Kutta_methods">Butcher tableau</a>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/ode_integrators.jl#L18">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.OdeIntegrators.OdeResultsSink" href="#RigidBodyDynamics.OdeIntegrators.OdeResultsSink"><code>RigidBodyDynamics.OdeIntegrators.OdeResultsSink</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">abstract type OdeResultsSink</code></pre><p>Does 'something' with the results of an ODE integration (e.g. storing results, visualizing, etc.). Subtypes must implement:</p><ul><li><code>initialize(sink, state)</code>: called with the initial state when integration begins.</li><li><code>process(sink, t, state)</code>: called at every integration time step with the current state and time.</li></ul></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/ode_integrators.jl#L57">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.OdeIntegrators.RingBufferStorage" href="#RigidBodyDynamics.OdeIntegrators.RingBufferStorage"><code>RigidBodyDynamics.OdeIntegrators.RingBufferStorage</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">mutable struct RingBufferStorage{T, Q<:(AbstractArray{T,1} where T), V<:(AbstractArray{T,1} where T)} <: OdeResultsSink</code></pre><p>An <code>OdeResultsSink</code> that stores the state at each integration time step in a ring buffer.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/ode_integrators.jl#L69">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.OdeIntegrators.ExpandingStorage" href="#RigidBodyDynamics.OdeIntegrators.ExpandingStorage"><code>RigidBodyDynamics.OdeIntegrators.ExpandingStorage</code></a> — <span class="docstring-category">Type</span>.</div><div><div><pre><code class="language-julia">mutable struct ExpandingStorage{T, Q<:(AbstractArray{T,1} where T), V<:(AbstractArray{T,1} where T)} <: OdeResultsSink</code></pre><p>An <code>OdeResultsSink</code> that stores the state at each integration time step in <code>Vectors</code> that may expand.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/ode_integrators.jl#L106">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.OdeIntegrators.integrate-Tuple{MuntheKaasIntegrator,Any,Any}" href="#RigidBodyDynamics.OdeIntegrators.integrate-Tuple{MuntheKaasIntegrator,Any,Any}"><code>RigidBodyDynamics.OdeIntegrators.integrate</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">integrate(integrator, final_time, Δt; max_realtime_rate)
</code></pre><p>Integrate dynamics from the initial state at time <span>$0$</span> to <code>final_time</code> using step size <code>Δt</code>.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/ode_integrators.jl#L297">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.OdeIntegrators.runge_kutta_4-Union{Tuple{Type{T}}, Tuple{T}} where T" href="#RigidBodyDynamics.OdeIntegrators.runge_kutta_4-Union{Tuple{Type{T}}, Tuple{T}} where T"><code>RigidBodyDynamics.OdeIntegrators.runge_kutta_4</code></a> — <span class="docstring-category">Method</span>.</div><div><div><p>Return the Butcher tableau for the standard fourth order Runge-Kutta integrator.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/ode_integrators.jl#L43">source</a></section><section class="docstring"><div class="docstring-header"><a class="docstring-binding" id="RigidBodyDynamics.OdeIntegrators.step-Tuple{MuntheKaasIntegrator,Real,Real}" href="#RigidBodyDynamics.OdeIntegrators.step-Tuple{MuntheKaasIntegrator,Real,Real}"><code>RigidBodyDynamics.OdeIntegrators.step</code></a> — <span class="docstring-category">Method</span>.</div><div><div><pre><code class="language-julia">step(integrator, t, Δt)
</code></pre><p>Take a single integration step.</p></div></div><a class="source-link" target="_blank" href="https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/76c23d59410d62be3d363142875ea9c8c1eea909/src/ode_integrators.jl#L224">source</a></section><footer><hr/><a class="previous" href="caches.html"><span class="direction">Previous</span><span class="title">Cache types</span></a><a class="next" href="benchmarks.html"><span class="direction">Next</span><span class="title">Benchmarks</span></a></footer></article></body></html>