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_ void set_damping_factor(const Scalar damping)
Modify the damping factor used in the operational space inertia matrix.
Abstract class for action model.
Action model for impulse forward dynamics in multibody systems.
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 impulse dynamics, and cost function.
void set_armature(const VectorXs &armature)
Modify the armature vector.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the impulse forward-dynamics data.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Check that the given data belongs to the impulse forward-dynamics data.
State multibody representation.
const Scalar get_damping_factor() const
Return the damping factor used in the operational space inertia matrix.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
const boost::shared_ptr< ImpulseModelMultiple > & get_impulses() const
Return the impulse model.
Define a stack of impulse models.
virtual void print(std::ostream &os) const
Print relevant information of the impulse forward-dynamics model.
const VectorXs & get_armature() const
Return the armature vector.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
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 system acceleration, and cost value.
void set_restitution_coefficient(const Scalar r_coeff)
Modify the restituion coefficient.
const Scalar get_restitution_coefficient() const
Return the restituion coefficient.
ActionModelImpulseFwdDynamicsTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ImpulseModelMultiple > impulses, boost::shared_ptr< CostModelSum > costs, const Scalar r_coeff=Scalar(0.), const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
Initialize the impulse forward-dynamics action model.