9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
10 #define CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
14 #include <pinocchio/algorithm/compute-all-terms.hpp>
15 #include <pinocchio/algorithm/frames.hpp>
16 #include <pinocchio/algorithm/contact-dynamics.hpp>
17 #include <pinocchio/algorithm/centroidal.hpp>
18 #include <pinocchio/algorithm/rnea-derivatives.hpp>
19 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
21 #include "crocoddyl/multibody/fwd.hpp"
22 #include "crocoddyl/core/utils/exception.hpp"
23 #include "crocoddyl/core/action-base.hpp"
24 #include "crocoddyl/core/costs/cost-sum.hpp"
25 #include "crocoddyl/multibody/states/multibody.hpp"
26 #include "crocoddyl/multibody/actuations/floating-base.hpp"
27 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
28 #include "crocoddyl/multibody/data/impulses.hpp"
29 #include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
61 template <
typename _Scalar>
64 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 typedef _Scalar Scalar;
74 typedef typename MathBase::VectorXs VectorXs;
75 typedef typename MathBase::MatrixXs MatrixXs;
92 boost::shared_ptr<ImpulseModelMultiple> impulses,
93 boost::shared_ptr<CostModelSum> costs,
const Scalar r_coeff = Scalar(0.),
94 const Scalar JMinvJt_damping = Scalar(0.),
const bool enable_force =
false);
106 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
107 const Eigen::Ref<const VectorXs>& u);
116 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
117 const Eigen::Ref<const VectorXs>& u);
124 virtual boost::shared_ptr<ActionDataAbstract>
createData();
129 virtual bool checkData(
const boost::shared_ptr<ActionDataAbstract>& data);
134 const boost::shared_ptr<ImpulseModelMultiple>&
get_impulses()
const;
139 const boost::shared_ptr<CostModelSum>&
get_costs()
const;
181 virtual void print(std::ostream& os)
const;
187 boost::shared_ptr<ImpulseModelMultiple> impulses_;
188 boost::shared_ptr<CostModelSum> costs_;
189 pinocchio::ModelTpl<Scalar>& pinocchio_;
193 Scalar JMinvJt_damping_;
195 pinocchio::MotionTpl<Scalar> gravity_;
198 template <
typename _Scalar>
200 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
201 typedef _Scalar Scalar;
204 typedef typename MathBase::VectorXs VectorXs;
205 typedef typename MathBase::MatrixXs MatrixXs;
207 template <
template <
typename Scalar>
class Model>
210 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
211 multibody(&pinocchio, model->get_impulses()->createData(&pinocchio)),
212 costs(model->get_costs()->createData(&multibody)),
213 vnone(model->get_state()->get_nv()),
214 Kinv(model->get_state()->get_nv() + model->get_impulses()->get_nc_total(),
215 model->get_state()->get_nv() + model->get_impulses()->get_nc_total()),
216 df_dx(model->get_impulses()->get_nc_total(), model->get_state()->get_ndx()),
217 dgrav_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
218 costs->shareMemory(
this);
225 pinocchio::DataTpl<Scalar> pinocchio;
227 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
239 #include <crocoddyl/multibody/actions/impulse-fwddyn.hxx>
241 #endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_