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"
49template <
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));
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_;
169template <
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>
Abstract class for the actuation-mapping model.
Summation of individual cost models.
Abstract class for differential action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
Differential action model for free forward dynamics in multibody systems.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the free forward-dynamics data.
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 contact dynamics, and cost function.
virtual void print(std::ostream &os) const
Print relevant information of the free forward-dynamics model.
const VectorXs & get_armature() const
Return the armature vector.
void set_armature(const VectorXs &armature)
Modify the armature vector.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost 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)
Check that the given data belongs to the free forward-dynamics data.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
const boost::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation 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.