9 #ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_ 10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_ 12 #include "crocoddyl/multibody/fwd.hpp" 13 #include "crocoddyl/multibody/contact-base.hpp" 14 #include "crocoddyl/multibody/frames.hpp" 16 #include <pinocchio/spatial/motion.hpp> 17 #include <pinocchio/multibody/data.hpp> 21 template <
typename _Scalar>
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef _Scalar Scalar;
33 typedef typename MathBase::Vector2s Vector2s;
34 typedef typename MathBase::Vector3s Vector3s;
35 typedef typename MathBase::VectorXs VectorXs;
36 typedef typename MathBase::MatrixXs MatrixXs;
38 ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
const FramePlacement& xref,
const std::size_t& nu,
39 const Vector2s& gains = Vector2s::Zero());
40 ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
const FramePlacement& xref,
41 const Vector2s& gains = Vector2s::Zero());
44 virtual void calc(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
45 virtual void calcDiff(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
46 virtual void updateForce(
const boost::shared_ptr<ContactDataAbstract>& data,
const VectorXs& force);
47 virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>*
const data);
49 const FramePlacement& get_Mref()
const;
50 const Vector2s& get_gains()
const;
62 template <
typename _Scalar>
64 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 typedef _Scalar Scalar;
69 typedef typename MathBase::Vector2s Vector2s;
70 typedef typename MathBase::Matrix3s Matrix3s;
71 typedef typename MathBase::Matrix6xs Matrix6xs;
72 typedef typename MathBase::Matrix6s Matrix6s;
74 typedef typename MathBase::Vector3s Vector3s;
75 typedef typename MathBase::VectorXs VectorXs;
76 typedef typename MathBase::MatrixXs MatrixXs;
78 template <
template <
typename Scalar>
class Model>
79 ContactData6DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
81 rMf(pinocchio::SE3Tpl<Scalar>::Identity()),
82 v_partial_dq(6, model->get_state()->get_nv()),
83 a_partial_dq(6, model->get_state()->get_nv()),
84 a_partial_dv(6, model->get_state()->get_nv()),
85 a_partial_da(6, model->get_state()->get_nv()) {
86 frame = model->get_Mref().frame;
87 joint = model->get_state()->get_pinocchio()->frames[frame].parent;
88 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
89 fXj = jMf.inverse().toActionMatrix();
90 v_partial_dq.setZero();
91 a_partial_dq.setZero();
92 a_partial_dv.setZero();
93 a_partial_da.setZero();
107 using Base::pinocchio;
109 pinocchio::SE3Tpl<Scalar> rMf;
110 pinocchio::MotionTpl<Scalar> v;
111 pinocchio::MotionTpl<Scalar> a;
112 Matrix6xs v_partial_dq;
113 Matrix6xs a_partial_dq;
114 Matrix6xs a_partial_dv;
115 Matrix6xs a_partial_da;
123 #include "crocoddyl/multibody/contacts/contact-6d.hxx" 125 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_