9 #ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_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"
21 #include "crocoddyl/multibody/frames-deprecated.hpp"
25 template <
typename _Scalar>
28 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 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::Matrix3s Matrix3s;
50 ContactModel1DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
const Scalar xref,
51 const std::size_t nu,
const Vector2s& gains = Vector2s::Zero());
63 ContactModel1DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
const Scalar xref,
64 const Vector2s& gains = Vector2s::Zero());
75 virtual void calc(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
84 virtual void calcDiff(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
92 virtual void updateForce(
const boost::shared_ptr<ContactDataAbstract>& data,
const VectorXs& force);
97 virtual boost::shared_ptr<ContactDataAbstract>
createData(pinocchio::DataTpl<Scalar>*
const data);
119 virtual void print(std::ostream& os)
const;
132 template <
typename _Scalar>
134 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
136 typedef _Scalar Scalar;
139 typedef typename MathBase::Matrix2s Matrix2s;
140 typedef typename MathBase::Matrix3s Matrix3s;
141 typedef typename MathBase::Matrix6xs Matrix6xs;
142 typedef typename MathBase::Vector3s Vector3s;
144 template <
template <
typename Scalar>
class Model>
145 ContactData1DTpl(Model<Scalar>*
const model, pinocchio::DataTpl<Scalar>*
const data)
147 fJf(6, model->get_state()->get_nv()),
148 v_partial_dq(6, model->get_state()->get_nv()),
149 a_partial_dq(6, model->get_state()->get_nv()),
150 a_partial_dv(6, model->get_state()->get_nv()),
151 a_partial_da(6, model->get_state()->get_nv()),
152 fXjdv_dq(6, model->get_state()->get_nv()),
153 fXjda_dq(6, model->get_state()->get_nv()),
154 fXjda_dv(6, model->get_state()->get_nv()) {
155 frame = model->get_id();
156 jMf = model->get_state()->get_pinocchio()->frames[
frame].placement;
157 fXj =
jMf.inverse().toActionMatrix();
159 v_partial_dq.setZero();
160 a_partial_dq.setZero();
161 a_partial_dv.setZero();
162 a_partial_da.setZero();
184 pinocchio::MotionTpl<Scalar> v;
185 pinocchio::MotionTpl<Scalar> a;
187 Matrix6xs v_partial_dq;
188 Matrix6xs a_partial_dq;
189 Matrix6xs a_partial_dv;
190 Matrix6xs a_partial_da;
206 #include "crocoddyl/multibody/contacts/contact-1d.hxx"
208 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_HPP_