dynamics_solver.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019, Wolfgang Merkt
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
31 #include <exotica_core/scene.h>
32 
33 namespace exotica
34 {
35 template <typename T, int NX, int NU>
37 
38 template <typename T, int NX, int NU>
40 
41 template <typename T, int NX, int NU>
43 {
45  DynamicsSolverInitializer dynamics_solver_initializer = DynamicsSolverInitializer(init);
46  this->SetDt(dynamics_solver_initializer.dt);
47  this->SetIntegrator(dynamics_solver_initializer.Integrator);
48 
49  // Just store the control limits supplied.
50  // They will need to be reshaped to the correct size when num_velocities_ becomes known.
51  raw_control_limits_low_ = dynamics_solver_initializer.ControlLimitsLow;
52  raw_control_limits_high_ = dynamics_solver_initializer.ControlLimitsHigh;
53 
54  if (debug_) INFO_NAMED(object_name_, "Initialized DynamicsSolver of type " << GetObjectName() << " with dt=" << dynamics_solver_initializer.dt << " and integrator=" << dynamics_solver_initializer.Integrator);
55 }
56 
57 template <typename T, int NX, int NU>
58 void AbstractDynamicsSolver<T, NX, NU>::AssignScene(std::shared_ptr<Scene> scene_in)
59 {
60 }
61 
62 template <typename T, int NX, int NU>
63 Eigen::Matrix<T, NX, 1> AbstractDynamicsSolver<T, NX, NU>::F(const StateVector& x, const ControlVector& u)
64 {
65  // TODO: Switch to Integrate here...
66  return SimulateOneStep(x, u); // ToDo: Fold SimulateOneStep into here
67 }
68 
69 template <typename T, int NX, int NU>
71 {
72  if (dt_in < 0.0001) ThrowPretty("dt needs to be strictly greater than 0. Provided: " << dt_in);
73  dt_ = dt_in;
74 }
75 
76 template <typename T, int NX, int NU>
78 {
79  switch (integrator_)
80  {
81  // Forward Euler (RK1), symplectic Euler
82  case Integrator::RK1:
84  {
85  StateVector xdot = f(x, u);
86  StateVector xout(get_num_state());
87  Integrate(x, xdot, dt_, xout);
88  return xout;
89  }
90  // NB: RK2 and RK4 are currently deactivated as we do not yet have correct derivatives for state transitions.
91  /*// Explicit trapezoid rule (RK2)
92  case Integrator::RK2:
93  {
94  assert(num_positions_ == num_velocities_); // If this is not true, we should have specialised methods.
95 
96  StateVector xdot0 = f(x, u);
97  StateVector x1est = x + dt_ * xdot0; // explicit Euler step
98  StateVector xdot1 = f(x1est, u);
99 
100  // 2nd order result: x = x0 + dt (xd0+xd1)/2.
101  return x + (dt_ / 2.) * (xdot0 + xdot1);
102  }
103  // Runge-Kutta 4
104  case Integrator::RK4:
105  {
106  assert(num_positions_ == num_velocities_); // If this is not true, we should have specialised methods.
107 
108  StateVector k1 = dt_ * f(x, u);
109  StateVector k2 = dt_ * f(x + 0.5 * k1, u);
110  StateVector k3 = dt_ * f(x + 0.5 * k2, u);
111  StateVector k4 = dt_ * f(x + k3, u);
112  StateVector dx = (k1 + k4) / 6. + (k2 + k3) / 3.;
113 
114  return x + dx;
115  }*/
116  default:
117  ThrowPretty("Not implemented!");
118  };
119 }
120 
121 template <typename T, int NX, int NU>
122 void AbstractDynamicsSolver<T, NX, NU>::Integrate(const StateVector& x, const StateVector& dx, const double dt, StateVector& xout)
123 {
124  assert(num_positions_ == num_velocities_); // Integration on manifolds needs to be handled using function overloads in specific dynamics solvers.
125  assert(x.size() == get_num_state());
126  assert(dx.size() == get_num_state_derivative());
127  assert(xout.size() == get_num_state());
128  if (dt < 1e-6) ThrowPretty("dt needs to be positive!");
129 
130  switch (integrator_)
131  {
132  // Forward Euler (RK1)
133  case Integrator::RK1:
134  {
135  xout.noalias() = x + dt * dx;
136  }
137  break;
138 
139  // Semi-implicit Euler
141  {
142  StateVector dx_new(get_num_state_derivative());
143  dx_new.head(num_positions_) = dt * x.tail(num_velocities_) + (dt * dt) * dx.tail(num_velocities_);
144  dx_new.tail(num_velocities_) = dt * dx.tail(num_velocities_);
145  xout.noalias() = x + dx_new;
146 
147  // xout.tail(num_velocities_).noalias() = x.tail(num_velocities_) + dt * dx.tail(num_velocities_); // Integrate acceleration to velocity
148  // xout.head(num_positions_).noalias() = x.head(num_positions_) + dt * xout.tail(num_velocities_); // Integrate position with new velocity
149  }
150  break;
151 
152  default:
153  ThrowPretty("Not implemented!");
154  // TODO implement the other solvers, but how to get dx updated?!
155  };
156 }
157 
158 template <typename T, int NX, int NU>
159 Eigen::Matrix<T, NX, 1> AbstractDynamicsSolver<T, NX, NU>::Simulate(const StateVector& x, const ControlVector& u, T t)
160 {
161  const int num_timesteps = static_cast<int>(t / dt_);
162  StateVector x_t_plus_1 = x;
163  for (int i = 0; i < num_timesteps; ++i)
164  {
165  x_t_plus_1 = SimulateOneStep(x_t_plus_1, u);
166  }
167  return x_t_plus_1;
168 }
169 
170 template <typename T, int NX, int NU>
172 {
173  assert(x_in.size() == get_num_state());
174  return x_in.head(num_positions_).eval();
175 }
176 
177 template <typename T, int NX, int NU>
179 {
180  return num_controls_;
181 }
182 
183 template <typename T, int NX, int NU>
185 {
186  return num_positions_;
187 }
188 
189 template <typename T, int NX, int NU>
191 {
192  return num_velocities_;
193 }
194 
195 template <typename T, int NX, int NU>
197 {
198  if (num_state_ == -1)
199  return num_positions_ + num_velocities_;
200  else
201  return num_state_;
202 }
203 
204 template <typename T, int NX, int NU>
206 {
207  if (num_state_derivative_ == -1)
208  return 2 * num_velocities_;
209  else
210  return num_state_derivative_;
211 }
212 
213 template <typename T, int NX, int NU>
215 {
216  return dt_;
217 }
218 
219 template <typename T, int NX, int NU>
221 {
222  return integrator_;
223 }
224 
225 template <typename T, int NX, int NU>
227 {
228  integrator_ = integrator_in;
229 }
230 
231 template <typename T, int NX, int NU>
232 void AbstractDynamicsSolver<T, NX, NU>::SetIntegrator(const std::string& integrator_in)
233 {
234  if (integrator_in == "RK1")
235  integrator_ = Integrator::RK1;
236  else if (integrator_in == "SymplecticEuler")
237  integrator_ = Integrator::SymplecticEuler;
238  else if (integrator_in == "RK2")
239  integrator_ = Integrator::RK2;
240  else if (integrator_in == "RK4")
241  integrator_ = Integrator::RK4;
242  else
243  ThrowPretty("Unknown integrator: " << integrator_in);
244 }
245 
246 template <typename T, int NX, int NU>
248 {
249  if (!control_limits_initialized_)
250  set_control_limits(raw_control_limits_low_, raw_control_limits_high_);
251  return control_limits_;
252 }
253 
254 template <typename T, int NX, int NU>
256 {
257  if (num_controls_ == -1)
258  ThrowPretty("Attempting to set control limits before num_controls is set.");
259 
260  control_limits_initialized_ = true;
261  control_limits_ = Eigen::MatrixXd(num_controls_, 2);
262 
263  if (control_limits_low.size() == num_controls_)
264  control_limits_.col(0) = control_limits_low;
265  else if (control_limits_low.size() == 1)
266  control_limits_.col(0) = Eigen::VectorXd::Constant(num_controls_, control_limits_low(0));
267  else
268  ThrowPretty("Wrong control limits (low) size. Should either be 1 or " << num_controls_);
269 
270  if (control_limits_high.size() == num_controls_)
271  control_limits_.col(1) = control_limits_high;
272  else if (control_limits_high.size() == 1)
273  control_limits_.col(1) = Eigen::VectorXd::Constant(num_controls_, control_limits_high(0));
274  else
275  ThrowPretty("Wrong control limits (high) size. Should either be 1 or " << num_controls_);
276 }
277 
278 template <typename T, int NX, int NU>
279 void AbstractDynamicsSolver<T, NX, NU>::ClampToStateLimits(Eigen::Ref<Eigen::VectorXd> state_in)
280 {
281  if (!has_state_limits_) ThrowPretty("No StateLimits!");
282  if (state_in.size() != get_num_state()) ThrowPretty("Wrong size state passed in!");
283  assert(state_in.size() == state_limits_lower_.size());
284  assert(state_limits_lower_.size() == state_limits_upper_.size());
285 
286  state_in = state_in.cwiseMax(state_limits_lower_).cwiseMin(state_limits_upper_);
287 }
288 
289 template <typename T, int NX, int NU>
291 {
292  if (second_order_derivatives_initialized_)
293  return;
294 
295  const int ndx = get_num_state_derivative();
296 
297  fxx_default_ = Eigen::Tensor<T, 3>(ndx, ndx, ndx);
298  fxx_default_.setZero();
299 
300  fuu_default_ = Eigen::Tensor<T, 3>(ndx, num_controls_, num_controls_);
301  fuu_default_.setZero();
302 
303  fxu_default_ = Eigen::Tensor<T, 3>(ndx, ndx, num_controls_);
304  fxu_default_.setZero();
305 
306  dStateDelta_.setIdentity(get_num_state_derivative(), get_num_state_derivative());
307  ddStateDelta_.setConstant(get_num_state_derivative(), Eigen::MatrixXd::Zero(get_num_state_derivative(), get_num_state_derivative()));
308 
309  second_order_derivatives_initialized_ = true;
310 }
311 
312 template <typename T, int NX, int NU>
313 Eigen::Tensor<T, 3> AbstractDynamicsSolver<T, NX, NU>::fxx(const StateVector& x, const ControlVector& u)
314 {
315  if (!second_order_derivatives_initialized_) InitializeSecondOrderDerivatives();
316  return fxx_default_;
317 }
318 
319 template <typename T, int NX, int NU>
320 Eigen::Tensor<T, 3> AbstractDynamicsSolver<T, NX, NU>::fuu(const StateVector& x, const ControlVector& u)
321 {
322  if (!second_order_derivatives_initialized_) InitializeSecondOrderDerivatives();
323  return fuu_default_;
324 }
325 
326 template <typename T, int NX, int NU>
327 Eigen::Tensor<T, 3> AbstractDynamicsSolver<T, NX, NU>::fxu(const StateVector& x, const ControlVector& u)
328 {
329  if (!second_order_derivatives_initialized_) InitializeSecondOrderDerivatives();
330  return fxu_default_;
331 }
332 
333 template <typename T, int NX, int NU>
335 {
336  ThrowPretty("This dynamics solver does not support inverse dynamics!");
337 }
338 
339 template <typename T, int NX, int NU>
340 Eigen::Matrix<T, NX, NX> AbstractDynamicsSolver<T, NX, NU>::fx_fd(const StateVector& x, const ControlVector& u)
341 {
342  const int nx = get_num_state();
343  const int ndx = get_num_state_derivative();
344 
345  // This finite differencing only works with RK1 due to Integrate(x, u, dt)
346  // We thus store the previous Integrator, set it to RK1, then set it back
347  // afterwards.
348  Integrator previous_integrator = integrator_;
349  integrator_ = Integrator::RK1; // Note, this by-passes potentially overriden virtual set_integrator callbacks
350 
351  // Finite differences
352  Eigen::MatrixXd fx_fd(ndx, ndx);
353  constexpr double eps = 1e-6;
354  Eigen::VectorXd x_low(nx), x_high(nx), xdiff(ndx);
355  for (int i = 0; i < ndx; ++i)
356  {
357  xdiff.setZero();
358  xdiff(i) = eps / 2.0;
359 
360  Integrate(x, -xdiff, 1., x_low);
361  Integrate(x, xdiff, 1., x_high);
362 
363  fx_fd.col(i) = (f(x_high, u) - f(x_low, u)) / eps;
364  }
365 
366  // Reset integrator
367  integrator_ = previous_integrator;
368 
369  return fx_fd;
370 }
371 
372 template <typename T, int NX, int NU>
373 Eigen::Matrix<T, NX, NX> AbstractDynamicsSolver<T, NX, NU>::fx(const StateVector& x, const ControlVector& u)
374 {
375  return fx_fd(x, u);
376 }
377 
378 template <typename T, int NX, int NU>
379 Eigen::Matrix<T, NX, NU> AbstractDynamicsSolver<T, NX, NU>::fu_fd(const StateVector& x, const ControlVector& u)
380 {
381  const int ndx = get_num_state_derivative();
382 
383  // Finite differences
384  constexpr double eps = 1e-6;
385  Eigen::MatrixXd fu_fd(ndx, num_controls_);
386  Eigen::VectorXd u_low(num_controls_), u_high(num_controls_);
387  for (int i = 0; i < num_controls_; ++i)
388  {
389  u_low = u;
390  u_high = u;
391  u_low(i) -= eps / 2.0;
392  u_high(i) += eps / 2.0;
393 
394  fu_fd.col(i) = (f(x, u_high) - f(x, u_low)) / eps;
395  }
396 
397  return fu_fd;
398 }
399 
400 template <typename T, int NX, int NU>
401 Eigen::Matrix<T, NX, NU> AbstractDynamicsSolver<T, NX, NU>::fu(const StateVector& x, const ControlVector& u)
402 {
403  return fu_fd(x, u);
404 }
405 
406 template <typename T, int NX, int NU>
408 {
409  // Compute derivatives of differential dynamics
410  fx_ = fx(x, u);
411  fu_ = fu(x, u);
412 
413  // Compute derivatives of state transition function
414  // NB: In our "f" order, we have both velocity and acceleration. We only need the acceleration derivative part:
415  Eigen::Block<Eigen::MatrixXd> da_dx = fx_.block(num_velocities_, 0, num_velocities_, get_num_state_derivative());
416  Eigen::Block<Eigen::MatrixXd> da_du = fu_.block(num_velocities_, 0, num_velocities_, num_controls_);
417 
418  // TODO: Do this only once...
419  Fx_.setZero(get_num_state_derivative(), get_num_state_derivative());
420  Fu_.setZero(get_num_state_derivative(), get_num_controls());
421 
422  switch (integrator_)
423  {
424  // Forward Euler (RK1)
425  case Integrator::RK1:
426  {
427  Fx_.topRightCorner(num_velocities_, num_velocities_).diagonal().array() = dt_;
428  Fx_.bottomRows(num_velocities_).noalias() = dt_ * da_dx;
429  Fx_.diagonal().array() += 1.0;
430 
431  Fu_.bottomRows(num_velocities_).noalias() = da_du * dt_;
432  }
433  break;
434  // Semi-implicit Euler
436  {
437  Fx_.topRows(num_velocities_).noalias() = da_dx * dt_ * dt_;
438  Fx_.bottomRows(num_velocities_).noalias() = da_dx * dt_;
439  Fx_.topRightCorner(num_velocities_, num_velocities_).diagonal().array() += dt_;
440  Fx_.diagonal().array() += 1.0;
441 
442  Fu_.topRows(num_velocities_).noalias() = da_du * dt_ * dt_; // Semi-implicit: configuration changes with acceleration in same step
443  Fu_.bottomRows(num_velocities_).noalias() = da_du * dt_;
444  }
445  break;
446  default:
447  ThrowPretty("Not implemented!");
448  };
449 }
450 
451 template <typename T, int NX, int NU>
452 const Eigen::Matrix<T, NX, NX>& AbstractDynamicsSolver<T, NX, NU>::get_fx() const
453 {
454  return fx_;
455 }
456 
457 template <typename T, int NX, int NU>
458 const Eigen::Matrix<T, NX, NU>& AbstractDynamicsSolver<T, NX, NU>::get_fu() const
459 {
460  return fu_;
461 }
462 
463 template <typename T, int NX, int NU>
464 const Eigen::Matrix<T, NX, NX>& AbstractDynamicsSolver<T, NX, NU>::get_Fx() const
465 {
466  return Fx_;
467 }
468 
469 template <typename T, int NX, int NU>
470 const Eigen::Matrix<T, NX, NU>& AbstractDynamicsSolver<T, NX, NU>::get_Fu() const
471 {
472  return Fu_;
473 }
474 
476 } // namespace exotica
exotica::Object::InstantiateObject
void InstantiateObject(const Initializer &init)
Definition: object.h:71
exotica::AbstractDynamicsSolver::get_fu
const ControlDerivative & get_fu() const
Returns derivative fu computed by ComputeDerivatives.
Definition: dynamics_solver.cpp:458
exotica::AbstractDynamicsSolver::fx_fd
StateDerivative fx_fd(const StateVector &x, const ControlVector &u)
Derivative of the forward dynamics w.r.t. the state [finite differencing].
Definition: dynamics_solver.cpp:340
exotica::AbstractDynamicsSolver
Definition: dynamics_solver.h:55
exotica::AbstractDynamicsSolver::StateVector
Eigen::Matrix< T, NX, 1 > StateVector
Convenience definition for a StateVector containing both position and velocity (dimension NX x 1)
Definition: dynamics_solver.h:58
exotica::AbstractDynamicsSolver::get_control_limits
const Eigen::MatrixXd & get_control_limits()
Returns the control limits vector.
Definition: dynamics_solver.cpp:247
exotica::AbstractDynamicsSolver::get_num_controls
int get_num_controls() const
Returns number of controls.
Definition: dynamics_solver.cpp:178
exotica::AbstractDynamicsSolver::get_Fx
const StateDerivative & get_Fx() const
Returns derivative Fx computed by ComputeDerivatives.
Definition: dynamics_solver.cpp:464
exotica::SymplecticEuler
@ SymplecticEuler
Semi-Implicit Euler.
Definition: dynamics_solver.h:49
exotica::AbstractDynamicsSolver::Integrate
virtual void Integrate(const StateVector &x, const StateVector &dx, const double dt, StateVector &xout)
Integrates without performing dynamics.
Definition: dynamics_solver.cpp:122
exotica::Integrator
Integrator
Definition: dynamics_solver.h:46
exotica::AbstractDynamicsSolver::InitializeSecondOrderDerivatives
void InitializeSecondOrderDerivatives()
Definition: dynamics_solver.cpp:290
exotica::AbstractDynamicsSolver::~AbstractDynamicsSolver
virtual ~AbstractDynamicsSolver()
exotica::AbstractDynamicsSolver::ComputeDerivatives
virtual void ComputeDerivatives(const StateVector &x, const ControlVector &u)
Computes derivatives fx, fu, Fx, Fu [single call for efficiency, derivatives can be retrieved with ge...
Definition: dynamics_solver.cpp:407
INFO_NAMED
#define INFO_NAMED(name, x)
Definition: printable.h:64
exotica::AbstractDynamicsSolver::AssignScene
virtual void AssignScene(std::shared_ptr< Scene > scene_in)
Passes the Scene of the PlanningProblem to the DynamicsSolver.
Definition: dynamics_solver.cpp:58
exotica::AbstractDynamicsSolver::fxx
virtual Eigen::Tensor< T, 3 > fxx(const StateVector &x, const ControlVector &u)
Definition: dynamics_solver.cpp:313
exotica::AbstractDynamicsSolver::get_fx
const StateDerivative & get_fx() const
Returns derivative fx computed by ComputeDerivatives.
Definition: dynamics_solver.cpp:452
exotica
Definition: collision_scene.h:46
f
f
exotica::AbstractDynamicsSolver::ClampToStateLimits
void ClampToStateLimits(Eigen::Ref< Eigen::VectorXd > state_in)
Clamps the passed in state to the state limits.
Definition: dynamics_solver.cpp:279
scene.h
exotica::AbstractDynamicsSolver::get_num_velocities
int get_num_velocities() const
Returns number of velocities.
Definition: dynamics_solver.cpp:190
exotica::AbstractDynamicsSolver::get_Fu
const ControlDerivative & get_Fu() const
Returns derivative Fu computed by ComputeDerivatives.
Definition: dynamics_solver.cpp:470
exotica::AbstractDynamicsSolver::get_num_state
int get_num_state() const
Returns size of state space (nx)
Definition: dynamics_solver.cpp:196
exotica::AbstractDynamicsSolver::get_num_state_derivative
int get_num_state_derivative() const
Returns size of derivative vector of state space (ndx)
Definition: dynamics_solver.cpp:205
exotica::AbstractDynamicsSolver::fuu
virtual Eigen::Tensor< T, 3 > fuu(const StateVector &x, const ControlVector &u)
Definition: dynamics_solver.cpp:320
Eigen::VectorXdRefConst
const typedef Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Definition: conversions.h:53
exotica::AbstractDynamicsSolver::get_dt
T get_dt() const
Returns integration timestep dt.
Definition: dynamics_solver.cpp:214
exotica::Initializer
Definition: property.h:70
exotica::RK4
@ RK4
Runge-Kutta 4.
Definition: dynamics_solver.h:51
exotica::AbstractDynamicsSolver::set_control_limits
void set_control_limits(Eigen::VectorXdRefConst control_limits_low, Eigen::VectorXdRefConst control_limits_high)
Sets the control limits.
Definition: dynamics_solver.cpp:255
exotica::AbstractDynamicsSolver::ControlVector
Eigen::Matrix< T, NU, 1 > ControlVector
Convenience definition for a ControlVector (dimension NU x 1)
Definition: dynamics_solver.h:59
exotica::AbstractDynamicsSolver::fu
virtual ControlDerivative fu(const StateVector &x, const ControlVector &u)
Derivative of the forward dynamics w.r.t. the control.
Definition: dynamics_solver.cpp:401
exotica::AbstractDynamicsSolver::SimulateOneStep
virtual StateVector SimulateOneStep(const StateVector &x, const ControlVector &u)
Integrates the dynamic system from state x with controls u applied for one timestep dt using the sele...
Definition: dynamics_solver.cpp:77
exotica::AbstractDynamicsSolver::fx
virtual StateDerivative fx(const StateVector &x, const ControlVector &u)
Derivative of the forward dynamics w.r.t. the state.
Definition: dynamics_solver.cpp:373
exotica::AbstractDynamicsSolver::AbstractDynamicsSolver
AbstractDynamicsSolver()
exotica::AbstractDynamicsSolver::InverseDynamics
virtual ControlVector InverseDynamics(const StateVector &state)
Returns a control vector corresponding to the state vector assuming zero acceleration.
Definition: dynamics_solver.cpp:334
exotica::AbstractDynamicsSolver::SetDt
virtual void SetDt(double dt_in)
Sets the timestep dt to be used for integration.
Definition: dynamics_solver.cpp:70
exotica::RK1
@ RK1
Forward Euler (explicit)
Definition: dynamics_solver.h:48
exotica::AbstractDynamicsSolver::SetIntegrator
void SetIntegrator(const std::string &integrator_in)
Sets integrator type based on request string.
Definition: dynamics_solver.cpp:232
ThrowPretty
#define ThrowPretty(m)
Definition: exception.h:36
exotica::AbstractDynamicsSolver::get_num_positions
int get_num_positions() const
Returns number of positions.
Definition: dynamics_solver.cpp:184
init
void init(const M_string &remappings)
exotica::AbstractDynamicsSolver::get_integrator
Integrator get_integrator() const
Returns used integration scheme.
Definition: dynamics_solver.cpp:220
x
double x
exotica::AbstractDynamicsSolver::fu_fd
ControlDerivative fu_fd(const StateVector &x, const ControlVector &u)
Derivative of the forward dynamics w.r.t. the control [finite differencing].
Definition: dynamics_solver.cpp:379
exotica::AbstractDynamicsSolver::InstantiateBase
virtual void InstantiateBase(const Initializer &init)
Instantiates the base properties of the DynamicsSolver.
Definition: dynamics_solver.cpp:42
exotica::AbstractDynamicsSolver::Simulate
StateVector Simulate(const StateVector &x, const ControlVector &u, T t)
Simulates the dynamic system from starting state x using control u for t seconds.
Definition: dynamics_solver.cpp:159
exotica::AbstractDynamicsSolver::GetPosition
virtual Eigen::Matrix< T, Eigen::Dynamic, 1 > GetPosition(Eigen::VectorXdRefConst x_in)
Returns the position-part of the state vector to update the scene. For types including SE(3) and rota...
Definition: dynamics_solver.cpp:171
dynamics_solver.h
exotica::AbstractDynamicsSolver::F
virtual StateVector F(const StateVector &x, const ControlVector &u)
State transition function. This internally computes the differential dynamics and applies the chosen ...
Definition: dynamics_solver.cpp:63
t
geometry_msgs::TransformStamped t
exotica::AbstractDynamicsSolver::fxu
virtual Eigen::Tensor< T, 3 > fxu(const StateVector &x, const ControlVector &u)
Definition: dynamics_solver.cpp:327
exotica::AbstractDynamicsSolver::set_integrator
void set_integrator(Integrator integrator_in)
Sets integrator type.
Definition: dynamics_solver.cpp:226
exotica::RK2
@ RK2
Explicit trapezoid rule.
Definition: dynamics_solver.h:50


exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Aug 2 2024 08:43:02