10#ifndef CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_
13#ifdef CROCODDYL_WITH_PAIR_COLLISION
15#ifdef PINOCCHIO_WITH_HPP_FCL
17#include "crocoddyl/core/residual-base.hpp"
18#include "crocoddyl/multibody/data/multibody.hpp"
19#include "crocoddyl/multibody/states/multibody.hpp"
41template <
typename _Scalar>
42class ResidualModelPairCollisionTpl :
public ResidualModelAbstractTpl<_Scalar> {
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelPairCollisionTpl)
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;
56 typedef typename MathBase::VectorXs VectorXs;
57 typedef typename MathBase::MatrixXs MatrixXs;
70 ResidualModelPairCollisionTpl(std::shared_ptr<StateMultibody> state,
72 std::shared_ptr<GeometryModel> geom_model,
73 const pinocchio::PairIndex pair_id,
74 const pinocchio::JointIndex joint_id);
76 virtual ~ResidualModelPairCollisionTpl() =
default;
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;
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;
100 virtual std::shared_ptr<ResidualDataAbstract> createData(
101 DataCollectorAbstract *
const data)
override;
112 template <
typename NewScalar>
113 ResidualModelPairCollisionTpl<NewScalar> cast()
const;
118 const pinocchio::GeometryModel &get_geometry()
const;
123 pinocchio::PairIndex get_pair_id()
const;
128 void set_pair_id(
const pinocchio::PairIndex pair_id);
133 using Base::v_dependent_;
136 typename StateMultibody::PinocchioModel
138 std::shared_ptr<pinocchio::GeometryModel>
142 pinocchio::JointIndex joint_id_;
146template <
typename _Scalar>
147struct ResidualDataPairCollisionTpl :
public ResidualDataAbstractTpl<_Scalar> {
148 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
150 typedef _Scalar Scalar;
151 typedef MathBaseTpl<Scalar> MathBase;
152 typedef ResidualDataAbstractTpl<Scalar> Base;
153 typedef StateMultibodyTpl<Scalar> StateMultibody;
154 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
156 typedef typename MathBase::Matrix6xs Matrix6xs;
157 typedef typename MathBase::Vector3s Vector3s;
159 template <
template <
typename Scalar>
class Model>
160 ResidualDataPairCollisionTpl(Model<Scalar> *
const model,
161 DataCollectorAbstract *
const data)
163 geometry(pinocchio::GeometryData(model->get_geometry())),
164 J(6, model->get_state()->get_nv()) {
168 DataCollectorMultibodyTpl<Scalar> *d =
169 dynamic_cast<DataCollectorMultibodyTpl<Scalar> *
>(shared);
172 "Invalid argument: the shared data should be derived from "
173 "DataCollectorActMultibodyTpl");
176 pinocchio = d->pinocchio;
178 virtual ~ResidualDataPairCollisionTpl() =
default;
180 pinocchio::GeometryData geometry;
181 pinocchio::DataTpl<Scalar> *pinocchio;
195#include "crocoddyl/multibody/residuals/pair-collision.hxx"
201CROCODDYL_PRAGMA_WARNING(
202 "This header is currently disabled as Pinocchio does not support casting "