9 #ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_ 10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_ 12 #include <pinocchio/spatial/motion.hpp> 13 #include <pinocchio/multibody/data.hpp> 14 #include <pinocchio/algorithm/frames.hpp> 15 #include <pinocchio/algorithm/kinematics-derivatives.hpp> 17 #include "crocoddyl/multibody/fwd.hpp" 18 #include "crocoddyl/multibody/frames.hpp" 19 #include "crocoddyl/core/utils/exception.hpp" 20 #include "crocoddyl/multibody/contact-base.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 ContactModel2DTpl(boost::shared_ptr<StateMultibody> state,
const FrameTranslation& xref,
const std::size_t nu,
42 const Vector2s& gains = Vector2s::Zero());
43 ContactModel2DTpl(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;
60 virtual void print(std::ostream& os)
const;
68 FrameTranslation xref_;
72 template <
typename _Scalar>
74 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
76 typedef _Scalar Scalar;
79 typedef typename MathBase::Vector2s Vector2s;
80 typedef typename MathBase::Matrix3s Matrix3s;
81 typedef typename MathBase::Matrix6xs Matrix6xs;
83 typedef typename MathBase::Vector3s Vector3s;
84 typedef typename MathBase::VectorXs VectorXs;
85 typedef typename MathBase::MatrixXs MatrixXs;
87 template <
template <
typename Scalar>
class Model>
88 ContactData2DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
90 fJf(6, model->get_state()->get_nv()),
91 v_partial_dq(6, model->get_state()->get_nv()),
92 a_partial_dq(6, model->get_state()->get_nv()),
93 a_partial_dv(6, model->get_state()->get_nv()),
94 a_partial_da(6, model->get_state()->get_nv()),
95 fXjdv_dq(6, model->get_state()->get_nv()),
96 fXjda_dq(6, model->get_state()->get_nv()),
97 fXjda_dv(6, model->get_state()->get_nv()) {
98 frame = model->get_xref().id;
99 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
100 fXj = jMf.inverse().toActionMatrix();
102 v_partial_dq.setZero();
103 a_partial_dq.setZero();
104 a_partial_dv.setZero();
105 a_partial_da.setZero();
125 using Base::pinocchio;
127 pinocchio::MotionTpl<Scalar> v;
128 pinocchio::MotionTpl<Scalar> a;
130 Matrix6xs v_partial_dq;
131 Matrix6xs a_partial_dq;
132 Matrix6xs a_partial_dv;
133 Matrix6xs a_partial_da;
149 #include "crocoddyl/multibody/contacts/contact-2d.hxx" 151 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
State multibody representation.