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" 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);
76 boost::shared_ptr<ActuationModelAbstract> actuation_;
77 boost::shared_ptr<CostModelSum> costs_;
78 pinocchio::ModelTpl<Scalar>& pinocchio_;
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;
92 template <
template <
typename Scalar>
class 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);
106 tmp_xstatic.setZero();
109 pinocchio::DataTpl<Scalar> pinocchio;
111 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
115 VectorXs tmp_xstatic;
134 #include <crocoddyl/multibody/actions/free-fwddyn.hxx> 136 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_ virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
VectorXs u_lb_
Lower control limits.
Abstract class for differential action model.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
std::size_t nu_
Control dimension.
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.
bool has_control_limits_
Indicates whether any of the control limits is finite.
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.
boost::shared_ptr< StateAbstract > state_
Model of the state.
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.
VectorXs u_ub_
Upper control limits.