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"
61template <
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);
129 virtual bool checkData(
const boost::shared_ptr<ActionDataAbstract>& data);
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_;
198template <
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>
Abstract class for action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
Action model for impulse forward dynamics in multibody systems.
void set_restitution_coefficient(const Scalar r_coeff)
Modify the restituion coefficient.
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.
void set_damping_factor(const Scalar damping)
Modify the damping factor used in the operational space inertia matrix.
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.
const boost::shared_ptr< ImpulseModelMultiple > & get_impulses() const
Return the impulse model.
const Scalar get_restitution_coefficient() const
Return the restituion coefficient.
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.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the impulse forward-dynamics data.
void set_armature(const VectorXs &armature)
Modify the armature vector.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Check that the given data belongs to the impulse forward-dynamics data.
const Scalar get_damping_factor() const
Return the damping factor used in the operational space inertia matrix.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
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.
Summation of individual cost models.
Define a stack of impulse models.
State multibody representation.