9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
10 #define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
14 #ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
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"
28 template <
typename _Scalar>
31 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33 typedef _Scalar Scalar;
41 typedef typename MathBase::VectorXs VectorXs;
42 typedef typename MathBase::MatrixXs MatrixXs;
45 boost::shared_ptr<ActuationModelAbstract> actuation,
46 boost::shared_ptr<CostModelSum> costs);
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);
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));
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);
71 virtual void print(std::ostream& os)
const;
78 boost::shared_ptr<ActuationModelAbstract> actuation_;
79 boost::shared_ptr<CostModelSum> costs_;
80 pinocchio::ModelTpl<Scalar>& pinocchio_;
81 bool without_armature_;
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;
94 template <
template <
typename Scalar>
class 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);
108 tmp_xstatic.setZero();
111 pinocchio::DataTpl<Scalar> pinocchio;
113 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
117 VectorXs tmp_xstatic;
136 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
Abstract class for differential action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
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.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
virtual void print(std::ostream &os) const
Print relevant information of the free forward-dynamics model.
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.
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.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
State multibody representation.
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
VectorXs xout
evolution state
MatrixXs Lxu
Hessian of the cost function.