crocoddyl  1.7.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 "crocoddyl/multibody/fwd.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
16 #include "crocoddyl/core/action-base.hpp"
17 #include "crocoddyl/core/costs/cost-sum.hpp"
18 #include "crocoddyl/multibody/states/multibody.hpp"
19 #include "crocoddyl/multibody/actuations/floating-base.hpp"
20 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
21 #include "crocoddyl/multibody/data/impulses.hpp"
22 #include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
23 
24 #include <pinocchio/algorithm/compute-all-terms.hpp>
25 #include <pinocchio/algorithm/frames.hpp>
26 #include <pinocchio/algorithm/contact-dynamics.hpp>
27 #include <pinocchio/algorithm/centroidal.hpp>
28 #include <pinocchio/algorithm/rnea-derivatives.hpp>
29 #include <pinocchio/algorithm/kinematics-derivatives.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 
76  template <class Scalar>
77  friend std::ostream& operator<<(std::ostream& os, const ActionModelImpulseFwdDynamicsTpl<Scalar>& model);
78 
79  protected:
81  using Base::nr_;
82  using Base::nu_;
83  using Base::state_;
84  using Base::u_lb_;
85  using Base::u_ub_;
86  using Base::unone_;
87 
88  private:
89  boost::shared_ptr<ImpulseModelMultiple> impulses_;
90  boost::shared_ptr<CostModelSum> costs_;
91  pinocchio::ModelTpl<Scalar>& pinocchio_;
92  bool with_armature_;
93  VectorXs armature_;
94  Scalar r_coeff_;
95  Scalar JMinvJt_damping_;
96  bool enable_force_;
97  pinocchio::MotionTpl<Scalar> gravity_;
98 };
99 
100 template <typename _Scalar>
102  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
103  typedef _Scalar Scalar;
106  typedef typename MathBase::VectorXs VectorXs;
107  typedef typename MathBase::MatrixXs MatrixXs;
108 
109  template <template <typename Scalar> class Model>
110  explicit ActionDataImpulseFwdDynamicsTpl(Model<Scalar>* const model)
111  : Base(model),
112  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
113  multibody(&pinocchio, model->get_impulses()->createData(&pinocchio)),
114  costs(model->get_costs()->createData(&multibody)),
115  vnone(model->get_state()->get_nv()),
116  Kinv(model->get_state()->get_nv() + model->get_impulses()->get_ni_total(),
117  model->get_state()->get_nv() + model->get_impulses()->get_ni_total()),
118  df_dx(model->get_impulses()->get_ni_total(), model->get_state()->get_ndx()),
119  dgrav_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
120  costs->shareMemory(this);
121  vnone.setZero();
122  Kinv.setZero();
123  df_dx.setZero();
124  dgrav_dq.setZero();
125  }
126 
127  pinocchio::DataTpl<Scalar> pinocchio;
129  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
130  VectorXs vnone;
131  MatrixXs Kinv;
132  MatrixXs df_dx;
133  MatrixXs dgrav_dq;
134 };
135 
136 } // namespace crocoddyl
137 
138 /* --- Details -------------------------------------------------------------- */
139 /* --- Details -------------------------------------------------------------- */
140 /* --- Details -------------------------------------------------------------- */
141 #include <crocoddyl/multibody/actions/impulse-fwddyn.hxx>
142 
143 #endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
Abstract class for action model.
Definition: action-base.hpp:60
bool has_control_limits_
Indicates whether any of the control limits is finite.
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.
std::size_t nu_
Control dimension.
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:215
Define a stack of impulse models.
Definition: fwd.hpp:253
VectorXs unone_
Neutral state.
VectorXs u_ub_
Upper control limits.
VectorXs u_lb_
Lower control limits.
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.
std::size_t nr_
Dimension of the cost residual.