crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
free-fwddyn.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
10 #define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
11 
12 #include <stdexcept>
13 
14 #ifdef PINOCCHIO_WITH_CPPAD_SUPPORT // TODO(cmastalli): Removed after merging Pinocchio v.2.4.8
15 #include <pinocchio/codegen/cppadcg.hpp>
16 #endif
17 
18 #include "crocoddyl/multibody/fwd.hpp"
19 #include "crocoddyl/core/diff-action-base.hpp"
20 #include "crocoddyl/core/costs/cost-sum.hpp"
21 #include "crocoddyl/core/actuation-base.hpp"
22 #include "crocoddyl/multibody/data/multibody.hpp"
23 #include "crocoddyl/multibody/states/multibody.hpp"
24 #include "crocoddyl/core/utils/exception.hpp"
25 
26 namespace crocoddyl {
27 
28 template <typename _Scalar>
30  public:
31  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 
33  typedef _Scalar Scalar;
41  typedef typename MathBase::VectorXs VectorXs;
42  typedef typename MathBase::MatrixXs MatrixXs;
43 
44  DifferentialActionModelFreeFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
45  boost::shared_ptr<ActuationModelAbstract> actuation,
46  boost::shared_ptr<CostModelSum> costs);
48 
49  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
50  const Eigen::Ref<const VectorXs>& u);
51  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
52  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
53  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
54  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
55 
56  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
57  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
58  const Scalar tol = Scalar(1e-9));
59 
60  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
61  const boost::shared_ptr<CostModelSum>& get_costs() const;
62  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
63  const VectorXs& get_armature() const;
64  void set_armature(const VectorXs& armature);
65 
71  virtual void print(std::ostream& os) const;
72 
73  protected:
74  using Base::nu_;
75  using Base::state_;
76 
77  private:
78  boost::shared_ptr<ActuationModelAbstract> actuation_;
79  boost::shared_ptr<CostModelSum> costs_;
80  pinocchio::ModelTpl<Scalar>& pinocchio_;
81  bool without_armature_;
82  VectorXs armature_;
83 };
84 
85 template <typename _Scalar>
87  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
88  typedef _Scalar Scalar;
91  typedef typename MathBase::VectorXs VectorXs;
92  typedef typename MathBase::MatrixXs MatrixXs;
93 
94  template <template <typename Scalar> class Model>
95  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
96  : Base(model),
97  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
98  multibody(&pinocchio, model->get_actuation()->createData()),
99  costs(model->get_costs()->createData(&multibody)),
100  Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
101  u_drift(model->get_nu()),
102  dtau_dx(model->get_nu(), model->get_state()->get_ndx()),
103  tmp_xstatic(model->get_state()->get_nx()) {
104  costs->shareMemory(this);
105  Minv.setZero();
106  u_drift.setZero();
107  dtau_dx.setZero();
108  tmp_xstatic.setZero();
109  }
110 
111  pinocchio::DataTpl<Scalar> pinocchio;
113  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
114  MatrixXs Minv;
115  VectorXs u_drift;
116  MatrixXs dtau_dx;
117  VectorXs tmp_xstatic;
118 
119  using Base::cost;
120  using Base::Fu;
121  using Base::Fx;
122  using Base::Lu;
123  using Base::Luu;
124  using Base::Lx;
125  using Base::Lxu;
126  using Base::Lxx;
127  using Base::r;
128  using Base::xout;
129 };
130 
131 } // namespace crocoddyl
132 
133 /* --- Details -------------------------------------------------------------- */
134 /* --- Details -------------------------------------------------------------- */
135 /* --- Details -------------------------------------------------------------- */
136 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
137 
138 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
crocoddyl::DifferentialActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: diff-action-base.hpp:264
crocoddyl::ActuationModelAbstractTpl
Definition: actuation-base.hpp:25
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl::quasiStatic
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.
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl::calcDiff
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 dynamics and cost functions.
crocoddyl::DifferentialActionDataAbstractTpl::Lx
VectorXs Lx
Jacobian of the cost function.
Definition: diff-action-base.hpp:261
crocoddyl::CostModelSumTpl< Scalar >
crocoddyl::DifferentialActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: diff-action-base.hpp:262
crocoddyl::DifferentialActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: diff-action-base.hpp:258
crocoddyl::DifferentialActionDataAbstractTpl::xout
VectorXs xout
evolution state
Definition: diff-action-base.hpp:257
crocoddyl::DifferentialActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: diff-action-base.hpp:212
crocoddyl::StateMultibodyTpl
State multibody representation.
Definition: fwd.hpp:300
crocoddyl::DifferentialActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: diff-action-base.hpp:260
crocoddyl::DifferentialActionDataAbstractTpl
Definition: diff-action-base.hpp:224
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl
Definition: free-fwddyn.hpp:29
crocoddyl::DifferentialActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: diff-action-base.hpp:214
crocoddyl::DifferentialActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: diff-action-base.hpp:265
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the free forward-dynamics model.
crocoddyl::DifferentialActionDataFreeFwdDynamicsTpl
Definition: free-fwddyn.hpp:86
crocoddyl::DifferentialActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: diff-action-base.hpp:263
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl::checkData
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
crocoddyl::DifferentialActionModelAbstractTpl
Abstract class for differential action model.
Definition: diff-action-base.hpp:54
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl::calc
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.
crocoddyl::DifferentialActionDataAbstractTpl::cost
Scalar cost
cost value
Definition: diff-action-base.hpp:256
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl::createData
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
crocoddyl::DataCollectorActMultibodyTpl
Definition: multibody.hpp:31
crocoddyl::DifferentialActionDataAbstractTpl::Fu
MatrixXs Fu
Jacobian of the dynamics.
Definition: diff-action-base.hpp:259