9 #ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_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;
42 const Vector2s& gains = Vector2s::Zero());
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);
53 const Vector2s& get_gains()
const;
60 virtual void print(std::ostream& os)
const;
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 ContactData3DTpl(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();
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-3d.hxx"
151 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_