Skip to content
Snippets Groups Projects
Verified Commit 2f8c3d3e authored by Justin Carpentier's avatar Justin Carpentier
Browse files

srdf: allows removing of collision pairs when FCL is not present

parent 75324ec0
No related branches found
No related tags found
No related merge requests found
...@@ -13,8 +13,6 @@ namespace pinocchio ...@@ -13,8 +13,6 @@ namespace pinocchio
namespace srdf namespace srdf
{ {
#ifdef PINOCCHIO_WITH_HPP_FCL
/// ///
/// \brief Deactive all possible collision pairs mentioned in the SRDF file. /// \brief Deactive all possible collision pairs mentioned in the SRDF file.
/// It throws if the SRDF file is incorrect. /// It throws if the SRDF file is incorrect.
...@@ -44,8 +42,6 @@ namespace pinocchio ...@@ -44,8 +42,6 @@ namespace pinocchio
const std::string & xmlString, const std::string & xmlString,
const bool verbose = false); const bool verbose = false);
#endif // ifdef PINOCCHIO_WITH_HPP_FCL
/// ///
/// \brief Get the reference configurations of a given model associated to a SRDF file. /// \brief Get the reference configurations of a given model associated to a SRDF file.
/// It throws if the SRDF file is incorrect. The reference configurations are /// It throws if the SRDF file is incorrect. The reference configurations are
...@@ -76,7 +72,6 @@ namespace pinocchio ...@@ -76,7 +72,6 @@ namespace pinocchio
std::istream & xmlStream, std::istream & xmlStream,
const bool verbose = false); const bool verbose = false);
/// ///
/// \brief Load the rotor params of a given model associated to a SRDF file. /// \brief Load the rotor params of a given model associated to a SRDF file.
/// It throws if the SRDF file is incorrect. /// It throws if the SRDF file is incorrect.
......
...@@ -23,8 +23,6 @@ namespace pinocchio ...@@ -23,8 +23,6 @@ namespace pinocchio
{ {
namespace srdf namespace srdf
{ {
#ifdef PINOCCHIO_WITH_HPP_FCL
namespace details namespace details
{ {
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
...@@ -129,8 +127,6 @@ namespace pinocchio ...@@ -129,8 +127,6 @@ namespace pinocchio
details::removeCollisionPairs(model, geom_model, srdf_stream, verbose); details::removeCollisionPairs(model, geom_model, srdf_stream, verbose);
} }
#endif // ifdef PINOCCHIO_WITH_HPP_FCL
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
bool loadRotorParameters(ModelTpl<Scalar,Options,JointCollectionTpl> & model, bool loadRotorParameters(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const std::string & filename, const std::string & filename,
...@@ -210,8 +206,8 @@ namespace pinocchio ...@@ -210,8 +206,8 @@ namespace pinocchio
typedef typename Model::ConfigVectorType ConfigVectorType; typedef typename Model::ConfigVectorType ConfigVectorType;
typedef boost::fusion::vector<const std::string&, typedef boost::fusion::vector<const std::string&,
const ConfigVectorType&, const ConfigVectorType&,
ConfigVectorType&> ArgsType; ConfigVectorType&> ArgsType;
template<typename JointModel> template<typename JointModel>
static void algo(const JointModelBase<JointModel> & joint, static void algo(const JointModelBase<JointModel> & joint,
...@@ -225,9 +221,9 @@ namespace pinocchio ...@@ -225,9 +221,9 @@ namespace pinocchio
private: private:
template<int axis> template<int axis>
static void _algo (const JointModelRevoluteUnboundedTpl<Scalar,Options,axis> & joint, static void _algo (const JointModelRevoluteUnboundedTpl<Scalar,Options,axis> & joint,
const std::string& joint_name, const std::string& joint_name,
const ConfigVectorType& fromXML, const ConfigVectorType& fromXML,
ConfigVectorType& config) ConfigVectorType& config)
{ {
typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelRUB; typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelRUB;
PINOCCHIO_STATIC_ASSERT(JointModelRUB::NQ == 2, JOINT_MODEL_REVOLUTE_SHOULD_HAVE_2_PARAMETERS); PINOCCHIO_STATIC_ASSERT(JointModelRUB::NQ == 2, JOINT_MODEL_REVOLUTE_SHOULD_HAVE_2_PARAMETERS);
...@@ -235,16 +231,16 @@ namespace pinocchio ...@@ -235,16 +231,16 @@ namespace pinocchio
std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl; std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl;
else { else {
SINCOS(fromXML[0], SINCOS(fromXML[0],
&config[joint.idx_q()+1], &config[joint.idx_q()+1],
&config[joint.idx_q()+0]); &config[joint.idx_q()+0]);
} }
} }
template<typename JointModel> template<typename JointModel>
static void _algo (const JointModel & joint, static void _algo (const JointModel & joint,
const std::string& joint_name, const std::string& joint_name,
const ConfigVectorType& fromXML, const ConfigVectorType& fromXML,
ConfigVectorType& config) ConfigVectorType& config)
{ {
if (joint.nq() != fromXML.size()) if (joint.nq() != fromXML.size())
std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl; std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment