-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
semi_explicit_euler_integrator.h
165 lines (143 loc) · 6.93 KB
/
semi_explicit_euler_integrator.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
#pragma once
#include <utility>
#include "drake/common/drake_copyable.h"
#include "drake/systems/analysis/integrator_base.h"
namespace drake {
namespace systems {
/**
* A first-order, semi-explicit Euler integrator. State is updated in the
* following manner: <pre>
* v(t₀+h) = v(t₀) + dv/dt(t₀) * h
* dq/dt = N(q(t₀)) * v(t₀+h)
* q(t₀+h) = q(t₀) + dq/dt * h
* </pre>
* where `v` are the generalized velocity variables and `q` are generalized
* coordinates. `h` is the integration step size, and `N` is a matrix
* (dependent upon `q(t₀)`) that maps velocities to time derivatives of
* generalized coordinates. For rigid body systems in 2D, for example, `N`
* will generally be an identity matrix. For a single rigid body in 3D, `N` and
* its pseudo-inverse (`N` is generally non-square but always left invertible)
* are frequently used to transform between time derivatives of Euler
* parameters (unit quaternions) and angular velocities (and vice versa),
* [Nikravesh 1988].
*
* Note that these equations imply that the velocity variables are updated
* first and that these new velocities are then used to update the generalized
* coordinates (compare to ExplicitEulerIntegrator, where the generalized
* coordinates are updated using the previous velocity variables).
*
* When a mechanical system is Hamiltonian (informally meaning that the
* system is not subject to velocity-dependent forces), the semi-explicit
* Euler integrator is a symplectic (energy conserving) integrator.
* Symplectic integrators advertise energetically consistent behavior with large
* step sizes compared to non-symplectic integrators. Multi-body systems
* are not Hamiltonian, even in the absence of externally applied
* velocity-dependent forces, due to the presence of both Coriolis and
* gyroscopic forces. This integrator thus does not generally conserve energy
* for such systems.
*
* <h4>Association between time stepping and the semi-explicit Euler
* integrator:</h4>
* Though many time stepping approaches use the formulations above, these
* equations do not represent a "time stepping scheme". The semi-explicit
* Euler integration equations can be applied from one point in state space to
* another, assuming smoothness in between, just like any other integrator using
* the following process:
* (1) a simulator integrates to discontinuities, (2) the state of the ODE/DAE
* is re-initialized, and (3) integration continues.
*
* In contrast, time stepping schemes enforce all constraints at a single
* time in the integration process: though a billiard break may consist of tens
* of collisions occurring sequentially over a millisecond of time, a time
* stepping method will treat all of these collisions as occurring
* simultaneously.
*
* - [Nikravesh 1988] P. Nikravesh. Computer-Aided Analysis of Mechanical
* Systems. Prentice Hall. New Jersey, 1988.
* - [Stewart 2000] D. Stewart. Rigid-body Dynamics with Friction and
* Impact. SIAM Review, 42:1, 2000.
*/
template <class T>
class SemiExplicitEulerIntegrator final : public IntegratorBase<T> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SemiExplicitEulerIntegrator)
virtual ~SemiExplicitEulerIntegrator() {}
// TODO(edrumwri): update documentation to account for stretching (after
// stretching has become a user settable).
/**
* Constructs a fixed-step integrator for a given system using the given
* context for initial conditions.
* @param system A reference to the system to be simulated.
* @param max_step_size The maximum (fixed) step size; the integrator will
* not take larger step sizes than this.
* @param context Pointer to the context (nullptr is ok, but the caller
* must set a non-null context before Initialize()-ing the
* integrator).
* @sa Initialize()
*/
SemiExplicitEulerIntegrator(const System<T>& system, const T& max_step_size,
Context<T>* context = nullptr)
: IntegratorBase<T>(system, context),
qdot_(context->get_continuous_state().num_q()) {
IntegratorBase<T>::set_maximum_step_size(max_step_size);
}
/**
* Gets the error estimate order (returns zero, since error estimation is
* not provided).
*/
int get_error_estimate_order() const override { return 0; }
/**
* Integrator does not support accuracy estimation.
*/
bool supports_error_estimation() const override { return false; }
private:
bool DoStep(const T& h) override;
// This is a pre-allocated temporary for use by integration
BasicVector<T> qdot_;
};
/**
* Integrates the system forward in time by h. This value is determined
* by IntegratorBase::StepOnce().
*/
template <class T>
bool SemiExplicitEulerIntegrator<T>::DoStep(const T& h) {
const System<T>& system = this->get_system();
Context<T>& context = *this->get_mutable_context();
// CAUTION: This is performance-sensitive inner loop code that uses dangerous
// long-lived references into state and cache to avoid unnecessary copying and
// cache invalidation. Be careful not to insert calls to methods that could
// invalidate any of these references before they are used.
// Evaluate derivative xcdot(t₀) ← xcdot(t₀, x(t₀), u(t₀)).
const ContinuousState<T>& xc_deriv = this->EvalTimeDerivatives(context);
// Retrieve the accelerations and auxiliary variable derivatives.
const VectorBase<T>& vdot = xc_deriv.get_generalized_velocity();
const VectorBase<T>& zdot = xc_deriv.get_misc_continuous_state();
// Cache: vdot and zdot reference the live derivative cache value, currently
// up to date but about to be marked out of date. We do not want to make
// an unnecessary copy of this data.
// This invalidates computations that are dependent on v or z.
// Marks v- and z-dependent cache entries out of date, including vdot and
// zdot; time doesn't change here.
std::pair<VectorBase<T>*, VectorBase<T>*> vz = context.GetMutableVZVectors();
VectorBase<T>& v = *vz.first;
VectorBase<T>& z = *vz.second;
// Cache: vdot and zdot still reference the derivative cache value, which is
// unchanged, although it is marked out of date.
// Update the velocity and auxiliary state variables.
v.PlusEqScaled(h, vdot);
z.PlusEqScaled(h, zdot);
// Convert the updated generalized velocity to the time derivative of
// generalized coordinates. Note that this mapping is q-dependent and
// hasn't been invalidated if it was pre-computed.
system.MapVelocityToQDot(context, v, &qdot_);
// Now set time and q to their final values. This marks time- and
// q-dependent cache entries out of date. That includes the derivative
// cache entry though we don't need it again here.
VectorBase<T>& q =
context.SetTimeAndGetMutableQVector(context.get_time() + h);
q.PlusEqScaled(h, qdot_);
// This integrator always succeeds at taking the step.
return true;
}
} // namespace systems
} // namespace drake