Crocoddyl
 
Loading...
Searching...
No Matches
contact-cop-position.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020-2025, University of Duisburg-Essen,
5// University of Edinburgh, Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
12
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/contact-base.hpp"
15#include "crocoddyl/multibody/contacts/contact-3d.hpp"
16#include "crocoddyl/multibody/contacts/contact-6d.hpp"
17#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
18#include "crocoddyl/multibody/cop-support.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
65template <typename _Scalar>
67 : public ResidualModelAbstractTpl<_Scalar> {
68 public:
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71
72 typedef _Scalar Scalar;
80 typedef typename MathBase::VectorXs VectorXs;
81 typedef typename MathBase::MatrixXs MatrixXs;
82 typedef typename MathBase::Matrix46s Matrix46;
83
97 ResidualModelContactCoPPositionTpl(std::shared_ptr<StateMultibody> state,
98 const pinocchio::FrameIndex id,
99 const CoPSupport& cref,
100 const std::size_t nu,
101 const bool fwddyn = true);
102
113 ResidualModelContactCoPPositionTpl(std::shared_ptr<StateMultibody> state,
114 const pinocchio::FrameIndex id,
115 const CoPSupport& cref);
116 virtual ~ResidualModelContactCoPPositionTpl() = default;
117
130 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
131 const Eigen::Ref<const VectorXs>& x,
132 const Eigen::Ref<const VectorXs>& u) override;
133
144 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
145 const Eigen::Ref<const VectorXs>& x) override;
146
159 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
160 const Eigen::Ref<const VectorXs>& x,
161 const Eigen::Ref<const VectorXs>& u) override;
162
174 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
175 const Eigen::Ref<const VectorXs>& x) override;
176
187 virtual std::shared_ptr<ResidualDataAbstract> createData(
188 DataCollectorAbstract* const data) override;
189
195 void updateJacobians(const std::shared_ptr<ResidualDataAbstract>& data);
196
207 template <typename NewScalar>
209
214 bool is_fwddyn() const;
215
219 pinocchio::FrameIndex get_id() const;
220
224 const CoPSupport& get_reference() const;
225
229 DEPRECATED("Do not use set_id, instead create a new model",
230 void set_id(pinocchio::FrameIndex id);)
231
235 void set_reference(const CoPSupport& reference);
236
242 virtual void print(std::ostream& os) const override;
243
244 protected:
245 using Base::nu_;
246 using Base::state_;
247
248 private:
249 bool fwddyn_;
251 bool update_jacobians_;
253 pinocchio::FrameIndex id_;
254 CoPSupport cref_;
255};
256
257template <typename _Scalar>
259 : public ResidualDataAbstractTpl<_Scalar> {
260 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
261
262 typedef _Scalar Scalar;
267 typedef typename MathBase::MatrixXs MatrixXs;
268
269 template <template <typename Scalar> class Model>
270 ResidualDataContactCoPPositionTpl(Model<Scalar>* const model,
271 DataCollectorAbstract* const data)
272 : Base(model, data) {
273 // Check that proper shared data has been passed
274 bool is_contact = true;
279 if (d1 == NULL && d2 == NULL) {
280 throw_pretty(
281 "Invalid argument: the shared data should be derived from "
282 "DataCollectorContact or DataCollectorImpulse");
283 }
284 if (d2 != NULL) {
285 is_contact = false;
286 }
287
288 // Avoids data casting at runtime
289 const pinocchio::FrameIndex id = model->get_id();
290 const std::shared_ptr<StateMultibody>& state =
291 std::static_pointer_cast<StateMultibody>(model->get_state());
292 std::string frame_name = state->get_pinocchio()->frames[id].name;
293 bool found_contact = false;
294 if (is_contact) {
295 for (typename ContactModelMultipleTpl<
296 Scalar>::ContactDataContainer::iterator it =
297 d1->contacts->contacts.begin();
298 it != d1->contacts->contacts.end(); ++it) {
299 if (it->second->frame == id) {
301 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
302 if (d3d != NULL) {
303 found_contact = true;
304 contact = it->second;
305 throw_pretty(
306 "Domain error: there isn't defined at least a 6d contact for " +
307 frame_name);
308 break;
309 }
311 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
312 if (d6d != NULL) {
313 found_contact = true;
314 contact = it->second;
315 break;
316 }
317 throw_pretty(
318 "Domain error: there isn't defined at least a 6d contact for " +
319 frame_name);
320 break;
321 }
322 }
323 } else {
324 for (typename ImpulseModelMultipleTpl<
325 Scalar>::ImpulseDataContainer::iterator it =
326 d2->impulses->impulses.begin();
327 it != d2->impulses->impulses.end(); ++it) {
328 if (it->second->frame == id) {
330 dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
331 if (d3d != NULL) {
332 found_contact = true;
333 contact = it->second;
334 throw_pretty(
335 "Domain error: there isn't defined at least a 6d contact for " +
336 frame_name);
337 break;
338 }
340 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
341 if (d6d != NULL) {
342 found_contact = true;
343 contact = it->second;
344 break;
345 }
346 throw_pretty(
347 "Domain error: there isn't defined at least a 6d contact for " +
348 frame_name);
349 break;
350 }
351 }
352 }
353 if (!found_contact) {
354 throw_pretty("Domain error: there isn't defined contact data for " +
355 frame_name);
356 }
357 }
358 virtual ~ResidualDataContactCoPPositionTpl() = default;
359
360 pinocchio::DataTpl<Scalar>* pinocchio;
361 std::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
362 using Base::r;
363 using Base::Ru;
364 using Base::Rx;
365 using Base::shared;
366};
367
368} // namespace crocoddyl
369
370/* --- Details -------------------------------------------------------------- */
371/* --- Details -------------------------------------------------------------- */
372/* --- Details -------------------------------------------------------------- */
373#include "crocoddyl/multibody/residuals/contact-cop-position.hxx"
374
375CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
377CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
379
380#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
This class encapsulates a center of pressure support of a 6d contact.
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.
Center of pressure residual function.
DEPRECATED("Do not use set_id, instead create a new model", void set_id(pinocchio::FrameIndex id);) void set_reference(const CoPSupport &reference)
Modify the reference frame id.
ResidualModelContactCoPPositionTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref)
Initialize the contact CoP residual model.
ResidualModelContactCoPPositionTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref, const std::size_t nu, const bool fwddyn=true)
Initialize the contact CoP residual model.
ResidualModelContactCoPPositionTpl< NewScalar > cast() const
Cast the contact-cop-position residual model to a different scalar type.
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.
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 CoP residual.
const CoPSupport & get_reference() const
Return the reference support region of CoP.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the contact CoP 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 CoP residual.
virtual void print(std::ostream &os) const override
Print relevant information of the cop-position 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.
DataCollectorAbstract * shared
Shared data allocated by the action model.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.