crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
free-fwddyn.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_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 
66  protected:
68  using Base::nr_;
69  using Base::nu_;
70  using Base::state_;
71  using Base::u_lb_;
72  using Base::u_ub_;
73  using Base::unone_;
74 
75  private:
76  boost::shared_ptr<ActuationModelAbstract> actuation_;
77  boost::shared_ptr<CostModelSum> costs_;
78  pinocchio::ModelTpl<Scalar>& pinocchio_;
79  bool without_armature_;
80  VectorXs armature_;
81 };
82 
83 template <typename _Scalar>
85  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
86  typedef _Scalar Scalar;
89  typedef typename MathBase::VectorXs VectorXs;
90  typedef typename MathBase::MatrixXs MatrixXs;
91 
92  template <template <typename Scalar> class Model>
93  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
94  : Base(model),
95  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
96  multibody(&pinocchio, model->get_actuation()->createData()),
97  costs(model->get_costs()->createData(&multibody)),
98  Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
99  u_drift(model->get_nu()),
100  dtau_dx(model->get_nu(), model->get_state()->get_ndx()),
101  tmp_xstatic(model->get_state()->get_nx()) {
102  costs->shareMemory(this);
103  Minv.setZero();
104  u_drift.setZero();
105  dtau_dx.setZero();
106  tmp_xstatic.setZero();
107  }
108 
109  pinocchio::DataTpl<Scalar> pinocchio;
111  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
112  MatrixXs Minv;
113  VectorXs u_drift;
114  MatrixXs dtau_dx;
115  VectorXs tmp_xstatic;
116 
117  using Base::cost;
118  using Base::Fu;
119  using Base::Fx;
120  using Base::Lu;
121  using Base::Luu;
122  using Base::Lx;
123  using Base::Lxu;
124  using Base::Lxx;
125  using Base::r;
126  using Base::xout;
127 };
128 
129 } // namespace crocoddyl
130 
131 /* --- Details -------------------------------------------------------------- */
132 /* --- Details -------------------------------------------------------------- */
133 /* --- Details -------------------------------------------------------------- */
134 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
135 
136 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
crocoddyl::DifferentialActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: diff-action-base.hpp:258
crocoddyl::DifferentialActionModelAbstractTpl::nr_
std::size_t nr_
Dimension of the cost residual.
Definition: diff-action-base.hpp:207
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:255
crocoddyl::CostModelSumTpl< Scalar >
crocoddyl::DifferentialActionModelAbstractTpl::has_control_limits_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: diff-action-base.hpp:212
crocoddyl::DifferentialActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: diff-action-base.hpp:256
crocoddyl::DifferentialActionModelAbstractTpl::unone_
VectorXs unone_
Neutral state.
Definition: diff-action-base.hpp:209
crocoddyl::DifferentialActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: diff-action-base.hpp:252
crocoddyl::DifferentialActionDataAbstractTpl::xout
VectorXs xout
evolution state
Definition: diff-action-base.hpp:251
crocoddyl::DifferentialActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: diff-action-base.hpp:206
crocoddyl::StateMultibodyTpl
State multibody representation.
Definition: fwd.hpp:215
crocoddyl::DifferentialActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: diff-action-base.hpp:254
crocoddyl::DifferentialActionDataAbstractTpl
Definition: diff-action-base.hpp:218
crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl
Definition: free-fwddyn.hpp:29
crocoddyl::DifferentialActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: diff-action-base.hpp:208
crocoddyl::DifferentialActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: diff-action-base.hpp:259
crocoddyl::DifferentialActionDataFreeFwdDynamicsTpl
Definition: free-fwddyn.hpp:84
crocoddyl::DifferentialActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: diff-action-base.hpp:257
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:250
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:253
crocoddyl::DifferentialActionModelAbstractTpl::u_lb_
VectorXs u_lb_
Lower control limits.
Definition: diff-action-base.hpp:210
crocoddyl::DifferentialActionModelAbstractTpl::u_ub_
VectorXs u_ub_
Upper control limits.
Definition: diff-action-base.hpp:211