Crocoddyl
free-fwddyn.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
11 #define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
12 
13 #include <stdexcept>
14 
15 #ifdef PINOCCHIO_WITH_CPPAD_SUPPORT // TODO(cmastalli): Removed after merging
16  // Pinocchio v.2.4.8
17 #include <pinocchio/codegen/cppadcg.hpp>
18 #endif
19 
20 #include "crocoddyl/core/actuation-base.hpp"
21 #include "crocoddyl/core/constraints/constraint-manager.hpp"
22 #include "crocoddyl/core/costs/cost-sum.hpp"
23 #include "crocoddyl/core/diff-action-base.hpp"
24 #include "crocoddyl/core/utils/exception.hpp"
25 #include "crocoddyl/multibody/data/multibody.hpp"
26 #include "crocoddyl/multibody/fwd.hpp"
27 #include "crocoddyl/multibody/states/multibody.hpp"
28 
29 namespace crocoddyl {
30 
58 template <typename _Scalar>
60  : public DifferentialActionModelAbstractTpl<_Scalar> {
61  public:
62  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 
64  typedef _Scalar Scalar;
74  typedef typename MathBase::VectorXs VectorXs;
75  typedef typename MathBase::MatrixXs MatrixXs;
76 
78  boost::shared_ptr<StateMultibody> state,
79  boost::shared_ptr<ActuationModelAbstract> actuation,
80  boost::shared_ptr<CostModelSum> costs,
81  boost::shared_ptr<ConstraintModelManager> constraints = nullptr);
83 
93  virtual void calc(
94  const boost::shared_ptr<DifferentialActionDataAbstract>& data,
95  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
96 
102  virtual void calc(
103  const boost::shared_ptr<DifferentialActionDataAbstract>& data,
104  const Eigen::Ref<const VectorXs>& x);
105 
113  virtual void calcDiff(
114  const boost::shared_ptr<DifferentialActionDataAbstract>& data,
115  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
116 
122  virtual void calcDiff(
123  const boost::shared_ptr<DifferentialActionDataAbstract>& data,
124  const Eigen::Ref<const VectorXs>& x);
125 
131  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
132 
136  virtual bool checkData(
137  const boost::shared_ptr<DifferentialActionDataAbstract>& data);
138 
142  virtual void quasiStatic(
143  const boost::shared_ptr<DifferentialActionDataAbstract>& data,
144  Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
145  const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
146 
150  virtual std::size_t get_ng() const;
151 
155  virtual std::size_t get_nh() const;
156 
160  virtual const VectorXs& get_g_lb() const;
161 
165  virtual const VectorXs& get_g_ub() const;
166 
170  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
171 
175  const boost::shared_ptr<CostModelSum>& get_costs() const;
176 
180  const boost::shared_ptr<ConstraintModelManager>& get_constraints() const;
181 
185  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
186 
190  const VectorXs& get_armature() const;
191 
195  void set_armature(const VectorXs& armature);
196 
202  virtual void print(std::ostream& os) const;
203 
204  protected:
205  using Base::g_lb_;
206  using Base::g_ub_;
207  using Base::nu_;
208  using Base::state_;
209 
210  private:
211  boost::shared_ptr<ActuationModelAbstract> actuation_;
212  boost::shared_ptr<CostModelSum> costs_;
213  boost::shared_ptr<ConstraintModelManager> constraints_;
214  pinocchio::ModelTpl<Scalar>& pinocchio_;
215  bool without_armature_;
216  VectorXs armature_;
217 };
218 
219 template <typename _Scalar>
221  : public DifferentialActionDataAbstractTpl<_Scalar> {
222  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
223  typedef _Scalar Scalar;
229  typedef typename MathBase::VectorXs VectorXs;
230  typedef typename MathBase::MatrixXs MatrixXs;
231 
232  template <template <typename Scalar> class Model>
233  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
234  : Base(model),
235  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
236  multibody(
237  &pinocchio, model->get_actuation()->createData(),
238  boost::make_shared<JointDataAbstract>(
239  model->get_state(), model->get_actuation(), model->get_nu())),
240  costs(model->get_costs()->createData(&multibody)),
241  Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
242  u_drift(model->get_state()->get_nv()),
243  dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
244  tmp_xstatic(model->get_state()->get_nx()) {
245  multibody.joint->dtau_du.diagonal().setOnes();
246  costs->shareMemory(this);
247  if (model->get_constraints() != nullptr) {
248  constraints = model->get_constraints()->createData(&multibody);
249  constraints->shareMemory(this);
250  }
251  Minv.setZero();
252  u_drift.setZero();
253  dtau_dx.setZero();
254  tmp_xstatic.setZero();
255  }
256 
257  pinocchio::DataTpl<Scalar> pinocchio;
259  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
260  boost::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints;
261  MatrixXs Minv;
262  VectorXs u_drift;
263  MatrixXs dtau_dx;
264  VectorXs tmp_xstatic;
265 
266  using Base::cost;
267  using Base::Fu;
268  using Base::Fx;
269  using Base::Lu;
270  using Base::Luu;
271  using Base::Lx;
272  using Base::Lxu;
273  using Base::Lxx;
274  using Base::r;
275  using Base::xout;
276 };
277 
278 } // namespace crocoddyl
279 
280 /* --- Details -------------------------------------------------------------- */
281 /* --- Details -------------------------------------------------------------- */
282 /* --- Details -------------------------------------------------------------- */
283 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
284 
285 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
Abstract class for the actuation-mapping model.
Abstract class for differential action model.
VectorXs g_ub_
Lower bound of the inequality constraints.
boost::shared_ptr< StateAbstract > state_
Model of the state.
VectorXs g_lb_
Lower bound of the inequality constraints.
Differential action model for free forward dynamics in multibody systems.
Definition: free-fwddyn.hpp:60
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual std::size_t get_ng() const
Return the number of inequality constraints.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the contact dynamics, and cost function.
const VectorXs & get_armature() const
Return the armature vector.
virtual std::size_t get_nh() const
Return the number of equality constraints.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the free forward-dynamics data.
virtual void print(std::ostream &os) const
Print relevant information of the free forward-dynamics model.
virtual const VectorXs & get_g_lb() const
Return the lower bound of the inequality constraints.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
void set_armature(const VectorXs &armature)
Modify the armature vector.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the system acceleration, and cost value.
const boost::shared_ptr< ConstraintModelManager > & get_constraints() const
Return the constraint model manager.
virtual void quasiStatic(const boost::shared_ptr< DifferentialActionDataAbstract > &data, Eigen::Ref< VectorXs > u, const Eigen::Ref< const VectorXs > &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9))
Computes the quasic static commands.
const boost::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation model.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Check that the given data belongs to the free forward-dynamics data.
virtual const VectorXs & get_g_ub() const
Return the upper bound of the inequality constraints.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
State multibody representation.
Definition: multibody.hpp:35
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .