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/core/utils/exception.hpp"
19 #include "crocoddyl/multibody/contact-base.hpp"
20 #include "crocoddyl/core/utils/deprecate.hpp"
22 #include "crocoddyl/multibody/frames-deprecated.hpp"
26 template <
typename _Scalar>
29 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 typedef _Scalar Scalar;
37 typedef typename MathBase::Vector2s Vector2s;
38 typedef typename MathBase::Vector3s Vector3s;
39 typedef typename MathBase::VectorXs VectorXs;
50 ContactModel3DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
const Vector3s& xref,
51 const std::size_t nu,
const Vector2s& gains = Vector2s::Zero());
63 ContactModel3DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
const Vector3s& xref,
64 const Vector2s& gains = Vector2s::Zero());
65 DEPRECATED(
"Use constructor which is not based on FrameTranslation.",
67 const std::size_t nu,
const Vector2s& gains = Vector2s::Zero());)
68 DEPRECATED(
"Use constructor which is not based on FrameTranslation.",
70 const Vector2s& gains = Vector2s::Zero());)
80 virtual void calc(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
89 virtual void calcDiff(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
97 virtual void updateForce(
const boost::shared_ptr<ContactDataAbstract>& data,
const VectorXs& force);
102 virtual boost::shared_ptr<ContactDataAbstract>
createData(pinocchio::DataTpl<Scalar>*
const data);
114 const Vector2s& get_gains()
const;
126 virtual void print(std::ostream& os)
const;
139 template <
typename _Scalar>
141 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143 typedef _Scalar Scalar;
146 typedef typename MathBase::Matrix3s Matrix3s;
147 typedef typename MathBase::Matrix6xs Matrix6xs;
148 typedef typename MathBase::Vector3s Vector3s;
150 template <
template <
typename Scalar>
class Model>
151 ContactData3DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
153 fJf(6, model->get_state()->get_nv()),
154 v_partial_dq(6, model->get_state()->get_nv()),
155 a_partial_dq(6, model->get_state()->get_nv()),
156 a_partial_dv(6, model->get_state()->get_nv()),
157 a_partial_da(6, model->get_state()->get_nv()),
158 fXjdv_dq(6, model->get_state()->get_nv()),
159 fXjda_dq(6, model->get_state()->get_nv()),
160 fXjda_dv(6, model->get_state()->get_nv()) {
161 frame = model->get_id();
162 jMf = model->get_state()->get_pinocchio()->frames[
frame].placement;
163 fXj =
jMf.inverse().toActionMatrix();
165 v_partial_dq.setZero();
166 a_partial_dq.setZero();
167 a_partial_dv.setZero();
168 a_partial_da.setZero();
190 pinocchio::MotionTpl<Scalar> v;
191 pinocchio::MotionTpl<Scalar> a;
193 Matrix6xs v_partial_dq;
194 Matrix6xs a_partial_dq;
195 Matrix6xs a_partial_dv;
196 Matrix6xs a_partial_da;
212 #include "crocoddyl/multibody/contacts/contact-3d.hxx"
214 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_