crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
control-gravity-contact.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_COSTS_CONTROL_GRAVITY_CONTACT_HPP_
10 #define CROCODDYL_CORE_COSTS_CONTROL_GRAVITY_CONTACT_HPP_
11 
12 #include "crocoddyl/core/cost-base.hpp"
13 #include "crocoddyl/multibody/states/multibody.hpp"
14 #include "crocoddyl/multibody/data/contacts.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
16 
17 namespace crocoddyl {
18 
40 template <typename _Scalar>
42  public:
43  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 
45  typedef _Scalar Scalar;
55  typedef typename MathBase::VectorXs VectorXs;
56  typedef typename MathBase::MatrixXs MatrixXs;
57 
65  CostModelControlGravContactTpl(boost::shared_ptr<StateMultibody> state,
66  boost::shared_ptr<ActivationModelAbstract> activation, const std::size_t nu);
67 
76  CostModelControlGravContactTpl(boost::shared_ptr<StateMultibody> state,
77  boost::shared_ptr<ActivationModelAbstract> activation);
78 
88  CostModelControlGravContactTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nu);
89 
99  explicit CostModelControlGravContactTpl(boost::shared_ptr<StateMultibody> state);
101 
109  virtual void calc(const boost::shared_ptr<CostDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
110  const Eigen::Ref<const VectorXs> &u);
111 
119  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
120  const Eigen::Ref<const VectorXs> &u);
121 
122  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract *const data);
123 
124  protected:
125  using Base::activation_;
126  using Base::nu_;
127  using Base::state_;
128  using Base::unone_;
129 
130  private:
131  typename StateMultibody::PinocchioModel pin_model_;
132 };
133 
134 template <typename _Scalar>
136  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
137 
138  typedef _Scalar Scalar;
143  typedef pinocchio::DataTpl<Scalar> PinocchioData;
144  typedef typename MathBase::MatrixXs MatrixXs;
145 
146  template <template <typename Scalar> class Model>
147  CostDataControlGravContactTpl(Model<Scalar> *const model, DataCollectorAbstract *const data)
148  : Base(model, data),
149  dg_dq(model->get_state()->get_nv(), model->get_state()->get_nv()),
150  Arr_dgdq(model->get_state()->get_nv(), model->get_state()->get_nv()),
151  Arr_dtaudu(model->get_state()->get_nv(), model->get_nu()) {
152  dg_dq.setZero();
153  Arr_dgdq.setZero();
154  Arr_dtaudu.setZero();
155 
156  StateMultibody *sm = static_cast<StateMultibody *>(model->get_state().get());
157  pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
158 
159  // Check that proper shared data has been passed
161  dynamic_cast<DataCollectorActMultibodyInContactTpl<Scalar> *>(shared);
162  if (d == NULL) {
163  throw_pretty(
164  "Invalid argument: the shared data should be derived from "
165  "DataCollectorActMultibodyInContactTpl");
166  }
167  // Avoids data casting at runtime
168  // pinocchio = d->pinocchio;
169  fext = d->contacts->fext;
170  actuation = d->actuation;
171  }
172 
173  PinocchioData pinocchio;
174  pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> > fext;
175  boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation;
176  MatrixXs dg_dq;
177  MatrixXs Arr_dgdq;
178  MatrixXs Arr_dtaudu;
179  using Base::activation;
180  using Base::cost;
181  using Base::Lu;
182  using Base::Luu;
183  using Base::Lx;
184  using Base::Lxu;
185  using Base::Lxx;
186  using Base::r;
187  using Base::shared;
188 };
189 
190 } // namespace crocoddyl
191 
192 /* --- Details -------------------------------------------------------------- */
193 /* --- Details -------------------------------------------------------------- */
194 /* --- Details -------------------------------------------------------------- */
195 #include "crocoddyl/multibody/costs/control-gravity-contact.hxx"
196 
197 #endif // CROCODDYL_MULTIBODY_COSTS_CONTROL_GRAVITY_CONTACT_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
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 control gravity contact cost.
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the control gravity contact cost.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
State multibody representation.
Definition: fwd.hpp:215
CostModelControlGravContactTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const std::size_t nu)
Initialize the control gravity contact cost model.
const boost::shared_ptr< PinocchioModel > & get_pinocchio() const
Return the Pinocchio model (i.e., model of the rigid body system)
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cost data.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193