crocoddyl  1.9.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 
61 template <typename _Scalar>
63  public:
64  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 
66  typedef _Scalar Scalar;
74  typedef typename MathBase::VectorXs VectorXs;
75  typedef typename MathBase::MatrixXs MatrixXs;
76 
91  ActionModelImpulseFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
92  boost::shared_ptr<ImpulseModelMultiple> impulses,
93  boost::shared_ptr<CostModelSum> costs, const Scalar r_coeff = Scalar(0.),
94  const Scalar JMinvJt_damping = Scalar(0.), const bool enable_force = false);
96 
106  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
107  const Eigen::Ref<const VectorXs>& u);
108 
116  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
117  const Eigen::Ref<const VectorXs>& u);
118 
124  virtual boost::shared_ptr<ActionDataAbstract> createData();
125 
129  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
130 
134  const boost::shared_ptr<ImpulseModelMultiple>& get_impulses() const;
135 
139  const boost::shared_ptr<CostModelSum>& get_costs() const;
140 
144  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
145 
149  const VectorXs& get_armature() const;
150 
154  const Scalar get_restitution_coefficient() const;
155 
159  const Scalar get_damping_factor() const;
160 
164  void set_armature(const VectorXs& armature);
165 
169  void set_restitution_coefficient(const Scalar r_coeff);
170 
174  void set_damping_factor(const Scalar damping);
175 
181  virtual void print(std::ostream& os) const;
182 
183  protected:
184  using Base::state_;
185 
186  private:
187  boost::shared_ptr<ImpulseModelMultiple> impulses_;
188  boost::shared_ptr<CostModelSum> costs_;
189  pinocchio::ModelTpl<Scalar>& pinocchio_;
190  bool with_armature_;
191  VectorXs armature_;
192  Scalar r_coeff_;
193  Scalar JMinvJt_damping_;
194  bool enable_force_;
195  pinocchio::MotionTpl<Scalar> gravity_;
196 };
197 
198 template <typename _Scalar>
200  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
201  typedef _Scalar Scalar;
204  typedef typename MathBase::VectorXs VectorXs;
205  typedef typename MathBase::MatrixXs MatrixXs;
206 
207  template <template <typename Scalar> class Model>
208  explicit ActionDataImpulseFwdDynamicsTpl(Model<Scalar>* const model)
209  : Base(model),
210  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
211  multibody(&pinocchio, model->get_impulses()->createData(&pinocchio)),
212  costs(model->get_costs()->createData(&multibody)),
213  vnone(model->get_state()->get_nv()),
214  Kinv(model->get_state()->get_nv() + model->get_impulses()->get_nc_total(),
215  model->get_state()->get_nv() + model->get_impulses()->get_nc_total()),
216  df_dx(model->get_impulses()->get_nc_total(), model->get_state()->get_ndx()),
217  dgrav_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
218  costs->shareMemory(this);
219  vnone.setZero();
220  Kinv.setZero();
221  df_dx.setZero();
222  dgrav_dq.setZero();
223  }
224 
225  pinocchio::DataTpl<Scalar> pinocchio;
227  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
228  VectorXs vnone;
229  MatrixXs Kinv;
230  MatrixXs df_dx;
231  MatrixXs dgrav_dq;
232 };
233 
234 } // namespace crocoddyl
235 
236 /* --- Details -------------------------------------------------------------- */
237 /* --- Details -------------------------------------------------------------- */
238 /* --- Details -------------------------------------------------------------- */
239 #include <crocoddyl/multibody/actions/impulse-fwddyn.hxx>
240 
241 #endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
crocoddyl::ActionModelImpulseFwdDynamicsTpl::set_armature
void set_armature(const VectorXs &armature)
Modify the armature vector.
crocoddyl::ActionDataAbstractTpl
Definition: action-base.hpp:236
crocoddyl::ActionModelImpulseFwdDynamicsTpl::get_restitution_coefficient
const Scalar get_restitution_coefficient() const
Return the restituion coefficient.
crocoddyl::ActionModelImpulseFwdDynamicsTpl
Action model for impulse forward dynamics in multibody systems.
Definition: impulse-fwddyn.hpp:62
crocoddyl::ActionModelImpulseFwdDynamicsTpl::get_costs
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
crocoddyl::ActionModelImpulseFwdDynamicsTpl::get_pinocchio
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ActionModelImpulseFwdDynamicsTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the impulse forward-dynamics model.
crocoddyl::ActionModelImpulseFwdDynamicsTpl::calc
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the system acceleration, and cost value.
crocoddyl::CostModelSumTpl< Scalar >
crocoddyl::ActionModelImpulseFwdDynamicsTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the impulse forward-dynamics data.
crocoddyl::ImpulseModelMultipleTpl
Define a stack of impulse models.
Definition: fwd.hpp:343
crocoddyl::ActionModelImpulseFwdDynamicsTpl::set_restitution_coefficient
void set_restitution_coefficient(const Scalar r_coeff)
Modify the restituion coefficient.
crocoddyl::ActionModelImpulseFwdDynamicsTpl::ActionModelImpulseFwdDynamicsTpl
ActionModelImpulseFwdDynamicsTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ImpulseModelMultiple > impulses, boost::shared_ptr< CostModelSum > costs, const Scalar r_coeff=Scalar(0.), const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
Initialize the impulse forward-dynamics action model.
crocoddyl::DataCollectorMultibodyInImpulseTpl
Definition: impulses.hpp:32
crocoddyl::StateMultibodyTpl
State multibody representation.
Definition: fwd.hpp:305
crocoddyl::ActionModelAbstractTpl
Abstract class for action model.
Definition: action-base.hpp:59
crocoddyl::ActionDataImpulseFwdDynamicsTpl
Definition: impulse-fwddyn.hpp:199
crocoddyl::ActionModelImpulseFwdDynamicsTpl::get_impulses
const boost::shared_ptr< ImpulseModelMultiple > & get_impulses() const
Return the impulse model.
crocoddyl::ActionModelImpulseFwdDynamicsTpl::set_damping_factor
void set_damping_factor(const Scalar damping)
Modify the damping factor used in the operational space inertia matrix.
crocoddyl::ActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:223
crocoddyl::ActionModelImpulseFwdDynamicsTpl::get_damping_factor
const Scalar get_damping_factor() const
Return the damping factor used in the operational space inertia matrix.
crocoddyl::ActionModelImpulseFwdDynamicsTpl::checkData
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Check that the given data belongs to the impulse forward-dynamics data.
crocoddyl::ActionModelImpulseFwdDynamicsTpl::get_armature
const VectorXs & get_armature() const
Return the armature vector.
crocoddyl::ActionModelImpulseFwdDynamicsTpl::calcDiff
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 impulse dynamics, and cost function.