crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
pair-collision.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, LAAS-CNRS, University of Edinburgh, INRIA
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
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 #include <pinocchio/multibody/fcl.hpp>
14 #include <pinocchio/multibody/geometry.hpp>
15 #include <pinocchio/algorithm/jacobian.hpp>
16 #include <pinocchio/algorithm/geometry.hpp>
17 #include "crocoddyl/core/residual-base.hpp"
18 #include "crocoddyl/multibody/states/multibody.hpp"
19 #include "crocoddyl/multibody/data/multibody.hpp"
20 
21 namespace crocoddyl {
22 
39 template <typename _Scalar>
40 class ResidualModelPairCollisionTpl : public ResidualModelAbstractTpl<_Scalar> {
41  public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 
44  typedef _Scalar Scalar;
45  typedef MathBaseTpl<Scalar> MathBase;
46  typedef ResidualModelAbstractTpl<Scalar> Base;
47  typedef ResidualDataPairCollisionTpl<Scalar> Data;
48  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
49  typedef StateMultibodyTpl<Scalar> StateMultibody;
50  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
51  typedef pinocchio::GeometryModel GeometryModel;
52 
53  typedef typename MathBase::VectorXs VectorXs;
54  typedef typename MathBase::MatrixXs MatrixXs;
55 
65  ResidualModelPairCollisionTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nu,
66  boost::shared_ptr<GeometryModel> geom_model, const pinocchio::PairIndex pair_id,
67  const pinocchio::JointIndex joint_id);
68 
69  virtual ~ResidualModelPairCollisionTpl();
70 
78  virtual void calc(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
79  const Eigen::Ref<const VectorXs> &u);
80 
88  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
89  const Eigen::Ref<const VectorXs> &u);
90 
91  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *const data);
92 
96  const pinocchio::GeometryModel &get_geometry() const;
97 
101  pinocchio::PairIndex get_pair_id() const;
102 
106  void set_pair_id(const pinocchio::PairIndex pair_id);
107 
108  protected:
109  using Base::nu_;
110  using Base::state_;
111  using Base::unone_;
112  using Base::v_dependent_;
113 
114  private:
115  typename StateMultibody::PinocchioModel pin_model_;
116  boost::shared_ptr<pinocchio::GeometryModel> geom_model_;
117  pinocchio::PairIndex pair_id_;
118  pinocchio::JointIndex joint_id_;
119 };
120 
121 template <typename _Scalar>
122 struct ResidualDataPairCollisionTpl : public ResidualDataAbstractTpl<_Scalar> {
123  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 
125  typedef _Scalar Scalar;
126  typedef MathBaseTpl<Scalar> MathBase;
127  typedef ResidualDataAbstractTpl<Scalar> Base;
128  typedef StateMultibodyTpl<Scalar> StateMultibody;
129  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
130 
131  typedef typename MathBase::Matrix6xs Matrix6xs;
132  typedef typename MathBase::Vector3s Vector3s;
133 
134  template <template <typename Scalar> class Model>
135  ResidualDataPairCollisionTpl(Model<Scalar> *const model, DataCollectorAbstract *const data)
136  : Base(model, data),
137  geometry(pinocchio::GeometryData(model->get_geometry())),
138  J(Matrix6xs::Zero(6, model->get_state()->get_nv())) {
139  // Check that proper shared data has been passed
140  DataCollectorMultibodyTpl<Scalar> *d = dynamic_cast<DataCollectorMultibodyTpl<Scalar> *>(shared);
141  if (d == NULL) {
142  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorActMultibodyTpl");
143  }
144  // Avoids data casting at runtime
145  pinocchio = d->pinocchio;
146  }
147  pinocchio::GeometryData geometry;
148  pinocchio::DataTpl<Scalar> *pinocchio;
149  Matrix6xs J;
150  Vector3s d;
151  using Base::r;
152  using Base::Ru;
153  using Base::Rx;
154  using Base::shared;
155 };
156 
157 } // namespace crocoddyl
158 
159 /* --- Details -------------------------------------------------------------- */
160 /* --- Details -------------------------------------------------------------- */
161 /* --- Details -------------------------------------------------------------- */
162 #include "crocoddyl/multibody/residuals/pair-collision.hxx"
163 
164 #endif // PINOCCHIO_WITH_HPP_FCL
165 
166 #endif // CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_