crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-com.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_COSTS_IMPULSE_COM_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_IMPULSE_COM_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/cost-base.hpp"
14 #include "crocoddyl/multibody/impulse-base.hpp"
15 #include "crocoddyl/multibody/data/impulses.hpp"
16 #include "crocoddyl/multibody/frames.hpp"
17 #include "crocoddyl/core/utils/exception.hpp"
18 
19 namespace crocoddyl {
20 
21 template <typename _Scalar>
23  public:
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
35  typedef typename MathBase::Vector3s Vector3s;
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::MatrixXs MatrixXs;
38 
39  CostModelImpulseCoMTpl(boost::shared_ptr<StateMultibody> state,
40  boost::shared_ptr<ActivationModelAbstract> activation);
41  CostModelImpulseCoMTpl(boost::shared_ptr<StateMultibody> state);
43 
44  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
45  const Eigen::Ref<const VectorXs>& u);
46  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
47  const Eigen::Ref<const VectorXs>& u);
48  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
49 
50  protected:
51  using Base::activation_;
52  using Base::nu_;
53  using Base::state_;
54  using Base::unone_;
55 };
56 
57 template <typename _Scalar>
58 struct CostDataImpulseCoMTpl : public CostDataAbstractTpl<_Scalar> {
59  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60 
61  typedef _Scalar Scalar;
65  typedef typename MathBase::VectorXs VectorXs;
66  typedef typename MathBase::MatrixXs MatrixXs;
67  typedef typename MathBase::Matrix3xs Matrix3xs;
68  typedef typename MathBase::Matrix6xs Matrix6xs;
69  typedef typename MathBase::Matrix6s Matrix6s;
70  typedef typename MathBase::Vector6s Vector6s;
71 
72  template <template <typename Scalar> class Model>
73  CostDataImpulseCoMTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
74  : Base(model, data),
75  Arr_Rx(3, model->get_state()->get_nv()),
76  dvc_dq(3, model->get_state()->get_nv()),
77  ddv_dv(model->get_state()->get_nv(), model->get_state()->get_nv()),
78  pinocchio_internal(pinocchio::DataTpl<Scalar>(*model->get_state()->get_pinocchio().get())) {
79  Arr_Rx.setZero();
80  dvc_dq.setZero();
81  ddv_dv.setZero();
82  // Check that proper shared data has been passed
84  if (d == NULL) {
85  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibodyInImpulse");
86  }
87  pinocchio = d->pinocchio;
88  impulses = d->impulses;
89  }
90 
91  pinocchio::DataTpl<Scalar>* pinocchio;
92  boost::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> > impulses;
93  Matrix3xs Arr_Rx;
94  Matrix3xs dvc_dq;
95  MatrixXs ddv_dv;
96  pinocchio::DataTpl<Scalar> pinocchio_internal;
97  using Base::activation;
98  using Base::cost;
99  using Base::Lu;
100  using Base::Luu;
101  using Base::Lx;
102  using Base::Lxu;
103  using Base::Lxx;
104  using Base::r;
105  using Base::Ru;
106  using Base::Rx;
107  using Base::shared;
108 };
109 
110 } // namespace crocoddyl
111 
112 /* --- Details -------------------------------------------------------------- */
113 /* --- Details -------------------------------------------------------------- */
114 /* --- Details -------------------------------------------------------------- */
115 #include "crocoddyl/multibody/costs/impulse-com.hxx"
116 
117 #endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_COM_HPP_