Crocoddyl
 
Loading...
Searching...
No Matches
contact-force.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_FORCE_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_
12
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/contact-base.hpp"
15#include "crocoddyl/multibody/contacts/contact-1d.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/fwd.hpp"
22#include "crocoddyl/multibody/impulse-base.hpp"
23#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
24#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
25#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
26#include "crocoddyl/multibody/states/multibody.hpp"
27
28namespace crocoddyl {
29
55template <typename _Scalar>
57 public:
58 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelContactForceTpl)
60
61 typedef _Scalar Scalar;
68 typedef pinocchio::ForceTpl<Scalar> Force;
69 typedef typename MathBase::VectorXs VectorXs;
70 typedef typename MathBase::MatrixXs MatrixXs;
71
87 ResidualModelContactForceTpl(std::shared_ptr<StateMultibody> state,
88 const pinocchio::FrameIndex id,
89 const Force& fref, const std::size_t nc,
90 const std::size_t nu, const bool fwddyn = true);
91
104 ResidualModelContactForceTpl(std::shared_ptr<StateMultibody> state,
105 const pinocchio::FrameIndex id,
106 const Force& fref, const std::size_t nc);
107 virtual ~ResidualModelContactForceTpl() = default;
108
121 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
122 const Eigen::Ref<const VectorXs>& x,
123 const Eigen::Ref<const VectorXs>& u) override;
124
135 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
136 const Eigen::Ref<const VectorXs>& x) override;
137
150 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
151 const Eigen::Ref<const VectorXs>& x,
152 const Eigen::Ref<const VectorXs>& u) override;
153
165 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
166 const Eigen::Ref<const VectorXs>& x) override;
167
174 virtual std::shared_ptr<ResidualDataAbstract> createData(
175 DataCollectorAbstract* const data) override;
176
182 void updateJacobians(const std::shared_ptr<ResidualDataAbstract>& data);
183
193 template <typename NewScalar>
195
200 bool is_fwddyn() const;
201
205 pinocchio::FrameIndex get_id() const;
206
211 const Force& get_reference() const;
212
216 DEPRECATED("Do not use set_id, instead create a new model",
217 void set_id(const pinocchio::FrameIndex id);)
218
223 void set_reference(const Force& reference);
224
230 virtual void print(std::ostream& os) const override;
231
232 protected:
233 using Base::nr_;
234 using Base::nu_;
235 using Base::state_;
236
237 private:
238 bool fwddyn_;
240 bool update_jacobians_;
242 pinocchio::FrameIndex id_;
243 Force fref_;
244};
245
246template <typename _Scalar>
248 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
249
250 typedef _Scalar Scalar;
256 typedef pinocchio::ForceTpl<Scalar> Force;
258 typedef typename MathBase::MatrixXs MatrixXs;
259
260 template <template <typename Scalar> class Model>
261 ResidualDataContactForceTpl(Model<Scalar>* const model,
262 DataCollectorAbstract* const data)
263 : Base(model, data) {
264 contact_type = ContactUndefined;
265
266 // Check that proper shared data has been passed
267 bool is_contact = true;
272 if (d1 == NULL && d2 == NULL) {
273 throw_pretty(
274 "Invalid argument: the shared data should be derived from "
275 "DataCollectorContact or DataCollectorImpulse");
276 }
277 if (d2 != NULL) {
278 is_contact = false;
279 }
280
281 // Avoids data casting at runtime
282 const pinocchio::FrameIndex id = model->get_id();
283 const std::shared_ptr<StateMultibody>& state =
284 std::static_pointer_cast<StateMultibody>(model->get_state());
285 std::string frame_name = state->get_pinocchio()->frames[id].name;
286 bool found_contact = false;
287 if (is_contact) {
288 for (typename ContactModelMultiple::ContactDataContainer::iterator it =
289 d1->contacts->contacts.begin();
290 it != d1->contacts->contacts.end(); ++it) {
291 if (it->second->frame == id) {
293 dynamic_cast<ContactData1DTpl<Scalar>*>(it->second.get());
294 if (d1d != NULL) {
295 contact_type = Contact1D;
296 found_contact = true;
297 contact = it->second;
298 break;
299 }
301 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
302 if (d3d != NULL) {
303 contact_type = Contact3D;
304 found_contact = true;
305 contact = it->second;
306 break;
307 }
309 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
310 if (d6d != NULL) {
311 contact_type = Contact6D;
312 found_contact = true;
313 contact = it->second;
314 break;
315 }
316 throw_pretty(
317 "Domain error: there isn't defined at least a 3d contact for " +
318 frame_name);
319 break;
320 }
321 }
322 } else {
323 for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
324 d2->impulses->impulses.begin();
325 it != d2->impulses->impulses.end(); ++it) {
326 if (it->second->frame == id) {
328 dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
329 if (d3d != NULL) {
330 contact_type = Contact3D;
331 found_contact = true;
332 contact = it->second;
333 break;
334 }
336 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
337 if (d6d != NULL) {
338 contact_type = Contact6D;
339 found_contact = true;
340 contact = it->second;
341 break;
342 }
343 throw_pretty(
344 "Domain error: there isn't defined at least a 3d impulse for " +
345 frame_name);
346 break;
347 }
348 }
349 }
350 if (!found_contact) {
351 throw_pretty(
352 "Domain error: there isn't defined contact/impulse data for " +
353 frame_name);
354 }
355 }
356 virtual ~ResidualDataContactForceTpl() = default;
357
358 std::shared_ptr<ForceDataAbstractTpl<Scalar> >
360 ContactType contact_type;
361 using Base::r;
362 using Base::Ru;
363 using Base::Rx;
364 using Base::shared;
365};
366
367} // namespace crocoddyl
368
369/* --- Details -------------------------------------------------------------- */
370/* --- Details -------------------------------------------------------------- */
371/* --- Details -------------------------------------------------------------- */
372#include "crocoddyl/multibody/residuals/contact-force.hxx"
373
374CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelContactForceTpl)
375CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataContactForceTpl)
376
377#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_
Define a stack of contact models.
Define a stack of impulse models.
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
std::size_t nr_
Residual vector dimension.
Define a contact force residual function.
const Force & get_reference() const
Return the reference spatial contact force in the contact coordinates.
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.
ResidualModelContactForceTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Force &fref, const std::size_t nc)
Initialize the contact force residual model.
ResidualModelContactForceTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Force &fref, const std::size_t nc, const std::size_t nu, const bool fwddyn=true)
Initialize the contact force residual model.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the contact force residual.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the contact force residual data.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
ResidualModelContactForceTpl< NewScalar > cast() const
Cast the contact-force residual model to a different scalar type.
void updateJacobians(const std::shared_ptr< ResidualDataAbstract > &data)
Update 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 Force &reference)
Modify the reference frame id.
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 force residual.
virtual void print(std::ostream &os) const override
Print relevant information of the contact-force 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 (3D / 6D)