crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-com.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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/core/cost-base.hpp"
14 #include "crocoddyl/multibody/states/multibody.hpp"
15 #include "crocoddyl/multibody/impulse-base.hpp"
16 #include "crocoddyl/multibody/data/impulses.hpp"
17 #include "crocoddyl/multibody/frames.hpp"
18 #include "crocoddyl/core/utils/exception.hpp"
19 
20 namespace crocoddyl {
21 
39 template <typename _Scalar>
41  public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 
44  typedef _Scalar Scalar;
52  typedef typename MathBase::VectorXs VectorXs;
53 
60  CostModelImpulseCoMTpl(boost::shared_ptr<StateMultibody> state,
61  boost::shared_ptr<ActivationModelAbstract> activation);
62 
71  CostModelImpulseCoMTpl(boost::shared_ptr<StateMultibody> state);
72  virtual ~CostModelImpulseCoMTpl();
73 
81  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
82  const Eigen::Ref<const VectorXs>& u);
83 
91  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
92  const Eigen::Ref<const VectorXs>& u);
93 
97  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
98 
99  protected:
100  using Base::activation_;
101  using Base::nu_;
102  using Base::state_;
103  using Base::unone_;
104 
105  private:
106  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
107 };
108 
109 template <typename _Scalar>
110 struct CostDataImpulseCoMTpl : public CostDataAbstractTpl<_Scalar> {
111  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
112 
113  typedef _Scalar Scalar;
118  typedef typename MathBase::MatrixXs MatrixXs;
119  typedef typename MathBase::Matrix3xs Matrix3xs;
120 
121  template <template <typename Scalar> class Model>
122  CostDataImpulseCoMTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
123  : Base(model, data),
124  Arr_Rx(3, model->get_state()->get_nv()),
125  dvc_dq(3, model->get_state()->get_nv()),
126  ddv_dv(model->get_state()->get_nv(), model->get_state()->get_nv()) {
127  Arr_Rx.setZero();
128  dvc_dq.setZero();
129  ddv_dv.setZero();
130  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
131  pinocchio_internal = pinocchio::DataTpl<Scalar>(*state->get_pinocchio().get());
132  // Check that proper shared data has been passed
134  if (d == NULL) {
135  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibodyInImpulse");
136  }
137  pinocchio = d->pinocchio;
138  impulses = d->impulses;
139  }
140 
141  pinocchio::DataTpl<Scalar>* pinocchio;
142  boost::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> > impulses;
143  Matrix3xs Arr_Rx;
144  Matrix3xs dvc_dq;
145  MatrixXs ddv_dv;
146  pinocchio::DataTpl<Scalar> pinocchio_internal;
147  using Base::activation;
148  using Base::cost;
149  using Base::Lu;
150  using Base::Luu;
151  using Base::Lx;
152  using Base::Lxu;
153  using Base::Lxx;
154  using Base::r;
155  using Base::Ru;
156  using Base::Rx;
157  using Base::shared;
158 };
159 
160 } // namespace crocoddyl
161 
162 /* --- Details -------------------------------------------------------------- */
163 /* --- Details -------------------------------------------------------------- */
164 /* --- Details -------------------------------------------------------------- */
165 #include "crocoddyl/multibody/costs/impulse-com.hxx"
166 
167 #endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_COM_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the impulse CoM cost.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the impulse CoM cost data.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
CostModelImpulseCoMTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation)
Initialize the impulse CoM cost model.
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the impulse CoM cost.