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/core/utils/deprecate.hpp"
19 #include "crocoddyl/multibody/frames-deprecated.hpp"
23 template <
typename _Scalar>
26 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 typedef _Scalar Scalar;
34 typedef pinocchio::SE3Tpl<Scalar> SE3;
35 typedef typename MathBase::Vector2s Vector2s;
36 typedef typename MathBase::Vector3s Vector3s;
37 typedef typename MathBase::VectorXs VectorXs;
48 ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
const SE3& pref,
49 const std::size_t nu,
const Vector2s& gains = Vector2s::Zero());
61 ContactModel6DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
const SE3& pref,
62 const Vector2s& gains = Vector2s::Zero());
63 DEPRECATED(
"Use constructor which is not based on FramePlacement.",
65 const std::size_t nu,
const Vector2s& gains = Vector2s::Zero());)
66 DEPRECATED(
"Use constructor which is not based on FramePlacement.",
68 const Vector2s& gains = Vector2s::Zero());)
78 virtual void calc(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
87 virtual void calcDiff(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
95 virtual void updateForce(
const boost::shared_ptr<ContactDataAbstract>& data,
const VectorXs& force);
100 virtual boost::shared_ptr<ContactDataAbstract>
createData(pinocchio::DataTpl<Scalar>*
const data);
111 const Vector2s& get_gains()
const;
123 virtual void print(std::ostream& os)
const;
136 template <
typename _Scalar>
138 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
140 typedef _Scalar Scalar;
143 typedef typename MathBase::Matrix6xs Matrix6xs;
144 typedef typename MathBase::Matrix6s Matrix6s;
146 template <
template <
typename Scalar>
class Model>
147 ContactData6DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
149 rMf(pinocchio::SE3Tpl<Scalar>::Identity()),
150 v_partial_dq(6, model->get_state()->get_nv()),
151 a_partial_dq(6, model->get_state()->get_nv()),
152 a_partial_dv(6, model->get_state()->get_nv()),
153 a_partial_da(6, model->get_state()->get_nv()) {
154 frame = model->get_id();
155 jMf = model->get_state()->get_pinocchio()->frames[
frame].placement;
156 fXj =
jMf.inverse().toActionMatrix();
157 v_partial_dq.setZero();
158 a_partial_dq.setZero();
159 a_partial_dv.setZero();
160 a_partial_da.setZero();
175 pinocchio::SE3Tpl<Scalar> rMf;
176 pinocchio::MotionTpl<Scalar> v;
177 pinocchio::MotionTpl<Scalar> a;
178 Matrix6xs v_partial_dq;
179 Matrix6xs a_partial_dq;
180 Matrix6xs a_partial_dv;
181 Matrix6xs a_partial_da;
189 #include "crocoddyl/multibody/contacts/contact-6d.hxx"
191 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_