Crocoddyl
 
Loading...
Searching...
No Matches
contact-2d.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
11#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
12
13#include "crocoddyl/multibody/contact-base.hpp"
14#include "crocoddyl/multibody/fwd.hpp"
15
16namespace crocoddyl {
17
18template <typename _Scalar>
20 public:
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModel2DTpl)
23
24 typedef _Scalar Scalar;
30 typedef typename MathBase::Vector2s Vector2s;
31 typedef typename MathBase::Vector3s Vector3s;
32 typedef typename MathBase::VectorXs VectorXs;
33 typedef typename MathBase::Matrix3s Matrix3s;
34
44 ContactModel2DTpl(std::shared_ptr<StateMultibody> state,
45 const pinocchio::FrameIndex id, const Vector2s& xref,
46 const std::size_t nu,
47 const Vector2s& gains = Vector2s::Zero());
48
59 ContactModel2DTpl(std::shared_ptr<StateMultibody> state,
60 const pinocchio::FrameIndex id, const Vector2s& xref,
61 const Vector2s& gains = Vector2s::Zero());
62 virtual ~ContactModel2DTpl() = default;
63
71 virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
72 const Eigen::Ref<const VectorXs>& x) override;
73
81 virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
82 const Eigen::Ref<const VectorXs>& x) override;
83
90 virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
91 const VectorXs& force) override;
92
96 virtual std::shared_ptr<ContactDataAbstract> createData(
97 pinocchio::DataTpl<Scalar>* const data) override;
98
108 template <typename NewScalar>
110
114 const Vector2s& get_reference() const;
115
119 const Vector2s& get_gains() const;
120
124 void set_reference(const Vector2s& reference);
125
131 virtual void print(std::ostream& os) const override;
132
133 protected:
134 using Base::id_;
135 using Base::nc_;
136 using Base::nu_;
137 using Base::state_;
138
139 private:
140 Vector2s xref_;
141 Vector2s gains_;
142};
143
144template <typename _Scalar>
145struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
146 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
147
148 typedef _Scalar Scalar;
151 typedef typename MathBase::Matrix2s Matrix2s;
152 typedef typename MathBase::Matrix3s Matrix3s;
153 typedef typename MathBase::Matrix6xs Matrix6xs;
154 typedef typename MathBase::Vector3s Vector3s;
155
156 template <template <typename Scalar> class Model>
157 ContactData2DTpl(Model<Scalar>* const model,
158 pinocchio::DataTpl<Scalar>* const data)
159 : Base(model, data),
160 fJf(6, model->get_state()->get_nv()),
161 v_partial_dq(6, model->get_state()->get_nv()),
162 a_partial_dq(6, model->get_state()->get_nv()),
163 a_partial_dv(6, model->get_state()->get_nv()),
164 a_partial_da(6, model->get_state()->get_nv()),
165 fXjdv_dq(6, model->get_state()->get_nv()),
166 fXjda_dq(6, model->get_state()->get_nv()),
167 fXjda_dv(6, model->get_state()->get_nv()) {
168 frame = model->get_id();
169 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
170 fXj = jMf.inverse().toActionMatrix();
171 fJf.setZero();
172 v_partial_dq.setZero();
173 a_partial_dq.setZero();
174 a_partial_dv.setZero();
175 a_partial_da.setZero();
176 fXjdv_dq.setZero();
177 fXjda_dq.setZero();
178 fXjda_dv.setZero();
179 vv.setZero();
180 vw.setZero();
181 vv_skew.setZero();
182 vw_skew.setZero();
183 oRf.setZero();
184 }
185 virtual ~ContactData2DTpl() = default;
186
187 using Base::a0;
188 using Base::da0_dx;
189 using Base::df_du;
190 using Base::df_dx;
191 using Base::f;
192 using Base::frame;
193 using Base::fXj;
194 using Base::Jc;
195 using Base::jMf;
196 using Base::pinocchio;
197
198 pinocchio::MotionTpl<Scalar> v;
199 pinocchio::MotionTpl<Scalar> a;
200 Matrix6xs fJf;
201 Matrix6xs v_partial_dq;
202 Matrix6xs a_partial_dq;
203 Matrix6xs a_partial_dv;
204 Matrix6xs a_partial_da;
205 Matrix6xs fXjdv_dq;
206 Matrix6xs fXjda_dq;
207 Matrix6xs fXjda_dv;
208 Vector3s vv;
209 Vector3s vw;
210 Matrix3s vv_skew;
211 Matrix3s vw_skew;
212 Matrix2s oRf;
213};
214
215} // namespace crocoddyl
216
217/* --- Details -------------------------------------------------------------- */
218/* --- Details -------------------------------------------------------------- */
219/* --- Details -------------------------------------------------------------- */
220#include "crocoddyl/multibody/contacts/contact-2d.hxx"
221
222CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel2DTpl)
223CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData2DTpl)
224
225#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
const Vector2s & get_gains() const
Create the 2d contact data.
ContactModel2DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector2s &xref, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 2d contact model.
virtual void calc(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the 2d contact Jacobian and drift.
ContactModel2DTpl< NewScalar > cast() const
Cast the contact-2d model to a different scalar type.
virtual std::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data) override
Create the 2d contact data.
const Vector2s & get_reference() const
Return the reference frame translation.
ContactModel2DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector2s &xref, const Vector2s &gains=Vector2s::Zero())
Initialize the 2d contact model.
virtual void calcDiff(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the 2d contact holonomic constraint.
virtual void updateForce(const std::shared_ptr< ContactDataAbstract > &data, const VectorXs &force) override
Convert the force into a stack of spatial forces.
virtual void print(std::ostream &os) const override
Print relevant information of the 2d contact model.
void set_reference(const Vector2s &reference)
Modify the reference frame translation.
pinocchio::FrameIndex id_
Reference frame id of the contact.
State multibody representation.
Definition multibody.hpp:34
PinocchioData * pinocchio
Pinocchio data.
pinocchio::FrameIndex frame
Frame index of the contact frame.
SE3 jMf
Local frame placement of the contact frame.
MatrixXs Jc
Contact Jacobian.
Force f
Contact force expressed in the coordinate defined by type.