9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_ 10 #define CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_ 14 #include "crocoddyl/multibody/fwd.hpp" 15 #include "crocoddyl/core/utils/exception.hpp" 16 #include "crocoddyl/core/action-base.hpp" 17 #include "crocoddyl/core/costs/cost-sum.hpp" 18 #include "crocoddyl/multibody/states/multibody.hpp" 19 #include "crocoddyl/multibody/actuations/floating-base.hpp" 20 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp" 21 #include "crocoddyl/multibody/data/impulses.hpp" 22 #include "crocoddyl/multibody/actions/impulse-fwddyn.hpp" 24 #include <pinocchio/algorithm/compute-all-terms.hpp> 25 #include <pinocchio/algorithm/frames.hpp> 26 #include <pinocchio/algorithm/contact-dynamics.hpp> 27 #include <pinocchio/algorithm/centroidal.hpp> 28 #include <pinocchio/algorithm/rnea-derivatives.hpp> 29 #include <pinocchio/algorithm/kinematics-derivatives.hpp> 33 template <
typename _Scalar>
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 typedef _Scalar Scalar;
46 typedef typename MathBase::VectorXs VectorXs;
47 typedef typename MathBase::MatrixXs MatrixXs;
50 boost::shared_ptr<ImpulseModelMultiple> impulses,
51 boost::shared_ptr<CostModelSum> costs,
const Scalar r_coeff = Scalar(0.),
52 const Scalar JMinvJt_damping = Scalar(0.),
const bool enable_force =
false);
55 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
56 const Eigen::Ref<const VectorXs>& u);
57 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
58 const Eigen::Ref<const VectorXs>& u);
59 virtual boost::shared_ptr<ActionDataAbstract>
createData();
60 virtual bool checkData(
const boost::shared_ptr<ActionDataAbstract>& data);
62 const boost::shared_ptr<ImpulseModelMultiple>& get_impulses()
const;
63 const boost::shared_ptr<CostModelSum>& get_costs()
const;
64 pinocchio::ModelTpl<Scalar>& get_pinocchio()
const;
65 const VectorXs& get_armature()
const;
66 const Scalar get_restitution_coefficient()
const;
67 const Scalar get_damping_factor()
const;
69 void set_armature(
const VectorXs& armature);
70 void set_restitution_coefficient(
const Scalar r_coeff);
71 void set_damping_factor(
const Scalar damping);
76 template <
class Scalar>
77 friend std::ostream& operator<<(std::ostream& os, const ActionModelImpulseFwdDynamicsTpl<Scalar>& model);
89 boost::shared_ptr<ImpulseModelMultiple> impulses_;
90 boost::shared_ptr<CostModelSum> costs_;
91 pinocchio::ModelTpl<Scalar>& pinocchio_;
95 Scalar JMinvJt_damping_;
97 pinocchio::MotionTpl<Scalar> gravity_;
100 template <
typename _Scalar>
102 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
103 typedef _Scalar Scalar;
106 typedef typename MathBase::VectorXs VectorXs;
107 typedef typename MathBase::MatrixXs MatrixXs;
109 template <
template <
typename Scalar>
class Model>
112 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
113 multibody(&pinocchio, model->get_impulses()->createData(&pinocchio)),
114 costs(model->get_costs()->createData(&multibody)),
115 vnone(model->get_state()->get_nv()),
116 Kinv(model->get_state()->get_nv() + model->get_impulses()->get_ni_total(),
117 model->get_state()->get_nv() + model->get_impulses()->get_ni_total()),
118 df_dx(model->get_impulses()->get_ni_total(), model->get_state()->get_ndx()),
119 dgrav_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
120 costs->shareMemory(
this);
127 pinocchio::DataTpl<Scalar> pinocchio;
129 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
141 #include <crocoddyl/multibody/actions/impulse-fwddyn.hxx> 143 #endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_ Abstract class for action model.
bool has_control_limits_
Indicates whether any of the control limits is finite.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
std::size_t nu_
Control dimension.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
State multibody representation.
Define a stack of impulse models.
VectorXs unone_
Neutral state.
VectorXs u_ub_
Upper control limits.
VectorXs u_lb_
Lower control limits.
boost::shared_ptr< StateAbstract > state_
Model of the state.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the next state and cost value.
std::size_t nr_
Dimension of the cost residual.