Exotica
pinocchio_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_PINOCCHIO_DYNAMICS_SOLVER_PINOCCHIO_DYNAMICS_SOLVER_H_
31 #define EXOTICA_PINOCCHIO_DYNAMICS_SOLVER_PINOCCHIO_DYNAMICS_SOLVER_H_
32 
35 #pragma GCC diagnostic push
36 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
37 
40 #include <pinocchio/fwd.hpp>
41 
43 #include <exotica_core/scene.h>
44 
45 #include <exotica_pinocchio_dynamics_solver/pinocchio_dynamics_solver_initializer.h>
46 
47 #include <pinocchio/multibody/data.hpp>
48 #include <pinocchio/multibody/model.hpp>
49 
50 #pragma GCC diagnostic pop
51 
52 namespace exotica
53 {
54 class PinocchioDynamicsSolver : public DynamicsSolver, public Instantiable<PinocchioDynamicsSolverInitializer>
55 {
56 public:
57  void AssignScene(ScenePtr scene_in) override;
58 
59  StateVector f(const StateVector& x, const ControlVector& u) override;
60  StateDerivative fx(const StateVector& x, const ControlVector& u) override;
61  ControlDerivative fu(const StateVector& x, const ControlVector& u) override;
62  void ComputeDerivatives(const StateVector& x, const ControlVector& u) override;
63  ControlVector InverseDynamics(const StateVector& x) override;
64  StateVector StateDelta(const StateVector& x_1, const StateVector& x_2) override;
65  Eigen::MatrixXd dStateDelta(const StateVector& x_1, const StateVector& x_2, const ArgumentPosition first_or_second) override;
66  void Integrate(const StateVector& x, const StateVector& dx, const double dt, StateVector& xout) override;
67 
68 private:
69  pinocchio::Model model_;
70  std::unique_ptr<pinocchio::Data> pinocchio_data_;
71 
72  Eigen::VectorXd xdot_analytic_;
73 };
74 } // namespace exotica
75 
76 #endif // EXOTICA_PINOCCHIO_DYNAMICS_SOLVER_PINOCCHIO_DYNAMICS_SOLVER_H_
exotica::PinocchioDynamicsSolver::AssignScene
void AssignScene(ScenePtr scene_in) override
exotica::PinocchioDynamicsSolver::fu
ControlDerivative fu(const StateVector &x, const ControlVector &u) override
Derivative of the forward dynamics w.r.t. the control.
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::PinocchioDynamicsSolver::dStateDelta
Eigen::MatrixXd dStateDelta(const StateVector &x_1, const StateVector &x_2, const ArgumentPosition first_or_second) override
Return the difference of the StateDelta operation between two state vectors. The ArgumentPosition arg...
exotica::PinocchioDynamicsSolver::StateDelta
StateVector StateDelta(const StateVector &x_1, const StateVector &x_2) override
Return the difference of two state vectors. Used when e.g. angle differences need to be wrapped from ...
exotica::AbstractDynamicsSolver::StateDerivative
Eigen::Matrix< T, NX, NX > StateDerivative
Convenience definition for a StateDerivative.
Definition: dynamics_solver.h:60
exotica::Instantiable
Definition: property.h:110
exotica
Definition: cartpole_dynamics_solver.h:38
exotica::PinocchioDynamicsSolver::pinocchio_data_
std::unique_ptr< pinocchio::Data > pinocchio_data_
Definition: pinocchio_dynamics_solver.h:70
exotica::PinocchioDynamicsSolver::model_
pinocchio::Model model_
Definition: pinocchio_dynamics_solver.h:69
scene.h
exotica::PinocchioDynamicsSolver::xdot_analytic_
Eigen::VectorXd xdot_analytic_
Definition: pinocchio_dynamics_solver.h:72
exotica::PinocchioDynamicsSolver::ComputeDerivatives
void ComputeDerivatives(const StateVector &x, const ControlVector &u) override
Computes derivatives fx, fu, Fx, Fu [single call for efficiency, derivatives can be retrieved with ge...
exotica::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: scene.h:246
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::AbstractDynamicsSolver::ControlVector
Eigen::Matrix< T, NU, 1 > ControlVector
Convenience definition for a ControlVector (dimension NU x 1)
Definition: dynamics_solver.h:59
exotica::PinocchioDynamicsSolver::InverseDynamics
ControlVector InverseDynamics(const StateVector &x) override
Returns a control vector corresponding to the state vector assuming zero acceleration.
exotica::PinocchioDynamicsSolver::Integrate
void Integrate(const StateVector &x, const StateVector &dx, const double dt, StateVector &xout) override
Integrates without performing dynamics.
exotica::PinocchioDynamicsSolver::fx
StateDerivative fx(const StateVector &x, const ControlVector &u) override
Derivative of the forward dynamics w.r.t. the state.
exotica::PinocchioDynamicsSolver::f
StateVector f(const StateVector &x, const ControlVector &u) override
Forward dynamics. This computes the differential dynamics.
dynamics_solver.h
exotica::PinocchioDynamicsSolver
Definition: pinocchio_dynamics_solver.h:54