10 #ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
11 #define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
15 #ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
17 #include <pinocchio/codegen/cppadcg.hpp>
20 #include "crocoddyl/core/actuation-base.hpp"
21 #include "crocoddyl/core/constraints/constraint-manager.hpp"
22 #include "crocoddyl/core/costs/cost-sum.hpp"
23 #include "crocoddyl/core/diff-action-base.hpp"
24 #include "crocoddyl/core/utils/exception.hpp"
25 #include "crocoddyl/multibody/data/multibody.hpp"
26 #include "crocoddyl/multibody/fwd.hpp"
27 #include "crocoddyl/multibody/states/multibody.hpp"
58 template <
typename _Scalar>
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 typedef _Scalar Scalar;
74 typedef typename MathBase::VectorXs VectorXs;
75 typedef typename MathBase::MatrixXs MatrixXs;
78 boost::shared_ptr<StateMultibody> state,
79 boost::shared_ptr<ActuationModelAbstract> actuation,
80 boost::shared_ptr<CostModelSum> costs,
81 boost::shared_ptr<ConstraintModelManager> constraints =
nullptr);
94 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
95 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
103 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
104 const Eigen::Ref<const VectorXs>& x);
114 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
115 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
123 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
124 const Eigen::Ref<const VectorXs>& x);
131 virtual boost::shared_ptr<DifferentialActionDataAbstract>
createData();
137 const boost::shared_ptr<DifferentialActionDataAbstract>& data);
143 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
144 Eigen::Ref<VectorXs> u,
const Eigen::Ref<const VectorXs>& x,
145 const std::size_t maxiter = 100,
const Scalar tol = Scalar(1e-9));
175 const boost::shared_ptr<CostModelSum>&
get_costs()
const;
202 virtual void print(std::ostream& os)
const;
211 boost::shared_ptr<ActuationModelAbstract> actuation_;
212 boost::shared_ptr<CostModelSum> costs_;
213 boost::shared_ptr<ConstraintModelManager> constraints_;
214 pinocchio::ModelTpl<Scalar>& pinocchio_;
215 bool without_armature_;
219 template <
typename _Scalar>
222 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
223 typedef _Scalar Scalar;
229 typedef typename MathBase::VectorXs VectorXs;
230 typedef typename MathBase::MatrixXs MatrixXs;
232 template <
template <
typename Scalar>
class Model>
235 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
237 &pinocchio, model->get_actuation()->createData(),
238 boost::make_shared<JointDataAbstract>(
239 model->get_state(), model->get_actuation(), model->get_nu())),
240 costs(model->get_costs()->createData(&multibody)),
241 Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
242 u_drift(model->get_state()->get_nv()),
243 dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
244 tmp_xstatic(model->get_state()->get_nx()) {
245 multibody.joint->dtau_du.diagonal().setOnes();
246 costs->shareMemory(
this);
247 if (model->get_constraints() !=
nullptr) {
248 constraints = model->get_constraints()->createData(&multibody);
249 constraints->shareMemory(
this);
254 tmp_xstatic.setZero();
257 pinocchio::DataTpl<Scalar> pinocchio;
259 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
260 boost::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints;
264 VectorXs tmp_xstatic;
283 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
Abstract class for the actuation-mapping model.
Abstract class for differential action model.
VectorXs g_ub_
Lower bound of the inequality constraints.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs g_lb_
Lower bound of the inequality constraints.
Differential action model for free forward dynamics in multibody systems.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual std::size_t get_ng() const
Return the number of inequality constraints.
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.
const VectorXs & get_armature() const
Return the armature vector.
virtual std::size_t get_nh() const
Return the number of equality constraints.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the free forward-dynamics data.
virtual void print(std::ostream &os) const
Print relevant information of the free forward-dynamics model.
virtual const VectorXs & get_g_lb() const
Return the lower bound of the inequality constraints.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
void set_armature(const VectorXs &armature)
Modify the armature vector.
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.
const boost::shared_ptr< ConstraintModelManager > & get_constraints() const
Return the constraint model manager.
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.
const boost::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation model.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Check that the given data belongs to the free forward-dynamics data.
virtual const VectorXs & get_g_ub() const
Return the upper bound of the inequality constraints.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
State multibody representation.
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .
VectorXs xout
evolution state