19 #ifndef HPP_RBPRM_TOOLS_HH
20 # define HPP_RBPRM_TOOLS_HH
24 # include <hpp/core/config-validation.hh>
25 # include <hpp/pinocchio/joint.hh>
27 # include <hpp/pinocchio/collision-object.hh>
28 # include <hpp/pinocchio/frame.hh>
29 # include <Eigen/Core>
34 Eigen::Matrix3d
GetRotationMatrix(
const Eigen::Vector3d& from,
const Eigen::Vector3d& to);
38 pinocchio::Configuration_t
interpolate(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2,
const pinocchio::value_type& u);
39 pinocchio::value_type
distance (pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2);
41 template<
typename K,
typename V>
42 void addToMap(
const K& key,
const V& value);
45 bool insertIfNew(std::vector<T>& data,
const T& value);
48 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
const core::ObjectStdVector_t& obstacles);
50 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
const pinocchio::CollisionObjectPtr_t obstacle);
58 void LockJointRec(
const std::string& spared,
const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector);
64 void LockJointRec(
const std::vector<std::string>& spared,
const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector);
70 void LockJoint(
const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector,
const bool constant=
true);
75 double StrToD (
const std::string &str);
76 int StrToI (
const std::string &str);
77 double StrToD (std::ifstream& input);
78 int StrToI (std::ifstream& input);
79 std::vector<std::string>
splitString(
const std::string& s,
const char sep);
80 void writeMatrix (
const Eigen::MatrixXd& mat, std::ostream& output);
81 void writeVecFCL (
const fcl::Vec3f& vec , std::ostream& output);
83 Eigen::MatrixXd
readMatrix (std::ifstream& myfile);
86 Eigen::MatrixXd
readMatrix (std::ifstream& myfile, std::string& line);
88 fcl::Vec3f
readVecFCL (std::ifstream& myfile, std::string& line);
96 validation.removeObstacleFromJoint(joint,obstacle);
98 catch(
const std::runtime_error& e)
100 std::cout <<
"WARNING: " << e.what() << std::endl;
104 for(std::size_t i =0; i < joint->numberChildJoints(); ++i)
106 RemoveEffectorCollisionRec<T>(validation, joint->childJoint(i), obstacle);
113 for(core::ObjectStdVector_t::const_iterator cit = obstacles.begin();
114 cit != obstacles.end(); ++cit)
116 RemoveEffectorCollision<T>(validation,effectorJoint,*cit);
121 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
const pinocchio::CollisionObjectPtr_t obstacle)
126 validation.removeObstacleFromJoint(effectorJoint,obstacle);
128 catch(
const std::runtime_error& e)
130 std::cout <<
"WARNING: " << e.what() << std::endl;
134 for(std::size_t i =0; i < effectorJoint->numberChildJoints(); ++i)
136 RemoveEffectorCollisionRec<T>(validation, effectorJoint->childJoint(i), obstacle);
142 const core::ObjectStdVector_t &collisionObjects,
143 T& collisionValidation,
const bool disableEffectorCollision)
145 if(disableEffectorCollision)
147 if (joint->name() == effector.name())
149 else if(joint->name() == effector.joint()->name() )
151 else if(joint->numberChildJoints() == 0)
154 for(core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin();
155 cit != collisionObjects.end(); ++cit)
156 collisionValidation.addObstacleToJoint(*cit,joint,
false);
157 for(std::size_t i=0; i<joint->numberChildJoints(); ++i)
158 addLimbCollisionRec<T>(joint->childJoint(i),effector, collisionObjects, collisionValidation,disableEffectorCollision);
164 const core::ObjectStdVector_t &collisionObjects,
165 T& collisionValidation)
167 if(joint->name() == limbname)
return;
168 for(core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin();
169 cit != collisionObjects.end(); ++cit)
173 if(joint->linkedBody ())
175 std::cout <<
"remiove obstacle: " << limbname <<
" "<< joint->name() <<
" " << (*cit)->name() << std::endl;
176 collisionValidation.removeObstacleFromJoint(joint, *cit);
179 catch(
const std::runtime_error& e)
181 std::cout <<
"WARNING: "<< e.what() << std::endl;
185 for(std::size_t i=0; i<joint->numberChildJoints(); ++i)
194 const core::ObjectStdVector_t &collisionObjects,
195 T& collisionValidation)
197 if(std::find(keepers.begin(), keepers.end(), joint->name()) != keepers.end())
return;
198 for(core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin();
199 cit != collisionObjects.end(); ++cit)
203 collisionValidation.removeObstacleFromJoint(joint, *cit);
205 catch(
const std::runtime_error& e)
207 std::cout <<
"WARNING: "<< e.what() << std::endl;
211 for(std::size_t i=0; i<joint->numberChildJoints(); ++i)
218 template<
typename K,
typename V>
219 void addToMap(std::map<K,V>& map,
const K& key,
const V& value)
221 if(map.find(key) != map.end())
224 map.insert(std::make_pair(key,value));
230 if(std::find(data.begin(), data.end(), value) == data.end())
232 data.push_back(value);
242 #endif // HPP_RBPRM_TOOLS_HH