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/multibody/states/multibody.hpp"
17 #include "crocoddyl/multibody/actuations/floating-base.hpp"
18 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
19 #include "crocoddyl/multibody/data/contacts.hpp"
20 #include "crocoddyl/multibody/costs/cost-sum.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<ActuationModelFloatingBase> 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);
56 const boost::shared_ptr<ActuationModelFloatingBase>& get_actuation()
const;
57 const boost::shared_ptr<ContactModelMultiple>& get_contacts()
const;
58 const boost::shared_ptr<CostModelSum>& get_costs()
const;
59 pinocchio::ModelTpl<Scalar>& get_pinocchio()
const;
60 const VectorXs& get_armature()
const;
61 const Scalar& get_damping_factor()
const;
63 void set_armature(
const VectorXs& armature);
64 void set_damping_factor(
const Scalar& damping);
76 boost::shared_ptr<ActuationModelFloatingBase> actuation_;
77 boost::shared_ptr<ContactModelMultiple> contacts_;
78 boost::shared_ptr<CostModelSum> costs_;
79 pinocchio::ModelTpl<Scalar>& pinocchio_;
82 Scalar JMinvJt_damping_;
86 template <
typename _Scalar>
88 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89 typedef _Scalar Scalar;
92 typedef typename MathBase::VectorXs VectorXs;
93 typedef typename MathBase::MatrixXs MatrixXs;
95 template <
template <
typename Scalar>
class Model>
98 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
99 multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
100 costs(model->get_costs()->createData(&multibody)),
101 Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
102 model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
103 df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
104 df_du(model->get_contacts()->get_nc_total(), model->get_nu()) {
105 costs->shareMemory(
this);
111 pinocchio::DataTpl<Scalar> pinocchio;
113 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
135 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
137 #endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_