Crocoddyl
 
Loading...
Searching...
No Matches
contact-base.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-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_CONTACT_BASE_HPP_
11#define CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
12
13#include "crocoddyl/core/utils/deprecate.hpp"
14#include "crocoddyl/multibody/force-base.hpp"
15#include "crocoddyl/multibody/fwd.hpp"
16#include "crocoddyl/multibody/states/multibody.hpp"
17
18namespace crocoddyl {
19
21 public:
22 virtual ~ContactModelBase() = default;
23
24 CROCODDYL_BASE_CAST(ContactModelBase, ContactModelAbstractTpl)
25};
26
27template <typename _Scalar>
29 public:
30 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32 typedef _Scalar Scalar;
36 typedef typename MathBase::VectorXs VectorXs;
37 typedef typename MathBase::MatrixXs MatrixXs;
38
47 ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
48 const pinocchio::ReferenceFrame type,
49 const std::size_t nc, const std::size_t nu);
50 ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
51 const pinocchio::ReferenceFrame type,
52 const std::size_t nc);
53
54 DEPRECATED(
55 "Use constructor that passes the type type of contact, this assumes is "
56 "pinocchio::LOCAL",
57 ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
58 const std::size_t nc, const std::size_t nu);)
59 DEPRECATED(
60 "Use constructor that passes the type type of contact, this assumes is "
61 "pinocchio::LOCAL",
62 ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
63 const std::size_t nc);)
64 virtual ~ContactModelAbstractTpl() = default;
65
73 virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
74 const Eigen::Ref<const VectorXs>& x) = 0;
75
83 virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
84 const Eigen::Ref<const VectorXs>& x) = 0;
85
92 virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
93 const VectorXs& force) = 0;
94
101 void updateForceDiff(const std::shared_ptr<ContactDataAbstract>& data,
102 const MatrixXs& df_dx, const MatrixXs& df_du) const;
103
109 void setZeroForce(const std::shared_ptr<ContactDataAbstract>& data) const;
110
116 void setZeroForceDiff(const std::shared_ptr<ContactDataAbstract>& data) const;
117
121 virtual std::shared_ptr<ContactDataAbstract> createData(
122 pinocchio::DataTpl<Scalar>* const data);
123
127 const std::shared_ptr<StateMultibody>& get_state() const;
128
132 std::size_t get_nc() const;
133
137 std::size_t get_nu() const;
138
142 pinocchio::FrameIndex get_id() const;
143
147 void set_id(const pinocchio::FrameIndex id);
148
152 void set_type(const pinocchio::ReferenceFrame type);
153
157 pinocchio::ReferenceFrame get_type() const;
158
162 template <class Scalar>
163 friend std::ostream& operator<<(std::ostream& os,
165
171 virtual void print(std::ostream& os) const;
172
173 protected:
174 std::shared_ptr<StateMultibody> state_;
175 std::size_t nc_;
176 std::size_t nu_;
177 pinocchio::FrameIndex id_;
178 pinocchio::ReferenceFrame type_;
179 ContactModelAbstractTpl() : state_(nullptr), nc_(0), nu_(0), id_(0) {};
180};
181
182template <typename _Scalar>
184 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
185
186 typedef _Scalar Scalar;
189 typedef typename MathBase::VectorXs VectorXs;
190 typedef typename MathBase::MatrixXs MatrixXs;
191 typedef typename pinocchio::SE3Tpl<Scalar> SE3;
192
193 template <template <typename Scalar> class Model>
194 ContactDataAbstractTpl(Model<Scalar>* const model,
195 pinocchio::DataTpl<Scalar>* const data)
196 : Base(model, data),
197 fXj(jMf.inverse().toActionMatrix()),
198 a0(model->get_nc()),
199 da0_dx(model->get_nc(), model->get_state()->get_ndx()),
200 dtau_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
201 a0.setZero();
202 da0_dx.setZero();
203 dtau_dq.setZero();
204 }
205 virtual ~ContactDataAbstractTpl() = default;
206
207 using Base::df_du;
208 using Base::df_dx;
209 using Base::f;
210 using Base::frame;
211 using Base::Jc;
212 using Base::jMf;
213 using Base::pinocchio;
214
215 typename SE3::ActionMatrixType fXj;
216 VectorXs a0;
217 MatrixXs da0_dx;
218 MatrixXs dtau_dq;
219};
220
221} // namespace crocoddyl
222
223/* --- Details -------------------------------------------------------------- */
224/* --- Details -------------------------------------------------------------- */
225/* --- Details -------------------------------------------------------------- */
226#include "crocoddyl/multibody/contact-base.hxx"
227
228CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModelAbstractTpl)
229CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactDataAbstractTpl)
230
231#endif // CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
friend std::ostream & operator<<(std::ostream &os, const ContactModelAbstractTpl< Scalar > &model)
Print information on the contact model.
virtual void calc(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)=0
Compute the contact Jacobian and acceleration drift.
std::size_t get_nc() const
Return the dimension of the contact.
virtual void print(std::ostream &os) const
Print relevant information of the contact model.
virtual void updateForce(const std::shared_ptr< ContactDataAbstract > &data, const VectorXs &force)=0
Convert the force into a stack of spatial forces.
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.
pinocchio::ReferenceFrame type_
Type of contact.
void setZeroForceDiff(const std::shared_ptr< ContactDataAbstract > &data) const
Set the stack of spatial forces Jacobians to zero.
virtual void calcDiff(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)=0
Compute the derivatives of the acceleration-based contact.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
void set_type(const pinocchio::ReferenceFrame type)
Modify the type of contact.
pinocchio::ReferenceFrame get_type() const
Return the type of contact.
const std::shared_ptr< StateMultibody > & get_state() const
Return the state.
void updateForceDiff(const std::shared_ptr< ContactDataAbstract > &data, const MatrixXs &df_dx, const MatrixXs &df_du) const
Convert the force into a stack of spatial forces.
void setZeroForce(const std::shared_ptr< ContactDataAbstract > &data) const
Set the stack of spatial forces to zero.
pinocchio::FrameIndex id_
Reference frame id of the contact.
virtual std::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create the contact data.
std::size_t get_nu() const
Return the dimension of the control vector.
ContactModelAbstractTpl(std::shared_ptr< StateMultibody > state, const pinocchio::ReferenceFrame type, const std::size_t nc, const std::size_t nu)
Initialize the contact abstraction.
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.
Force f
Contact force expressed in the coordinate defined by type.