9 #ifndef CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_ 10 #define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_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 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 std::size_t get_frame()
const;
51 virtual void print(std::ostream& os)
const;
60 template <
typename _Scalar>
61 struct ImpulseData6DTpl :
public ImpulseDataAbstractTpl<_Scalar> {
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 typedef _Scalar Scalar;
64 typedef MathBaseTpl<Scalar> MathBase;
65 typedef ImpulseDataAbstractTpl<Scalar> Base;
66 typedef typename MathBase::Matrix6xs Matrix6xs;
68 template <
template <
typename Scalar>
class Model>
69 ImpulseData6DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
71 fJf(6, model->get_state()->get_nv()),
72 v_partial_dq(6, model->get_state()->get_nv()),
73 v_partial_dv(6, model->get_state()->get_nv()) {
74 frame = model->get_frame();
75 jMf = model->get_state()->get_pinocchio()->frames[model->get_frame()].placement;
76 fXj =
jMf.inverse().toActionMatrix();
78 v_partial_dq.setZero();
79 v_partial_dv.setZero();
90 typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
92 Matrix6xs v_partial_dq;
93 Matrix6xs v_partial_dv;
101 #include "crocoddyl/multibody/impulses/impulse-6d.hxx" 103 #endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_ pinocchio::FrameIndex frame
Frame index of the contact frame.
MatrixXs df_dx
Jacobian of the contact forces.
virtual void print(std::ostream &os) const
Print relevant information of the 6d impulse model.
pinocchio::SE3Tpl< Scalar > jMf
Local frame placement of the contact frame.
pinocchio::ForceTpl< Scalar > f
MatrixXs Jc
Contact Jacobian.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.