crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-fwddyn.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
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/multibody/states/multibody.hpp"
18 #include "crocoddyl/multibody/actuations/floating-base.hpp"
19 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
20 #include "crocoddyl/multibody/data/impulses.hpp"
21 #include "crocoddyl/multibody/costs/cost-sum.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 
73  protected:
75  using Base::nr_;
76  using Base::nu_;
77  using Base::state_;
78  using Base::u_lb_;
79  using Base::u_ub_;
80  using Base::unone_;
81 
82  private:
83  boost::shared_ptr<ImpulseModelMultiple> impulses_;
84  boost::shared_ptr<CostModelSum> costs_;
85  pinocchio::ModelTpl<Scalar>& pinocchio_;
86  bool with_armature_;
87  VectorXs armature_;
88  Scalar r_coeff_;
89  Scalar JMinvJt_damping_;
90  bool enable_force_;
91  pinocchio::MotionTpl<Scalar> gravity_;
92 };
93 
94 template <typename _Scalar>
96  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
97  typedef _Scalar Scalar;
100  typedef typename MathBase::VectorXs VectorXs;
101  typedef typename MathBase::MatrixXs MatrixXs;
102 
103  template <template <typename Scalar> class Model>
104  explicit ActionDataImpulseFwdDynamicsTpl(Model<Scalar>* const model)
105  : Base(model),
106  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
107  multibody(&pinocchio, model->get_impulses()->createData(&pinocchio)),
108  costs(model->get_costs()->createData(&multibody)),
109  vnone(model->get_state()->get_nv()),
110  Kinv(model->get_state()->get_nv() + model->get_impulses()->get_ni_total(),
111  model->get_state()->get_nv() + model->get_impulses()->get_ni_total()),
112  df_dx(model->get_impulses()->get_ni_total(), model->get_state()->get_ndx()),
113  dgrav_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
114  costs->shareMemory(this);
115  vnone.setZero();
116  Kinv.setZero();
117  df_dx.setZero();
118  dgrav_dq.setZero();
119  }
120 
121  pinocchio::DataTpl<Scalar> pinocchio;
123  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
124  VectorXs vnone;
125  MatrixXs Kinv;
126  MatrixXs df_dx;
127  MatrixXs dgrav_dq;
128 };
129 
130 } // namespace crocoddyl
131 
132 /* --- Details -------------------------------------------------------------- */
133 /* --- Details -------------------------------------------------------------- */
134 /* --- Details -------------------------------------------------------------- */
135 #include <crocoddyl/multibody/actions/impulse-fwddyn.hxx>
136 
137 #endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: action-base.hpp:96
std::size_t nu_
Control dimension.
Definition: action-base.hpp:90
Define a stack of impulse models.
Definition: fwd.hpp:254
VectorXs unone_
Neutral state.
Definition: action-base.hpp:93
VectorXs u_ub_
Upper control limits.
Definition: action-base.hpp:95
VectorXs u_lb_
Lower control limits.
Definition: action-base.hpp:94
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:92
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:91