9 #ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_ 10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_ 12 #include "crocoddyl/multibody/fwd.hpp" 13 #include "crocoddyl/multibody/frames.hpp" 14 #include "crocoddyl/core/utils/exception.hpp" 15 #include "crocoddyl/multibody/contact-base.hpp" 17 #include <pinocchio/spatial/motion.hpp> 18 #include <pinocchio/multibody/data.hpp> 19 #include <pinocchio/algorithm/frames.hpp> 20 #include <pinocchio/algorithm/kinematics-derivatives.hpp> 24 template <
typename _Scalar>
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 typedef _Scalar Scalar;
36 typedef typename MathBase::Vector2s Vector2s;
37 typedef typename MathBase::Vector3s Vector3s;
38 typedef typename MathBase::VectorXs VectorXs;
39 typedef typename MathBase::MatrixXs MatrixXs;
41 ContactModel3DTpl(boost::shared_ptr<StateMultibody> state,
const FrameTranslation& xref,
const std::size_t& nu,
42 const Vector2s& gains = Vector2s::Zero());
43 ContactModel3DTpl(boost::shared_ptr<StateMultibody> state,
const FrameTranslation& xref,
44 const Vector2s& gains = Vector2s::Zero());
47 virtual void calc(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
48 virtual void calcDiff(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
49 virtual void updateForce(
const boost::shared_ptr<ContactDataAbstract>& data,
const VectorXs& force);
50 virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>*
const data);
52 const FrameTranslation& get_xref()
const;
53 const Vector2s& get_gains()
const;
61 FrameTranslation xref_;
65 template <
typename _Scalar>
67 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 typedef _Scalar Scalar;
72 typedef typename MathBase::Vector2s Vector2s;
73 typedef typename MathBase::Matrix3s Matrix3s;
74 typedef typename MathBase::Matrix6xs Matrix6xs;
76 typedef typename MathBase::Vector3s Vector3s;
77 typedef typename MathBase::VectorXs VectorXs;
78 typedef typename MathBase::MatrixXs MatrixXs;
80 template <
template <
typename Scalar>
class Model>
81 ContactData3DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
83 fJf(6, model->get_state()->get_nv()),
84 v_partial_dq(6, model->get_state()->get_nv()),
85 a_partial_dq(6, model->get_state()->get_nv()),
86 a_partial_dv(6, model->get_state()->get_nv()),
87 a_partial_da(6, model->get_state()->get_nv()),
88 fXjdv_dq(6, model->get_state()->get_nv()),
89 fXjda_dq(6, model->get_state()->get_nv()),
90 fXjda_dv(6, model->get_state()->get_nv()) {
91 frame = model->get_xref().frame;
92 joint = model->get_state()->get_pinocchio()->frames[frame].parent;
93 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
94 fXj = jMf.inverse().toActionMatrix();
96 v_partial_dq.setZero();
97 a_partial_dq.setZero();
98 a_partial_dv.setZero();
99 a_partial_da.setZero();
120 using Base::pinocchio;
122 pinocchio::MotionTpl<Scalar> v;
123 pinocchio::MotionTpl<Scalar> a;
125 Matrix6xs v_partial_dq;
126 Matrix6xs a_partial_dq;
127 Matrix6xs a_partial_dv;
128 Matrix6xs a_partial_da;
144 #include "crocoddyl/multibody/contacts/contact-3d.hxx" 146 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_