crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-fwddyn.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
10 #define CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
11 
12 #include <stdexcept>
13 
14 #include <pinocchio/algorithm/compute-all-terms.hpp>
15 #include <pinocchio/algorithm/frames.hpp>
16 #include <pinocchio/algorithm/contact-dynamics.hpp>
17 #include <pinocchio/algorithm/centroidal.hpp>
18 #include <pinocchio/algorithm/rnea-derivatives.hpp>
19 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
20 
21 #include "crocoddyl/multibody/fwd.hpp"
22 #include "crocoddyl/core/utils/exception.hpp"
23 #include "crocoddyl/core/action-base.hpp"
24 #include "crocoddyl/core/costs/cost-sum.hpp"
25 #include "crocoddyl/multibody/states/multibody.hpp"
26 #include "crocoddyl/multibody/actuations/floating-base.hpp"
27 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
28 #include "crocoddyl/multibody/data/impulses.hpp"
29 #include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
30 
31 namespace crocoddyl {
32 
33 template <typename _Scalar>
35  public:
36  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 
38  typedef _Scalar Scalar;
46  typedef typename MathBase::VectorXs VectorXs;
47  typedef typename MathBase::MatrixXs MatrixXs;
48 
49  ActionModelImpulseFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
50  boost::shared_ptr<ImpulseModelMultiple> impulses,
51  boost::shared_ptr<CostModelSum> costs, const Scalar r_coeff = Scalar(0.),
52  const Scalar JMinvJt_damping = Scalar(0.), const bool enable_force = false);
54 
55  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
56  const Eigen::Ref<const VectorXs>& u);
57  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
58  const Eigen::Ref<const VectorXs>& u);
59  virtual boost::shared_ptr<ActionDataAbstract> createData();
60  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
61 
62  const boost::shared_ptr<ImpulseModelMultiple>& get_impulses() const;
63  const boost::shared_ptr<CostModelSum>& get_costs() const;
64  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
65  const VectorXs& get_armature() const;
66  const Scalar get_restitution_coefficient() const;
67  const Scalar get_damping_factor() const;
68 
69  void set_armature(const VectorXs& armature);
70  void set_restitution_coefficient(const Scalar r_coeff);
71  void set_damping_factor(const Scalar damping);
72 
78  virtual void print(std::ostream& os) const;
79 
80  protected:
81  using Base::state_;
82 
83  private:
84  boost::shared_ptr<ImpulseModelMultiple> impulses_;
85  boost::shared_ptr<CostModelSum> costs_;
86  pinocchio::ModelTpl<Scalar>& pinocchio_;
87  bool with_armature_;
88  VectorXs armature_;
89  Scalar r_coeff_;
90  Scalar JMinvJt_damping_;
91  bool enable_force_;
92  pinocchio::MotionTpl<Scalar> gravity_;
93 };
94 
95 template <typename _Scalar>
97  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98  typedef _Scalar Scalar;
101  typedef typename MathBase::VectorXs VectorXs;
102  typedef typename MathBase::MatrixXs MatrixXs;
103 
104  template <template <typename Scalar> class Model>
105  explicit ActionDataImpulseFwdDynamicsTpl(Model<Scalar>* const model)
106  : Base(model),
107  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
108  multibody(&pinocchio, model->get_impulses()->createData(&pinocchio)),
109  costs(model->get_costs()->createData(&multibody)),
110  vnone(model->get_state()->get_nv()),
111  Kinv(model->get_state()->get_nv() + model->get_impulses()->get_nc_total(),
112  model->get_state()->get_nv() + model->get_impulses()->get_nc_total()),
113  df_dx(model->get_impulses()->get_nc_total(), model->get_state()->get_ndx()),
114  dgrav_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
115  costs->shareMemory(this);
116  vnone.setZero();
117  Kinv.setZero();
118  df_dx.setZero();
119  dgrav_dq.setZero();
120  }
121 
122  pinocchio::DataTpl<Scalar> pinocchio;
124  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
125  VectorXs vnone;
126  MatrixXs Kinv;
127  MatrixXs df_dx;
128  MatrixXs dgrav_dq;
129 };
130 
131 } // namespace crocoddyl
132 
133 /* --- Details -------------------------------------------------------------- */
134 /* --- Details -------------------------------------------------------------- */
135 /* --- Details -------------------------------------------------------------- */
136 #include <crocoddyl/multibody/actions/impulse-fwddyn.hxx>
137 
138 #endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
Abstract class for action model.
Definition: action-base.hpp:60
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
State multibody representation.
Definition: fwd.hpp:300
Define a stack of impulse models.
Definition: fwd.hpp:338
virtual void print(std::ostream &os) const
Print relevant information of the impulase forward-dynamics model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the next state and cost value.