9 #ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_ 10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_ 12 #include <pinocchio/spatial/motion.hpp> 13 #include <pinocchio/multibody/data.hpp> 15 #include "crocoddyl/multibody/fwd.hpp" 16 #include "crocoddyl/multibody/contact-base.hpp" 17 #include "crocoddyl/multibody/frames.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;
57 virtual void print(std::ostream& os)
const;
69 template <
typename _Scalar>
71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 typedef _Scalar Scalar;
76 typedef typename MathBase::Vector2s Vector2s;
77 typedef typename MathBase::Matrix3s Matrix3s;
78 typedef typename MathBase::Matrix6xs Matrix6xs;
79 typedef typename MathBase::Matrix6s Matrix6s;
81 typedef typename MathBase::Vector3s Vector3s;
82 typedef typename MathBase::VectorXs VectorXs;
83 typedef typename MathBase::MatrixXs MatrixXs;
85 template <
template <
typename Scalar>
class Model>
86 ContactData6DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
88 rMf(pinocchio::SE3Tpl<Scalar>::Identity()),
89 v_partial_dq(6, model->get_state()->get_nv()),
90 a_partial_dq(6, model->get_state()->get_nv()),
91 a_partial_dv(6, model->get_state()->get_nv()),
92 a_partial_da(6, model->get_state()->get_nv()) {
93 frame = model->get_Mref().id;
94 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
95 fXj = jMf.inverse().toActionMatrix();
96 v_partial_dq.setZero();
97 a_partial_dq.setZero();
98 a_partial_dv.setZero();
99 a_partial_da.setZero();
112 using Base::pinocchio;
114 pinocchio::SE3Tpl<Scalar> rMf;
115 pinocchio::MotionTpl<Scalar> v;
116 pinocchio::MotionTpl<Scalar> a;
117 Matrix6xs v_partial_dq;
118 Matrix6xs a_partial_dq;
119 Matrix6xs a_partial_dv;
120 Matrix6xs a_partial_da;
128 #include "crocoddyl/multibody/contacts/contact-6d.hxx" 130 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
State multibody representation.