9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_ 10 #define CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_ 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" 39 template <
typename _Scalar>
40 class ResidualModelPairCollisionTpl :
public ResidualModelAbstractTpl<_Scalar> {
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
53 typedef typename MathBase::VectorXs VectorXs;
54 typedef typename MathBase::MatrixXs MatrixXs;
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);
69 virtual ~ResidualModelPairCollisionTpl();
78 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
79 const Eigen::Ref<const VectorXs> &u);
88 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
89 const Eigen::Ref<const VectorXs> &u);
91 virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *
const data);
96 const pinocchio::GeometryModel &get_geometry()
const;
101 pinocchio::PairIndex get_pair_id()
const;
106 void set_pair_id(
const pinocchio::PairIndex pair_id);
112 using Base::v_dependent_;
115 typename StateMultibody::PinocchioModel pin_model_;
116 boost::shared_ptr<pinocchio::GeometryModel> geom_model_;
117 pinocchio::PairIndex pair_id_;
118 pinocchio::JointIndex joint_id_;
121 template <
typename _Scalar>
122 struct ResidualDataPairCollisionTpl :
public ResidualDataAbstractTpl<_Scalar> {
123 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
125 typedef _Scalar Scalar;
126 typedef MathBaseTpl<Scalar> MathBase;
127 typedef ResidualDataAbstractTpl<Scalar> Base;
128 typedef StateMultibodyTpl<Scalar> StateMultibody;
129 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
131 typedef typename MathBase::Matrix6xs Matrix6xs;
132 typedef typename MathBase::Vector3s Vector3s;
134 template <
template <
typename Scalar>
class Model>
135 ResidualDataPairCollisionTpl(Model<Scalar> *
const model, DataCollectorAbstract *
const data)
137 geometry(pinocchio::GeometryData(model->get_geometry())),
138 J(Matrix6xs::Zero(6, model->get_state()->get_nv())) {
140 DataCollectorMultibodyTpl<Scalar> *d =
dynamic_cast<DataCollectorMultibodyTpl<Scalar> *
>(shared);
142 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorActMultibodyTpl");
145 pinocchio = d->pinocchio;
147 pinocchio::GeometryData geometry;
148 pinocchio::DataTpl<Scalar> *pinocchio;
162 #include "crocoddyl/multibody/residuals/pair-collision.hxx" 164 #endif // PINOCCHIO_WITH_HPP_FCL 166 #endif // CROCODDYL_MULTIBODY_RESIDUALS_PAIR_COLLISION_HPP_