Crocoddyl
 
Loading...
Searching...
No Matches
contact-3d.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University f_world 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_3D_HPP_
11#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
12
13#include "crocoddyl/core/utils/deprecate.hpp"
14#include "crocoddyl/multibody/contact-base.hpp"
15#include "crocoddyl/multibody/fwd.hpp"
16
17namespace crocoddyl {
18
19template <typename _Scalar>
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModel3DTpl)
24
25 typedef _Scalar Scalar;
31 typedef typename MathBase::Matrix3s Matrix3s;
32 typedef typename MathBase::Vector2s Vector2s;
33 typedef typename MathBase::Vector3s Vector3s;
34 typedef typename MathBase::VectorXs VectorXs;
35
51 ContactModel3DTpl(std::shared_ptr<StateMultibody> state,
52 const pinocchio::FrameIndex id, const Vector3s& xref,
53 const pinocchio::ReferenceFrame type, const std::size_t nu,
54 const Vector2s& gains = Vector2s::Zero());
55
71 ContactModel3DTpl(std::shared_ptr<StateMultibody> state,
72 const pinocchio::FrameIndex id, const Vector3s& xref,
73 const pinocchio::ReferenceFrame type,
74 const Vector2s& gains = Vector2s::Zero());
75
76 DEPRECATED(
77 "Use constructor that passes the type type of contact, this assumes is "
78 "pinocchio::LOCAL",
79 ContactModel3DTpl(std::shared_ptr<StateMultibody> state,
80 const pinocchio::FrameIndex id, const Vector3s& xref,
81 const std::size_t nu,
82 const Vector2s& gains = Vector2s::Zero());)
83 DEPRECATED(
84 "Use constructor that passes the type type of contact, this assumes is "
85 "pinocchio::LOCAL",
86 ContactModel3DTpl(std::shared_ptr<StateMultibody> state,
87 const pinocchio::FrameIndex id, const Vector3s& xref,
88 const Vector2s& gains = Vector2s::Zero());)
89 virtual ~ContactModel3DTpl() = default;
90
98 virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
99 const Eigen::Ref<const VectorXs>& x) override;
100
108 virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
109 const Eigen::Ref<const VectorXs>& x) override;
110
117 virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
118 const VectorXs& force) override;
119
123 virtual std::shared_ptr<ContactDataAbstract> createData(
124 pinocchio::DataTpl<Scalar>* const data) override;
125
135 template <typename NewScalar>
137
141 const Vector3s& get_reference() const;
142
146 const Vector2s& get_gains() const;
147
151 void set_reference(const Vector3s& reference);
152
158 virtual void print(std::ostream& os) const override;
159
160 protected:
161 using Base::id_;
162 using Base::nc_;
163 using Base::nu_;
164 using Base::state_;
165 using Base::type_;
166
167 private:
168 Vector3s xref_;
169 Vector2s gains_;
170};
171
172template <typename _Scalar>
173struct ContactData3DTpl : public ContactDataAbstractTpl<_Scalar> {
174 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
175
176 typedef _Scalar Scalar;
179 typedef typename MathBase::Vector3s Vector3s;
180 typedef typename MathBase::Matrix3s Matrix3s;
181 typedef typename MathBase::Matrix3xs Matrix3xs;
182 typedef typename MathBase::Matrix6xs Matrix6xs;
183 typedef typename pinocchio::MotionTpl<Scalar> Motion;
184 typedef typename pinocchio::ForceTpl<Scalar> Force;
185
186 template <template <typename Scalar> class Model>
187 ContactData3DTpl(Model<Scalar>* const model,
188 pinocchio::DataTpl<Scalar>* const data)
189 : Base(model, data),
190 v(Motion::Zero()),
191 f_local(Force::Zero()),
192 da0_local_dx(3, model->get_state()->get_ndx()),
193 fJf(6, model->get_state()->get_nv()),
194 v_partial_dq(6, model->get_state()->get_nv()),
195 a_partial_dq(6, model->get_state()->get_nv()),
196 a_partial_dv(6, model->get_state()->get_nv()),
197 a_partial_da(6, model->get_state()->get_nv()),
198 fXjdv_dq(6, model->get_state()->get_nv()),
199 fXjda_dq(6, model->get_state()->get_nv()),
200 fXjda_dv(6, model->get_state()->get_nv()),
201 fJf_df(3, model->get_state()->get_nv()) {
202 frame = model->get_id();
203 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
204 fXj = jMf.inverse().toActionMatrix();
205 a0_local.setZero();
206 dp.setZero();
207 dp_local.setZero();
208 da0_local_dx.setZero();
209 fJf.setZero();
210 v_partial_dq.setZero();
211 a_partial_dq.setZero();
212 a_partial_dv.setZero();
213 a_partial_da.setZero();
214 vv_skew.setZero();
215 vw_skew.setZero();
216 a0_skew.setZero();
217 a0_world_skew.setZero();
218 dp_skew.setZero();
219 f_skew.setZero();
220 fXjdv_dq.setZero();
221 fXjda_dq.setZero();
222 fXjda_dv.setZero();
223 fJf_df.setZero();
224 }
225 virtual ~ContactData3DTpl() = default;
226
227 using Base::a0;
228 using Base::da0_dx;
229 using Base::df_du;
230 using Base::df_dx;
231 using Base::f;
232 using Base::frame;
233 using Base::fXj;
234 using Base::Jc;
235 using Base::jMf;
236 using Base::pinocchio;
237
238 Motion v;
239 Vector3s a0_local;
240 Vector3s dp;
241 Vector3s dp_local;
242 Force f_local;
243 Matrix3xs da0_local_dx;
244 Matrix6xs fJf;
245 Matrix6xs v_partial_dq;
246 Matrix6xs a_partial_dq;
247 Matrix6xs a_partial_dv;
248 Matrix6xs a_partial_da;
249 Matrix3s vv_skew;
250 Matrix3s vw_skew;
251 Matrix3s a0_skew;
252 Matrix3s a0_world_skew;
253 Matrix3s dp_skew;
254 Matrix3s f_skew;
255 Matrix6xs fXjdv_dq;
256 Matrix6xs fXjda_dq;
257 Matrix6xs fXjda_dv;
258 Matrix3xs fJf_df;
259};
260
261} // namespace crocoddyl
262
263/* --- Details -------------------------------------------------------------- */
264/* --- Details -------------------------------------------------------------- */
265/* --- Details -------------------------------------------------------------- */
266#include "crocoddyl/multibody/contacts/contact-3d.hxx"
267
268CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel3DTpl)
269CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData3DTpl)
270
271#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
const Vector2s & get_gains() const
Return the Baumgarte stabilization gains.
void set_reference(const Vector3s &reference)
Modify the reference frame translation.
ContactModel3DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector3s &xref, const pinocchio::ReferenceFrame type, const Vector2s &gains=Vector2s::Zero())
Initialize the 3d contact model.
virtual void calc(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the 3d contact Jacobian and drift.
const Vector3s & get_reference() const
Return the reference frame translation.
ContactModel3DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector3s &xref, const pinocchio::ReferenceFrame type, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 3d contact model.
virtual std::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data) override
Create the 3d contact data.
virtual void calcDiff(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the 3d 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.
ContactModel3DTpl< NewScalar > cast() const
Cast the contact-3d model to a different scalar type.
virtual void print(std::ostream &os) const override
Print relevant information of the 3d contact model.
pinocchio::ReferenceFrame type_
Type of contact.
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.