Crocoddyl
 
Loading...
Searching...
No Matches
pair-collision.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh, INRIA,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_
12
13#ifdef CROCODDYL_WITH_PAIR_COLLISION
14
15#ifdef PINOCCHIO_WITH_HPP_FCL
16
17#include "crocoddyl/core/residual-base.hpp"
18#include "crocoddyl/multibody/data/multibody.hpp"
19#include "crocoddyl/multibody/states/multibody.hpp"
20
21namespace crocoddyl {
22
41template <typename _Scalar>
42class ResidualModelPairCollisionTpl : public ResidualModelAbstractTpl<_Scalar> {
43 public:
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelPairCollisionTpl)
46
47 typedef _Scalar Scalar;
48 typedef MathBaseTpl<Scalar> MathBase;
49 typedef ResidualModelAbstractTpl<Scalar> Base;
50 typedef ResidualDataPairCollisionTpl<Scalar> Data;
51 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
52 typedef StateMultibodyTpl<Scalar> StateMultibody;
53 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
54 typedef pinocchio::GeometryModel GeometryModel;
55
56 typedef typename MathBase::VectorXs VectorXs;
57 typedef typename MathBase::MatrixXs MatrixXs;
58
70 ResidualModelPairCollisionTpl(std::shared_ptr<StateMultibody> state,
71 const std::size_t nu,
72 std::shared_ptr<GeometryModel> geom_model,
73 const pinocchio::PairIndex pair_id,
74 const pinocchio::JointIndex joint_id);
75
76 virtual ~ResidualModelPairCollisionTpl() = default;
77
85 virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data,
86 const Eigen::Ref<const VectorXs> &x,
87 const Eigen::Ref<const VectorXs> &u) override;
88
96 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract> &data,
97 const Eigen::Ref<const VectorXs> &x,
98 const Eigen::Ref<const VectorXs> &u) override;
99
100 virtual std::shared_ptr<ResidualDataAbstract> createData(
101 DataCollectorAbstract *const data) override;
102
112 template <typename NewScalar>
113 ResidualModelPairCollisionTpl<NewScalar> cast() const;
114
118 const pinocchio::GeometryModel &get_geometry() const;
119
123 pinocchio::PairIndex get_pair_id() const;
124
128 void set_pair_id(const pinocchio::PairIndex pair_id);
129
130 protected:
131 using Base::nu_;
132 using Base::state_;
133 using Base::v_dependent_;
134
135 private:
136 typename StateMultibody::PinocchioModel
137 pin_model_;
138 std::shared_ptr<pinocchio::GeometryModel>
139 geom_model_;
140 pinocchio::PairIndex
141 pair_id_;
142 pinocchio::JointIndex joint_id_;
144};
145
146template <typename _Scalar>
147struct ResidualDataPairCollisionTpl : public ResidualDataAbstractTpl<_Scalar> {
148 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
149
150 typedef _Scalar Scalar;
151 typedef MathBaseTpl<Scalar> MathBase;
152 typedef ResidualDataAbstractTpl<Scalar> Base;
153 typedef StateMultibodyTpl<Scalar> StateMultibody;
154 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
155
156 typedef typename MathBase::Matrix6xs Matrix6xs;
157 typedef typename MathBase::Vector3s Vector3s;
158
159 template <template <typename Scalar> class Model>
160 ResidualDataPairCollisionTpl(Model<Scalar> *const model,
161 DataCollectorAbstract *const data)
162 : Base(model, data),
163 geometry(pinocchio::GeometryData(model->get_geometry())),
164 J(6, model->get_state()->get_nv()) {
165 d.setZero();
166 J.setZero();
167 // Check that proper shared data has been passed
168 DataCollectorMultibodyTpl<Scalar> *d =
169 dynamic_cast<DataCollectorMultibodyTpl<Scalar> *>(shared);
170 if (d == NULL) {
171 throw_pretty(
172 "Invalid argument: the shared data should be derived from "
173 "DataCollectorActMultibodyTpl");
174 }
175 // Avoids data casting at runtime
176 pinocchio = d->pinocchio;
177 }
178 virtual ~ResidualDataPairCollisionTpl() = default;
179
180 pinocchio::GeometryData geometry;
181 pinocchio::DataTpl<Scalar> *pinocchio;
182 Matrix6xs J;
183 Vector3s d;
184 using Base::r;
185 using Base::Ru;
186 using Base::Rx;
187 using Base::shared;
188};
189
190} // namespace crocoddyl
191
192/* --- Details -------------------------------------------------------------- */
193/* --- Details -------------------------------------------------------------- */
194/* --- Details -------------------------------------------------------------- */
195#include "crocoddyl/multibody/residuals/pair-collision.hxx"
196
197#endif // PINOCCHIO_WITH_HPP_FCL
198
199#else
200
201CROCODDYL_PRAGMA_WARNING(
202 "This header is currently disabled as Pinocchio does not support casting "
203 "of GeometryData")
204
205#endif // CROCODDYL_WITH_PAIR_COLLISION
206
207#endif // CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_