9 #ifndef CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_ 10 #define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_ 12 #include "crocoddyl/multibody/fwd.hpp" 13 #include "crocoddyl/multibody/impulse-base.hpp" 15 #include <pinocchio/spatial/motion.hpp> 16 #include <pinocchio/multibody/data.hpp> 20 template <
typename _Scalar>
21 class ImpulseModel6DTpl :
public ImpulseModelAbstractTpl<_Scalar> {
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 typedef _Scalar Scalar;
26 typedef MathBaseTpl<Scalar> MathBase;
27 typedef ImpulseModelAbstractTpl<Scalar> Base;
28 typedef ImpulseData6DTpl<Scalar> Data;
29 typedef StateMultibodyTpl<Scalar> StateMultibody;
30 typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
31 typedef typename MathBase::Vector2s Vector2s;
32 typedef typename MathBase::Vector3s Vector3s;
33 typedef typename MathBase::VectorXs VectorXs;
34 typedef typename MathBase::MatrixXs MatrixXs;
36 ImpulseModel6DTpl(boost::shared_ptr<StateMultibody> state,
const std::size_t& frame);
37 virtual ~ImpulseModel6DTpl();
39 virtual void calc(
const boost::shared_ptr<ImpulseDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
40 virtual void calcDiff(
const boost::shared_ptr<ImpulseDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
41 virtual void updateForce(
const boost::shared_ptr<ImpulseDataAbstract>& data,
const VectorXs& force);
42 virtual boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::DataTpl<Scalar>*
const data);
44 const std::size_t& get_frame()
const;
54 template <
typename _Scalar>
55 struct ImpulseData6DTpl :
public ImpulseDataAbstractTpl<_Scalar> {
56 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 typedef _Scalar Scalar;
58 typedef MathBaseTpl<Scalar> MathBase;
59 typedef ImpulseDataAbstractTpl<Scalar> Base;
60 typedef typename MathBase::Matrix6xs Matrix6xs;
62 template <
template <
typename Scalar>
class Model>
63 ImpulseData6DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
65 fJf(6, model->get_state()->get_nv()),
66 v_partial_dq(6, model->get_state()->get_nv()),
67 v_partial_dv(6, model->get_state()->get_nv()) {
68 frame = model->get_frame();
69 joint = model->get_state()->get_pinocchio()->frames[frame].parent;
70 jMf = model->get_state()->get_pinocchio()->frames[model->get_frame()].placement;
71 fXj = jMf.inverse().toActionMatrix();
73 v_partial_dq.setZero();
74 v_partial_dv.setZero();
84 using Base::pinocchio;
86 typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
88 Matrix6xs v_partial_dq;
89 Matrix6xs v_partial_dv;
97 #include "crocoddyl/multibody/impulses/impulse-6d.hxx" 99 #endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_