Exotica
dynamics_solver.h
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 
30 #ifndef EXOTICA_CORE_DYNAMICS_SOLVER_H_
31 #define EXOTICA_CORE_DYNAMICS_SOLVER_H_
32 
33 #include <exotica_core/factory.h>
34 #include <exotica_core/object.h>
35 #include <exotica_core/property.h>
36 #include <exotica_core/tools.h>
37 
38 #include <exotica_core/dynamics_solver_initializer.h>
39 
40 #define REGISTER_DYNAMICS_SOLVER_TYPE(TYPE, DERIV) EXOTICA_CORE_REGISTER(exotica::DynamicsSolver, TYPE, DERIV)
41 
42 namespace exotica
43 {
44 class Scene;
45 
47 {
48  RK1 = 0,
50  RK2,
51  RK4,
52 };
53 
54 template <typename T, int NX, int NU>
56 {
57 public:
58  typedef Eigen::Matrix<T, NX, 1> StateVector;
59  typedef Eigen::Matrix<T, NU, 1> ControlVector;
60  typedef Eigen::Matrix<T, NX, NX> StateDerivative;
61  typedef Eigen::Matrix<T, NX, NU> ControlDerivative;
62 
64  virtual ~AbstractDynamicsSolver();
65 
67  virtual void InstantiateBase(const Initializer& init);
68 
72  virtual void AssignScene(std::shared_ptr<Scene> scene_in);
73 
75  virtual void SetDt(double dt_in);
76 
78  virtual StateVector f(const StateVector& x, const ControlVector& u) = 0;
79 
81  virtual StateVector F(const StateVector& x, const ControlVector& u);
82 
84  virtual void ComputeDerivatives(const StateVector& x, const ControlVector& u);
85 
87  const StateDerivative& get_Fx() const;
88 
90  const ControlDerivative& get_Fu() const;
91 
93  const StateDerivative& get_fx() const;
94 
96  const ControlDerivative& get_fu() const;
97 
99  virtual StateDerivative fx(const StateVector& x, const ControlVector& u);
100 
102  virtual ControlDerivative fu(const StateVector& x, const ControlVector& u);
103 
105  StateDerivative fx_fd(const StateVector& x, const ControlVector& u);
106 
108  ControlDerivative fu_fd(const StateVector& x, const ControlVector& u);
109 
112  {
114  }
115 
116  // NOTE: Second order derivatives a 3D matrices, i.e. tensors
117  // We use the numerator convention (see https://en.wikipedia.org/wiki/Matrix_calculus)
118  // X_i,j,k = d(X_i,j)/d x_k
119  //
120  // Additionally, the first subscript is the *second* partial derivative.
121  // I.e. f_xu = (f_u)_x
122  // TODO: Eigen::Tensor to be replaced with exotica::Hessian
123  virtual Eigen::Tensor<T, 3> fxx(const StateVector& x, const ControlVector& u);
124  virtual Eigen::Tensor<T, 3> fuu(const StateVector& x, const ControlVector& u);
125  virtual Eigen::Tensor<T, 3> fxu(const StateVector& x, const ControlVector& u);
126 
130  // TODO: To be deprecated - or at least remove its use - as it's difficult to get partial derivatives.
131  StateVector Simulate(const StateVector& x, const ControlVector& u, T t);
132 
136  virtual StateVector StateDelta(const StateVector& x_1, const StateVector& x_2)
137  {
138  assert(x_1.size() == x_2.size());
139  return x_1 - x_2;
140  }
141 
142  void StateDelta(const StateVector& x_1, const StateVector& x_2, Eigen::VectorXdRef xout)
143  {
144  xout = StateDelta(x_1, x_2);
145  }
146 
149  virtual Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> dStateDelta(const StateVector& x_1, const StateVector& x_2, const ArgumentPosition first_or_second)
150  {
151  assert(x_1.size() == x_2.size());
152  assert(first_or_second == ArgumentPosition::ARG0 || first_or_second == ArgumentPosition::ARG1);
153 
155 
156  if (first_or_second == ArgumentPosition::ARG0)
157  return dStateDelta_;
158  else
159  return -1.0 * dStateDelta_;
160  }
161 
162  virtual Hessian ddStateDelta(const StateVector& x_1, const StateVector& x_2, const ArgumentPosition first_or_second)
163  {
164  // In Euclidean spaces, this is zero.
165 
166  assert(x_1.size() == x_2.size());
167  assert(first_or_second == ArgumentPosition::ARG0 || first_or_second == ArgumentPosition::ARG1);
168 
170 
171  return ddStateDelta_;
172  }
173 
176  virtual Eigen::Matrix<T, Eigen::Dynamic, 1> GetPosition(Eigen::VectorXdRefConst x_in);
177 
179  int get_num_controls() const;
180 
182  int get_num_positions() const;
183 
185  int get_num_velocities() const;
186 
188  int get_num_state() const;
189 
191  int get_num_state_derivative() const;
192 
194  T get_dt() const;
195 
197  Integrator get_integrator() const;
198 
200  void set_integrator(Integrator integrator_in);
201 
203  void SetIntegrator(const std::string& integrator_in);
204 
206  // returns: Two-column matrix, first column contains low control limits,
207  // second - the high control limits
208  const Eigen::MatrixXd& get_control_limits();
209 
211  void set_control_limits(Eigen::VectorXdRefConst control_limits_low, Eigen::VectorXdRefConst control_limits_high);
212 
214  const bool& get_has_state_limits() const
215  {
216  return has_state_limits_;
217  }
218 
220  void ClampToStateLimits(Eigen::Ref<Eigen::VectorXd> state_in);
221 
223  virtual ControlVector InverseDynamics(const StateVector& state);
224 
226  virtual void Integrate(const StateVector& x, const StateVector& dx, const double dt, StateVector& xout);
227 
228 private:
231  Eigen::MatrixXd dStateDelta_;
233 
234 protected:
235  int num_controls_ = -1;
236  int num_positions_ = -1;
237  int num_velocities_ = -1;
238  int num_state_ = -1;
240 
243 
244  bool has_state_limits_ = false;
245  Eigen::VectorXd state_limits_lower_;
246  Eigen::VectorXd state_limits_upper_;
247 
248  T dt_ = 0.01;
250  // TODO: Need to enforce control limits.
251  // First column is the low limits, second is the high limits.
252  Eigen::MatrixXd control_limits_;
253 
255  // TODO: To be deprecated in favour of explicit call to Integrate in Simulate
256  virtual StateVector SimulateOneStep(const StateVector& x, const ControlVector& u);
257 
259  Eigen::Tensor<T, 3> fxx_default_, fuu_default_, fxu_default_;
260 
261  Eigen::MatrixXd fx_;
262  Eigen::MatrixXd fu_;
263  Eigen::MatrixXd Fx_;
264  Eigen::MatrixXd Fu_;
265 };
266 
268 
269 typedef std::shared_ptr<exotica::DynamicsSolver> DynamicsSolverPtr;
270 } // namespace exotica
271 
272 #endif // EXOTICA_CORE_DYNAMICS_SOLVER_H_
exotica::AbstractDynamicsSolver::get_fu
const ControlDerivative & get_fu() const
Returns derivative fu computed by ComputeDerivatives.
exotica::AbstractDynamicsSolver::StateDelta
void StateDelta(const StateVector &x_1, const StateVector &x_2, Eigen::VectorXdRef xout)
Definition: dynamics_solver.h:142
exotica::AbstractDynamicsSolver::num_state_
int num_state_
Size of state space (num_positions + num_velocities)
Definition: dynamics_solver.h:238
exotica::AbstractDynamicsSolver::fx
virtual StateDerivative fx(const StateVector &x, const ControlVector &u)
Derivative of the forward dynamics w.r.t. the state.
exotica::ARG1
@ ARG1
Definition: tools.h:126
exotica::AbstractDynamicsSolver::Integrate
virtual void Integrate(const StateVector &x, const StateVector &dx, const double dt, StateVector &xout)
Integrates without performing dynamics.
exotica::AbstractDynamicsSolver
Definition: dynamics_solver.h:55
exotica::Uncopyable
Definition: uncopyable.h:35
factory.h
exotica::AbstractDynamicsSolver::fxu
virtual Eigen::Tensor< T, 3 > fxu(const StateVector &x, const ControlVector &u)
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::fxx
virtual Eigen::Tensor< T, 3 > fxx(const StateVector &x, const ControlVector &u)
exotica::AbstractDynamicsSolver::has_state_limits_
bool has_state_limits_
Whether the solver specifies state limits.
Definition: dynamics_solver.h:244
exotica::AbstractDynamicsSolver::ddStateDelta
virtual Hessian ddStateDelta(const StateVector &x_1, const StateVector &x_2, const ArgumentPosition first_or_second)
Definition: dynamics_solver.h:162
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 ...
exotica::AbstractDynamicsSolver::get_num_controls
int get_num_controls() const
Returns number of controls.
exotica::AbstractDynamicsSolver::get_has_second_order_derivatives
const bool & get_has_second_order_derivatives() const
Returns whether second-order derivatives are available.
Definition: dynamics_solver.h:111
exotica::SymplecticEuler
@ SymplecticEuler
Semi-Implicit Euler.
Definition: dynamics_solver.h:49
exotica::Integrator
Integrator
Definition: dynamics_solver.h:46
exotica::AbstractDynamicsSolver::InitializeSecondOrderDerivatives
void InitializeSecondOrderDerivatives()
property.h
exotica::AbstractDynamicsSolver::ddStateDelta_
Hessian ddStateDelta_
Definition: dynamics_solver.h:232
exotica::AbstractDynamicsSolver::fxx_default_
Eigen::Tensor< T, 3 > fxx_default_
Definition: dynamics_solver.h:259
exotica::AbstractDynamicsSolver::second_order_derivatives_initialized_
bool second_order_derivatives_initialized_
Whether fxx, fxu and fuu have been initialized to 0.
Definition: dynamics_solver.h:242
exotica::AbstractDynamicsSolver::state_limits_upper_
Eigen::VectorXd state_limits_upper_
Upper state limits (configuration and velocity)
Definition: dynamics_solver.h:246
exotica::AbstractDynamicsSolver::dStateDelta
virtual Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > dStateDelta(const StateVector &x_1, const StateVector &x_2, const ArgumentPosition first_or_second)
Return the difference of the StateDelta operation between two state vectors. The ArgumentPosition arg...
Definition: dynamics_solver.h:149
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...
exotica::AbstractDynamicsSolver::num_positions_
int num_positions_
Number of positions in the dynamic system.
Definition: dynamics_solver.h:236
exotica::AbstractDynamicsSolver::raw_control_limits_low_
Eigen::VectorXd raw_control_limits_low_
Definition: dynamics_solver.h:230
exotica::AbstractDynamicsSolver::fxu_default_
Eigen::Tensor< T, 3 > fxu_default_
Definition: dynamics_solver.h:259
exotica::AbstractDynamicsSolver::fu_
Eigen::MatrixXd fu_
Internal storage of differential dynamics partial derivative fu computed by ComputeDerivatives.
Definition: dynamics_solver.h:262
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].
exotica::AbstractDynamicsSolver::raw_control_limits_high_
Eigen::VectorXd raw_control_limits_high_
Definition: dynamics_solver.h:230
exotica::InstantiableBase
Definition: property.h:98
exotica::DynamicsSolverPtr
std::shared_ptr< exotica::DynamicsSolver > DynamicsSolverPtr
Definition: dynamics_solver.h:269
exotica::AbstractDynamicsSolver::StateDerivative
Eigen::Matrix< T, NX, NX > StateDerivative
Convenience definition for a StateDerivative.
Definition: dynamics_solver.h:60
exotica::AbstractDynamicsSolver::control_limits_initialized_
bool control_limits_initialized_
Definition: dynamics_solver.h:229
exotica
Definition: cartpole_dynamics_solver.h:38
exotica::Hessian
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
Definition: conversions.h:154
exotica::AbstractDynamicsSolver::ClampToStateLimits
void ClampToStateLimits(Eigen::Ref< Eigen::VectorXd > state_in)
Clamps the passed in state to the state limits.
exotica::AbstractDynamicsSolver::dStateDelta_
Eigen::MatrixXd dStateDelta_
Definition: dynamics_solver.h:231
exotica::AbstractDynamicsSolver::num_state_derivative_
int num_state_derivative_
Size of the tangent vector to the state space (2 * num_velocities)
Definition: dynamics_solver.h:239
exotica::AbstractDynamicsSolver::get_control_limits
const Eigen::MatrixXd & get_control_limits()
Returns the control limits vector.
exotica::AbstractDynamicsSolver::AssignScene
virtual void AssignScene(std::shared_ptr< Scene > scene_in)
Passes the Scene of the PlanningProblem to the DynamicsSolver.
exotica::AbstractDynamicsSolver::has_second_order_derivatives_
bool has_second_order_derivatives_
Whether this solver provides second order derivatives. If false (default), assumed to be all zeros.
Definition: dynamics_solver.h:241
exotica::AbstractDynamicsSolver::get_num_velocities
int get_num_velocities() const
Returns number of velocities.
exotica::AbstractDynamicsSolver::get_num_state
int get_num_state() const
Returns size of state space (nx)
exotica::AbstractDynamicsSolver::get_num_state_derivative
int get_num_state_derivative() const
Returns size of derivative vector of state space (ndx)
exotica::AbstractDynamicsSolver::state_limits_lower_
Eigen::VectorXd state_limits_lower_
Lower state limits (configuration and velocity)
Definition: dynamics_solver.h:245
exotica::Object
Definition: object.h:44
Eigen::VectorXdRef
Eigen::Ref< Eigen::VectorXd > VectorXdRef
Definition: conversions.h:54
Eigen::VectorXdRefConst
const typedef Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Definition: conversions.h:52
exotica::AbstractDynamicsSolver::get_dt
T get_dt() const
Returns integration timestep dt.
exotica::AbstractDynamicsSolver::dt_
T dt_
Internal timestep used for integration. Defaults to 10ms.
Definition: dynamics_solver.h:248
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.
exotica::AbstractDynamicsSolver::ControlDerivative
Eigen::Matrix< T, NX, NU > ControlDerivative
Convenience definition for a ControlDerivative.
Definition: dynamics_solver.h:61
exotica::ArgumentPosition
ArgumentPosition
Argument position. Used as parameter to refer to an argument.
Definition: tools.h:123
exotica::ARG0
@ ARG0
Definition: tools.h:125
exotica::AbstractDynamicsSolver::get_Fu
const ControlDerivative & get_Fu() const
Returns derivative Fu computed by ComputeDerivatives.
exotica::AbstractDynamicsSolver::~AbstractDynamicsSolver
virtual ~AbstractDynamicsSolver()
exotica::AbstractDynamicsSolver::fu
virtual ControlDerivative fu(const StateVector &x, const ControlVector &u)
Derivative of the forward dynamics w.r.t. the control.
exotica::Initializer
Definition: property.h:70
exotica::AbstractDynamicsSolver::fx_
Eigen::MatrixXd fx_
Internal storage of differential dynamics partial derivative fx computed by ComputeDerivatives.
Definition: dynamics_solver.h:261
exotica::RK4
@ RK4
Runge-Kutta 4.
Definition: dynamics_solver.h:51
exotica::AbstractDynamicsSolver::Fx_
Eigen::MatrixXd Fx_
Internal storage of state transition partial derivative Fx computed by ComputeDerivatives.
Definition: dynamics_solver.h:263
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...
exotica::AbstractDynamicsSolver::InverseDynamics
virtual ControlVector InverseDynamics(const StateVector &state)
Returns a control vector corresponding to the state vector assuming zero acceleration.
exotica::AbstractDynamicsSolver::integrator_
Integrator integrator_
Chosen integrator. Defaults to Euler (RK1).
Definition: dynamics_solver.h:249
exotica::AbstractDynamicsSolver::set_control_limits
void set_control_limits(Eigen::VectorXdRefConst control_limits_low, Eigen::VectorXdRefConst control_limits_high)
Sets the control limits.
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::AbstractDynamicsSolver
AbstractDynamicsSolver()
exotica::AbstractDynamicsSolver::SetDt
virtual void SetDt(double dt_in)
Sets the timestep dt to be used for integration.
exotica::AbstractDynamicsSolver::control_limits_
Eigen::MatrixXd control_limits_
ControlLimits. Default is empty vector.
Definition: dynamics_solver.h:252
exotica::RK1
@ RK1
Forward Euler (explicit)
Definition: dynamics_solver.h:48
exotica::AbstractDynamicsSolver::get_Fx
const StateDerivative & get_Fx() const
Returns derivative Fx computed by ComputeDerivatives.
exotica::AbstractDynamicsSolver::fuu_default_
Eigen::Tensor< T, 3 > fuu_default_
Definition: dynamics_solver.h:259
exotica::AbstractDynamicsSolver::SetIntegrator
void SetIntegrator(const std::string &integrator_in)
Sets integrator type based on request string.
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].
exotica::AbstractDynamicsSolver::get_num_positions
int get_num_positions() const
Returns number of positions.
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...
exotica::AbstractDynamicsSolver::f
virtual StateVector f(const StateVector &x, const ControlVector &u)=0
Forward dynamics. This computes the differential dynamics.
exotica::AbstractDynamicsSolver::Fu_
Eigen::MatrixXd Fu_
Internal storage of state transition partial derivative Fu computed by ComputeDerivatives.
Definition: dynamics_solver.h:264
exotica::AbstractDynamicsSolver::get_integrator
Integrator get_integrator() const
Returns used integration scheme.
exotica::AbstractDynamicsSolver::get_has_state_limits
const bool & get_has_state_limits() const
Returns whether state limits are available.
Definition: dynamics_solver.h:214
exotica::AbstractDynamicsSolver::fuu
virtual Eigen::Tensor< T, 3 > fuu(const StateVector &x, const ControlVector &u)
exotica::AbstractDynamicsSolver::num_controls_
int num_controls_
Number of controls in the dynamic system.
Definition: dynamics_solver.h:235
exotica::AbstractDynamicsSolver::InstantiateBase
virtual void InstantiateBase(const Initializer &init)
Instantiates the base properties of the DynamicsSolver.
tools.h
exotica::AbstractDynamicsSolver::StateDelta
virtual StateVector StateDelta(const StateVector &x_1, const StateVector &x_2)
Return the difference of two state vectors. Used when e.g. angle differences need to be wrapped from ...
Definition: dynamics_solver.h:136
exotica::AbstractDynamicsSolver::get_fx
const StateDerivative & get_fx() const
Returns derivative fx computed by ComputeDerivatives.
exotica::DynamicsSolver
AbstractDynamicsSolver< double, Eigen::Dynamic, Eigen::Dynamic > DynamicsSolver
Definition: dynamics_solver.h:267
object.h
exotica::AbstractDynamicsSolver::set_integrator
void set_integrator(Integrator integrator_in)
Sets integrator type.
exotica::AbstractDynamicsSolver::num_velocities_
int num_velocities_
Number of velocities in the dynamic system.
Definition: dynamics_solver.h:237
exotica::RK2
@ RK2
Explicit trapezoid rule.
Definition: dynamics_solver.h:50