9 #ifndef CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_ 10 #define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_ 12 #include <pinocchio/spatial/motion.hpp> 13 #include <pinocchio/multibody/data.hpp> 15 #include "crocoddyl/multibody/fwd.hpp" 16 #include "crocoddyl/multibody/impulse-base.hpp" 20 template <
typename _Scalar>
21 class ImpulseModel3DTpl :
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 ImpulseData3DTpl<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 ImpulseModel3DTpl(boost::shared_ptr<StateMultibody> state,
const std::size_t frame);
37 virtual ~ImpulseModel3DTpl();
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 std::size_t get_frame()
const;
51 virtual void print(std::ostream& os)
const;
60 template <
typename _Scalar>
61 struct ImpulseData3DTpl :
public ImpulseDataAbstractTpl<_Scalar> {
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 typedef _Scalar Scalar;
65 typedef MathBaseTpl<Scalar> MathBase;
66 typedef ImpulseDataAbstractTpl<Scalar> Base;
67 typedef typename MathBase::Matrix6xs Matrix6xs;
69 template <
template <
typename Scalar>
class Model>
70 ImpulseData3DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
72 fJf(6, model->get_state()->get_nv()),
73 v_partial_dq(6, model->get_state()->get_nv()),
74 v_partial_dv(6, model->get_state()->get_nv()) {
75 frame = model->get_frame();
76 jMf = model->get_state()->get_pinocchio()->frames[model->get_frame()].placement;
77 fXj = jMf.inverse().toActionMatrix();
79 v_partial_dq.setZero();
80 v_partial_dv.setZero();
89 using Base::pinocchio;
91 typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
93 Matrix6xs v_partial_dq;
94 Matrix6xs v_partial_dv;
102 #include "crocoddyl/multibody/impulses/impulse-3d.hxx" 104 #endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the 3d impulse model.