Crocoddyl
 
Loading...
Searching...
No Matches
contact-1d.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_1D_HPP_
11#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_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, ContactModel1DTpl)
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
51 ContactModel1DTpl(std::shared_ptr<StateMultibody> state,
52 const pinocchio::FrameIndex id, const Scalar xref,
53 const pinocchio::ReferenceFrame type,
54 const Matrix3s& rotation, const std::size_t nu,
55 const Vector2s& gains = Vector2s::Zero());
56
72 ContactModel1DTpl(std::shared_ptr<StateMultibody> state,
73 const pinocchio::FrameIndex id, const Scalar xref,
74 const pinocchio::ReferenceFrame type,
75 const Vector2s& gains = Vector2s::Zero());
76
77 DEPRECATED(
78 "Use constructor that passes the type type of contact, this assumes is "
79 "pinocchio::LOCAL",
80 ContactModel1DTpl(std::shared_ptr<StateMultibody> state,
81 const pinocchio::FrameIndex id, const Scalar xref,
82 const std::size_t nu,
83 const Vector2s& gains = Vector2s::Zero());)
84 DEPRECATED(
85 "Use constructor that passes the type type of contact, this assumes is "
86 "pinocchio::LOCAL",
87 ContactModel1DTpl(std::shared_ptr<StateMultibody> state,
88 const pinocchio::FrameIndex id, const Scalar xref,
89 const Vector2s& gains = Vector2s::Zero());)
90 virtual ~ContactModel1DTpl() = default;
91
99 virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
100 const Eigen::Ref<const VectorXs>& x) override;
101
109 virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
110 const Eigen::Ref<const VectorXs>& x) override;
111
118 virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
119 const VectorXs& force) override;
120
124 virtual std::shared_ptr<ContactDataAbstract> createData(
125 pinocchio::DataTpl<Scalar>* const data) override;
126
136 template <typename NewScalar>
138
142 const Scalar get_reference() const;
143
147 const Vector2s& get_gains() const;
148
152 const Matrix3s& get_axis_rotation() const;
153
157 void set_reference(const Scalar reference);
158
162 void set_axis_rotation(const Matrix3s& rotation);
163
169 virtual void print(std::ostream& os) const override;
170
171 protected:
172 using Base::id_;
173 using Base::nc_;
174 using Base::nu_;
175 using Base::state_;
176 using Base::type_;
177
178 private:
179 Scalar xref_;
180 Matrix3s Raxis_;
181 Vector2s gains_;
182};
183
184template <typename _Scalar>
185struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> {
186 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
187
188 typedef _Scalar Scalar;
191 typedef typename MathBase::Matrix2s Matrix2s;
192 typedef typename MathBase::Matrix3s Matrix3s;
193 typedef typename MathBase::Matrix3xs Matrix3xs;
194 typedef typename MathBase::Matrix6xs Matrix6xs;
195 typedef typename MathBase::Vector3s Vector3s;
196 typedef typename pinocchio::MotionTpl<Scalar> Motion;
197 typedef typename pinocchio::ForceTpl<Scalar> Force;
198
199 template <template <typename Scalar> class Model>
200 ContactData1DTpl(Model<Scalar>* const model,
201 pinocchio::DataTpl<Scalar>* const data)
202 : Base(model, data),
203 v(Motion::Zero()),
204 f_local(Force::Zero()),
205 da0_local_dx(3, model->get_state()->get_ndx()),
206 fJf(6, model->get_state()->get_nv()),
207 v_partial_dq(6, model->get_state()->get_nv()),
208 a_partial_dq(6, model->get_state()->get_nv()),
209 a_partial_dv(6, model->get_state()->get_nv()),
210 a_partial_da(6, model->get_state()->get_nv()),
211 fXjdv_dq(6, model->get_state()->get_nv()),
212 fXjda_dq(6, model->get_state()->get_nv()),
213 fXjda_dv(6, model->get_state()->get_nv()),
214 fJf_df(3, model->get_state()->get_nv()) {
215 frame = model->get_id();
216 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
217 fXj = jMf.inverse().toActionMatrix();
218 a0_local.setZero();
219 dp.setZero();
220 dp_local.setZero();
221 da0_local_dx.setZero();
222 fJf.setZero();
223 v_partial_dq.setZero();
224 a_partial_dq.setZero();
225 a_partial_dv.setZero();
226 a_partial_da.setZero();
227 vv_skew.setZero();
228 vw_skew.setZero();
229 a0_skew.setZero();
230 a0_world_skew.setZero();
231 dp_skew.setZero();
232 f_skew.setZero();
233 fXjdv_dq.setZero();
234 fXjda_dq.setZero();
235 fXjda_dv.setZero();
236 oRf.setZero();
237 fJf_df.setZero();
238 }
239 virtual ~ContactData1DTpl() = default;
240
241 using Base::a0;
242 using Base::da0_dx;
243 using Base::df_du;
244 using Base::df_dx;
245 using Base::f;
246 using Base::frame;
247 using Base::fXj;
248 using Base::Jc;
249 using Base::jMf;
250 using Base::pinocchio;
251
252 Motion v;
253 Vector3s a0_local;
254 Vector3s dp;
255 Vector3s dp_local;
256 Force f_local;
257 Matrix3xs da0_local_dx;
258 Matrix6xs fJf;
259 Matrix6xs v_partial_dq;
260 Matrix6xs a_partial_dq;
261 Matrix6xs a_partial_dv;
262 Matrix6xs a_partial_da;
263 Matrix3s vv_skew;
264 Matrix3s vw_skew;
265 Matrix3s a0_skew;
266 Matrix3s a0_world_skew;
267 Matrix3s dp_skew;
268 Matrix3s f_skew;
269 Matrix6xs fXjdv_dq;
270 Matrix6xs fXjda_dq;
271 Matrix6xs fXjda_dv;
272 Matrix2s oRf;
273 Matrix3xs fJf_df;
274};
275
276} // namespace crocoddyl
277
278/* --- Details -------------------------------------------------------------- */
279/* --- Details -------------------------------------------------------------- */
280/* --- Details -------------------------------------------------------------- */
281#include "crocoddyl/multibody/contacts/contact-1d.hxx"
282
283CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel1DTpl)
284CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData1DTpl)
285
286#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_HPP_
const Vector2s & get_gains() const
Create the 1d contact data.
virtual void calc(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the 1d contact Jacobian and drift.
ContactModel1DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Scalar xref, const pinocchio::ReferenceFrame type, const Matrix3s &rotation, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 1d contact model.
virtual std::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data) override
Create the 1d contact data.
const Matrix3s & get_axis_rotation() const
Return the rotation of the reference frames's z axis.
void set_reference(const Scalar reference)
Modify the reference frame translation.
void set_axis_rotation(const Matrix3s &rotation)
Modify the rotation of the reference frames's z axis.
const Scalar get_reference() const
Return the reference frame translation.
ContactModel1DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Scalar xref, const pinocchio::ReferenceFrame type, const Vector2s &gains=Vector2s::Zero())
Initialize the 1d contact model.
ContactModel1DTpl< NewScalar > cast() const
Cast the contact-1d model to a different scalar type.
virtual void calcDiff(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the 1d 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 1d 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.