1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2021-2022, LAAS-CNRS, University of Edinburgh, INRIA |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#ifndef CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_ |
10 |
|
|
#define CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_ |
11 |
|
|
|
12 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
13 |
|
|
|
14 |
|
|
#include <pinocchio/multibody/geometry.hpp> |
15 |
|
|
|
16 |
|
|
#include "crocoddyl/core/residual-base.hpp" |
17 |
|
|
#include "crocoddyl/multibody/data/multibody.hpp" |
18 |
|
|
#include "crocoddyl/multibody/states/multibody.hpp" |
19 |
|
|
|
20 |
|
|
namespace crocoddyl { |
21 |
|
|
|
22 |
|
|
/** |
23 |
|
|
* @brief Pair collision residual |
24 |
|
|
* |
25 |
|
|
* This residual function defines the euclidean distance between a geometric |
26 |
|
|
* collision pair as \f$\mathbf{r}=\mathbf{p}_1-\mathbf{p}_2^*\f$, where |
27 |
|
|
* \f$\mathbf{p}_1,\mathbf{p}_2^*\in~\mathbb{R}^3\f$ are the nearest points on |
28 |
|
|
* each collision object with respect to the other object. One of the objects is |
29 |
|
|
* a body frame of the robot, the other is an external object. Note that for the |
30 |
|
|
* sake of fast computation, it is easier to define the collision objects as |
31 |
|
|
* inflated capsules. Note also that the dimension of the residual vector is |
32 |
|
|
* obtained from 3. Furthermore, the Jacobians of the residual function are |
33 |
|
|
* computed analytically. |
34 |
|
|
* |
35 |
|
|
* As described in `ResidualModelAbstractTpl()`, the residual value and its |
36 |
|
|
* Jacobians are calculated by `calc` and `calcDiff`, respectively. |
37 |
|
|
* |
38 |
|
|
* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()` |
39 |
|
|
*/ |
40 |
|
|
template <typename _Scalar> |
41 |
|
|
class ResidualModelPairCollisionTpl : public ResidualModelAbstractTpl<_Scalar> { |
42 |
|
|
public: |
43 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
44 |
|
|
|
45 |
|
|
typedef _Scalar Scalar; |
46 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
47 |
|
|
typedef ResidualModelAbstractTpl<Scalar> Base; |
48 |
|
|
typedef ResidualDataPairCollisionTpl<Scalar> Data; |
49 |
|
|
typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract; |
50 |
|
|
typedef StateMultibodyTpl<Scalar> StateMultibody; |
51 |
|
|
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; |
52 |
|
|
typedef pinocchio::GeometryModel GeometryModel; |
53 |
|
|
|
54 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
55 |
|
|
typedef typename MathBase::MatrixXs MatrixXs; |
56 |
|
|
|
57 |
|
|
/** |
58 |
|
|
* @brief Initialize the pair collision residual model |
59 |
|
|
* |
60 |
|
|
* @param[in] state State of the multibody system |
61 |
|
|
* @param[in] nu Dimension of the control vector |
62 |
|
|
* @param[in] geom_model Pinocchio geometry model containing the collision |
63 |
|
|
* pair |
64 |
|
|
* @param[in] pair_id Index of the collision pair in the geometry model |
65 |
|
|
* @param[in] joint_id Index of the nearest joint on which the collision |
66 |
|
|
* link is attached |
67 |
|
|
*/ |
68 |
|
|
ResidualModelPairCollisionTpl(boost::shared_ptr<StateMultibody> state, |
69 |
|
|
const std::size_t nu, |
70 |
|
|
boost::shared_ptr<GeometryModel> geom_model, |
71 |
|
|
const pinocchio::PairIndex pair_id, |
72 |
|
|
const pinocchio::JointIndex joint_id); |
73 |
|
|
|
74 |
|
|
virtual ~ResidualModelPairCollisionTpl(); |
75 |
|
|
|
76 |
|
|
/** |
77 |
|
|
* @brief Compute the pair collision residual |
78 |
|
|
* |
79 |
|
|
* @param[in] data Pair collision residual data |
80 |
|
|
* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ |
81 |
|
|
* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ |
82 |
|
|
*/ |
83 |
|
|
virtual void calc(const boost::shared_ptr<ResidualDataAbstract> &data, |
84 |
|
|
const Eigen::Ref<const VectorXs> &x, |
85 |
|
|
const Eigen::Ref<const VectorXs> &u); |
86 |
|
|
|
87 |
|
|
/** |
88 |
|
|
* @brief Compute the derivatives of the pair collision residual |
89 |
|
|
* |
90 |
|
|
* @param[in] data Pair collision residual data |
91 |
|
|
* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ |
92 |
|
|
* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ |
93 |
|
|
*/ |
94 |
|
|
virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data, |
95 |
|
|
const Eigen::Ref<const VectorXs> &x, |
96 |
|
|
const Eigen::Ref<const VectorXs> &u); |
97 |
|
|
|
98 |
|
|
virtual boost::shared_ptr<ResidualDataAbstract> createData( |
99 |
|
|
DataCollectorAbstract *const data); |
100 |
|
|
|
101 |
|
|
/** |
102 |
|
|
* @brief Return the Pinocchio geometry model |
103 |
|
|
*/ |
104 |
|
|
const pinocchio::GeometryModel &get_geometry() const; |
105 |
|
|
|
106 |
|
|
/** |
107 |
|
|
* @brief Return the reference collision pair id |
108 |
|
|
*/ |
109 |
|
|
pinocchio::PairIndex get_pair_id() const; |
110 |
|
|
|
111 |
|
|
/** |
112 |
|
|
* @brief Modify the reference collision pair id |
113 |
|
|
*/ |
114 |
|
|
void set_pair_id(const pinocchio::PairIndex pair_id); |
115 |
|
|
|
116 |
|
|
protected: |
117 |
|
|
using Base::nu_; |
118 |
|
|
using Base::state_; |
119 |
|
|
using Base::v_dependent_; |
120 |
|
|
|
121 |
|
|
private: |
122 |
|
|
typename StateMultibody::PinocchioModel |
123 |
|
|
pin_model_; //!< Pinocchio model used for internal computations |
124 |
|
|
boost::shared_ptr<pinocchio::GeometryModel> |
125 |
|
|
geom_model_; //!< Pinocchio geometry model containing collision pair |
126 |
|
|
pinocchio::PairIndex |
127 |
|
|
pair_id_; //!< Index of the collision pair in geometry model |
128 |
|
|
pinocchio::JointIndex joint_id_; //!< Index of joint on which the collision |
129 |
|
|
//!< body frame of the robot is attached |
130 |
|
|
}; |
131 |
|
|
|
132 |
|
|
template <typename _Scalar> |
133 |
|
|
struct ResidualDataPairCollisionTpl : public ResidualDataAbstractTpl<_Scalar> { |
134 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
135 |
|
|
|
136 |
|
|
typedef _Scalar Scalar; |
137 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
138 |
|
|
typedef ResidualDataAbstractTpl<Scalar> Base; |
139 |
|
|
typedef StateMultibodyTpl<Scalar> StateMultibody; |
140 |
|
|
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; |
141 |
|
|
|
142 |
|
|
typedef typename MathBase::Matrix6xs Matrix6xs; |
143 |
|
|
typedef typename MathBase::Vector3s Vector3s; |
144 |
|
|
|
145 |
|
|
template <template <typename Scalar> class Model> |
146 |
|
2716 |
ResidualDataPairCollisionTpl(Model<Scalar> *const model, |
147 |
|
|
DataCollectorAbstract *const data) |
148 |
|
|
: Base(model, data), |
149 |
|
|
geometry(pinocchio::GeometryData(model->get_geometry())), |
150 |
✓✗✓✗ ✓✗ |
2716 |
J(6, model->get_state()->get_nv()) { |
151 |
✓✗ |
2716 |
d.setZero(); |
152 |
✓✗ |
2716 |
J.setZero(); |
153 |
|
|
// Check that proper shared data has been passed |
154 |
|
|
DataCollectorMultibodyTpl<Scalar> *d = |
155 |
✓✗ |
2716 |
dynamic_cast<DataCollectorMultibodyTpl<Scalar> *>(shared); |
156 |
✗✓ |
2716 |
if (d == NULL) { |
157 |
|
|
throw_pretty( |
158 |
|
|
"Invalid argument: the shared data should be derived from " |
159 |
|
|
"DataCollectorActMultibodyTpl"); |
160 |
|
|
} |
161 |
|
|
// Avoids data casting at runtime |
162 |
|
2716 |
pinocchio = d->pinocchio; |
163 |
|
2716 |
} |
164 |
|
|
pinocchio::GeometryData geometry; //!< Pinocchio geometry data |
165 |
|
|
pinocchio::DataTpl<Scalar> *pinocchio; //!< Pinocchio data |
166 |
|
|
Matrix6xs J; //!< Jacobian at the collision joint |
167 |
|
|
Vector3s d; //!< Vector from joint point to collision point in world frame |
168 |
|
|
using Base::r; |
169 |
|
|
using Base::Ru; |
170 |
|
|
using Base::Rx; |
171 |
|
|
using Base::shared; |
172 |
|
|
}; |
173 |
|
|
|
174 |
|
|
} // namespace crocoddyl |
175 |
|
|
|
176 |
|
|
/* --- Details -------------------------------------------------------------- */ |
177 |
|
|
/* --- Details -------------------------------------------------------------- */ |
178 |
|
|
/* --- Details -------------------------------------------------------------- */ |
179 |
|
|
#include "crocoddyl/multibody/residuals/pair-collision.hxx" |
180 |
|
|
|
181 |
|
|
#endif // PINOCCHIO_WITH_HPP_FCL |
182 |
|
|
|
183 |
|
|
#endif // CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_ |