Crocoddyl
 
Loading...
Searching...
No Matches
contact-friction-cone.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_RESIDUALS_CONTACT_FRICTION_CONE_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FRICTION_CONE_HPP_
12
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/contact-base.hpp"
15#include "crocoddyl/multibody/contacts/contact-2d.hpp"
16#include "crocoddyl/multibody/contacts/contact-3d.hpp"
17#include "crocoddyl/multibody/contacts/contact-6d.hpp"
18#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
19#include "crocoddyl/multibody/data/contacts.hpp"
20#include "crocoddyl/multibody/data/impulses.hpp"
21#include "crocoddyl/multibody/friction-cone.hpp"
22#include "crocoddyl/multibody/fwd.hpp"
23#include "crocoddyl/multibody/impulse-base.hpp"
24#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
25#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
26#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
27#include "crocoddyl/multibody/states/multibody.hpp"
28
29namespace crocoddyl {
30
58template <typename _Scalar>
60 : public ResidualModelAbstractTpl<_Scalar> {
61 public:
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65 typedef _Scalar Scalar;
73 typedef typename MathBase::VectorXs VectorXs;
74 typedef typename MathBase::MatrixXs MatrixXs;
75 typedef typename MathBase::MatrixX3s MatrixX3s;
76
90 ResidualModelContactFrictionConeTpl(std::shared_ptr<StateMultibody> state,
91 const pinocchio::FrameIndex id,
92 const FrictionCone& fref,
93 const std::size_t nu,
94 const bool fwddyn = true);
95
106 ResidualModelContactFrictionConeTpl(std::shared_ptr<StateMultibody> state,
107 const pinocchio::FrameIndex id,
108 const FrictionCone& fref);
109 virtual ~ResidualModelContactFrictionConeTpl() = default;
110
118 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
119 const Eigen::Ref<const VectorXs>& x,
120 const Eigen::Ref<const VectorXs>& u) override;
121
132 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
133 const Eigen::Ref<const VectorXs>& x) override;
134
142 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
143 const Eigen::Ref<const VectorXs>& x,
144 const Eigen::Ref<const VectorXs>& u) override;
145
157 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
158 const Eigen::Ref<const VectorXs>& x) override;
159
163 virtual std::shared_ptr<ResidualDataAbstract> createData(
164 DataCollectorAbstract* const data) override;
165
171 void updateJacobians(const std::shared_ptr<ResidualDataAbstract>& data);
172
183 template <typename NewScalar>
185
190 bool is_fwddyn() const;
191
195 pinocchio::FrameIndex get_id() const;
196
201
205 DEPRECATED("Do not use set_id, instead create a new model",
206 void set_id(const pinocchio::FrameIndex id);)
207
211 void set_reference(const FrictionCone& reference);
212
218 virtual void print(std::ostream& os) const override;
219
220 protected:
221 using Base::nu_;
222 using Base::state_;
223
224 private:
225 bool fwddyn_;
227 bool update_jacobians_;
229 pinocchio::FrameIndex id_;
230 FrictionCone fref_;
231};
232
233template <typename _Scalar>
235 : public ResidualDataAbstractTpl<_Scalar> {
236 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
237
238 typedef _Scalar Scalar;
245 typedef typename MathBase::MatrixXs MatrixXs;
246
247 template <template <typename Scalar> class Model>
248 ResidualDataContactFrictionConeTpl(Model<Scalar>* const model,
249 DataCollectorAbstract* const data)
250 : Base(model, data) {
251 contact_type = ContactUndefined;
252 // Check that proper shared data has been passed
253 bool is_contact = true;
258 if (d1 == NULL && d2 == NULL) {
259 throw_pretty(
260 "Invalid argument: the shared data should be derived from "
261 "DataCollectorContact or DataCollectorImpulse");
262 }
263 if (d2 != NULL) {
264 is_contact = false;
265 }
266
267 // Avoids data casting at runtime
268 const pinocchio::FrameIndex id = model->get_id();
269 const std::shared_ptr<StateMultibody>& state =
270 std::static_pointer_cast<StateMultibody>(model->get_state());
271 std::string frame_name = state->get_pinocchio()->frames[id].name;
272 bool found_contact = false;
273 if (is_contact) {
274 for (typename ContactModelMultiple::ContactDataContainer::iterator it =
275 d1->contacts->contacts.begin();
276 it != d1->contacts->contacts.end(); ++it) {
277 if (it->second->frame == id) {
279 dynamic_cast<ContactData2DTpl<Scalar>*>(it->second.get());
280 if (d2d != NULL) {
281 contact_type = Contact2D;
282 found_contact = true;
283 contact = it->second;
284 break;
285 }
287 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
288 if (d3d != NULL) {
289 contact_type = Contact3D;
290 found_contact = true;
291 contact = it->second;
292 break;
293 }
295 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
296 if (d6d != NULL) {
297 contact_type = Contact6D;
298 found_contact = true;
299 contact = it->second;
300 break;
301 }
302 throw_pretty(
303 "Domain error: there isn't defined at least a 2d contact for " +
304 frame_name);
305 break;
306 }
307 }
308 } else {
309 for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
310 d2->impulses->impulses.begin();
311 it != d2->impulses->impulses.end(); ++it) {
312 if (it->second->frame == id) {
314 dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
315 if (d3d != NULL) {
316 contact_type = Contact3D;
317 found_contact = true;
318 contact = it->second;
319 break;
320 }
322 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
323 if (d6d != NULL) {
324 contact_type = Contact6D;
325 found_contact = true;
326 contact = it->second;
327 break;
328 }
329 throw_pretty(
330 "Domain error: there isn't defined at least a 3d contact for " +
331 frame_name);
332 break;
333 }
334 }
335 }
336 if (!found_contact) {
337 throw_pretty("Domain error: there isn't defined contact data for " +
338 frame_name);
339 }
340 }
341 virtual ~ResidualDataContactFrictionConeTpl() = default;
342
343 std::shared_ptr<ForceDataAbstractTpl<Scalar> >
345 ContactType contact_type;
346 using Base::r;
347 using Base::Ru;
348 using Base::Rx;
349 using Base::shared;
350};
351
352} // namespace crocoddyl
353
354/* --- Details -------------------------------------------------------------- */
355/* --- Details -------------------------------------------------------------- */
356/* --- Details -------------------------------------------------------------- */
357#include "crocoddyl/multibody/residuals/contact-friction-cone.hxx"
358
359CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
361CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
363
364#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FRICTION_CONE_HPP_
Define a stack of contact models.
This class encapsulates a friction cone.
Define a stack of impulse models.
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
ResidualModelContactFrictionConeTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const FrictionCone &fref)
Initialize the contact friction cone residual model.
bool is_fwddyn() const
Indicates if we are using the forward-dynamics (true) or inverse-dynamics (false)
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the residual vector for nodes that depends only on the state.
ResidualModelContactFrictionConeTpl< NewScalar > cast() const
Cast the contact-friction-cone residual model to a different scalar type.
const FrictionCone & get_reference() const
Return the reference contact friction cone.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the Jacobians of the contact friction cone residual.
DEPRECATED("Do not use set_id, instead create a new model", void set_id(const pinocchio::FrameIndex id);) void set_reference(const FrictionCone &reference)
Modify the reference frame id.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the contact friction cone residual data.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
void updateJacobians(const std::shared_ptr< ResidualDataAbstract > &data)
Update the Jacobians of the contact friction cone residual.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the contact friction cone residual.
ResidualModelContactFrictionConeTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const FrictionCone &fref, const std::size_t nu, const bool fwddyn=true)
Initialize the contact friction cone residual model.
virtual void print(std::ostream &os) const override
Print relevant information of the contact-friction-cone residual.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the Jacobian of the residual functions with respect to the state only.
State multibody representation.
Definition multibody.hpp:34
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
std::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Contact force data.
DataCollectorAbstract * shared
Shared data allocated by the action model.
ContactType contact_type
Type of contact (2D / 3D / 6D)