crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
pair-collision.hpp
1
2// 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
21namespace crocoddyl {
22
39template <typename _Scalar>
40class 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
121template <typename _Scalar>
122struct 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_