9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
10 #define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
14 #include "crocoddyl/multibody/fwd.hpp"
15 #include "crocoddyl/core/diff-action-base.hpp"
16 #include "crocoddyl/core/costs/cost-sum.hpp"
17 #include "crocoddyl/multibody/states/multibody.hpp"
18 #include "crocoddyl/core/actuation-base.hpp"
19 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
20 #include "crocoddyl/multibody/data/contacts.hpp"
24 template <
typename _Scalar>
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 typedef _Scalar Scalar;
38 typedef typename MathBase::VectorXs VectorXs;
39 typedef typename MathBase::MatrixXs MatrixXs;
42 boost::shared_ptr<ActuationModelAbstract> actuation,
43 boost::shared_ptr<ContactModelMultiple> contacts,
44 boost::shared_ptr<CostModelSum> costs,
45 const Scalar JMinvJt_damping = Scalar(0.),
46 const bool enable_force =
false);
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);
55 virtual void quasiStatic(
const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
56 const Eigen::Ref<const VectorXs>& x,
const std::size_t maxiter = 100,
57 const Scalar tol = Scalar(1e-9));
59 const boost::shared_ptr<ActuationModelAbstract>& get_actuation()
const;
60 const boost::shared_ptr<ContactModelMultiple>& get_contacts()
const;
61 const boost::shared_ptr<CostModelSum>& get_costs()
const;
62 pinocchio::ModelTpl<Scalar>& get_pinocchio()
const;
63 const VectorXs& get_armature()
const;
64 const Scalar get_damping_factor()
const;
66 void set_armature(
const VectorXs& armature);
67 void set_damping_factor(
const Scalar damping);
74 virtual void print(std::ostream& os)
const;
81 boost::shared_ptr<ActuationModelAbstract> actuation_;
82 boost::shared_ptr<ContactModelMultiple> contacts_;
83 boost::shared_ptr<CostModelSum> costs_;
84 pinocchio::ModelTpl<Scalar>& pinocchio_;
87 Scalar JMinvJt_damping_;
91 template <
typename _Scalar>
93 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94 typedef _Scalar Scalar;
97 typedef typename MathBase::VectorXs VectorXs;
98 typedef typename MathBase::MatrixXs MatrixXs;
100 template <
template <
typename Scalar>
class Model>
103 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
104 multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
105 costs(model->get_costs()->createData(&multibody)),
106 Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
107 model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
108 df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
109 df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
110 tmp_xstatic(model->get_state()->get_nx()),
111 tmp_Jstatic(model->get_state()->get_nv(), model->get_nu() + model->get_contacts()->get_nc_total()) {
112 costs->shareMemory(
this);
116 tmp_xstatic.setZero();
117 tmp_Jstatic.setZero();
118 pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
119 pinocchio.lambda_c.setZero();
122 pinocchio::DataTpl<Scalar> pinocchio;
124 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
128 VectorXs tmp_xstatic;
129 MatrixXs tmp_Jstatic;
148 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
Abstract class for differential action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
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.