9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
10 #define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
14 #ifdef PINOCCHIO_WITH_CPPAD_SUPPORT // TODO(cmastalli): Removed after merging Pinocchio v.2.4.8
15 #include <pinocchio/codegen/cppadcg.hpp>
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"
49 template <
typename _Scalar>
52 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54 typedef _Scalar Scalar;
62 typedef typename MathBase::VectorXs VectorXs;
63 typedef typename MathBase::MatrixXs MatrixXs;
66 boost::shared_ptr<ActuationModelAbstract> actuation,
67 boost::shared_ptr<CostModelSum> costs);
79 virtual void calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
80 const Eigen::Ref<const VectorXs>& u);
86 virtual void calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x);
96 virtual void calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
97 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
103 virtual void calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
104 const Eigen::Ref<const VectorXs>& x);
111 virtual boost::shared_ptr<DifferentialActionDataAbstract>
createData();
116 virtual bool checkData(
const boost::shared_ptr<DifferentialActionDataAbstract>& data);
121 virtual void quasiStatic(
const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
122 const Eigen::Ref<const VectorXs>& x,
const std::size_t maxiter = 100,
123 const Scalar tol = Scalar(1e-9));
128 const boost::shared_ptr<ActuationModelAbstract>&
get_actuation()
const;
133 const boost::shared_ptr<CostModelSum>&
get_costs()
const;
155 virtual void print(std::ostream& os)
const;
162 boost::shared_ptr<ActuationModelAbstract> actuation_;
163 boost::shared_ptr<CostModelSum> costs_;
164 pinocchio::ModelTpl<Scalar>& pinocchio_;
165 bool without_armature_;
169 template <
typename _Scalar>
171 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
172 typedef _Scalar Scalar;
175 typedef typename MathBase::VectorXs VectorXs;
176 typedef typename MathBase::MatrixXs MatrixXs;
178 template <
template <
typename Scalar>
class Model>
181 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
182 multibody(&pinocchio, model->get_actuation()->createData()),
183 costs(model->get_costs()->createData(&multibody)),
184 Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
185 u_drift(model->get_nu()),
186 dtau_dx(model->get_nu(), model->get_state()->get_ndx()),
187 tmp_xstatic(model->get_state()->get_nx()) {
188 costs->shareMemory(
this);
192 tmp_xstatic.setZero();
195 pinocchio::DataTpl<Scalar> pinocchio;
197 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
201 VectorXs tmp_xstatic;
220 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
222 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_