19 #ifndef HPP_RBPRM_TOOLS_HH
20 #define HPP_RBPRM_TOOLS_HH
23 #include <hpp/core/config-validation.hh>
24 #include <hpp/pinocchio/collision-object.hh>
25 #include <hpp/pinocchio/frame.hh>
26 #include <hpp/pinocchio/joint.hh>
34 const Eigen::Vector3d& to);
38 pinocchio::Configuration_t
interpolate(pinocchio::ConfigurationIn_t q1,
39 pinocchio::ConfigurationIn_t q2,
40 const pinocchio::value_type& u);
41 pinocchio::value_type
distance(pinocchio::ConfigurationIn_t q1,
42 pinocchio::ConfigurationIn_t q2);
44 template <
typename K,
typename V>
48 bool insertIfNew(std::vector<T>& data,
const T& value);
52 const core::ObjectStdVector_t& obstacles);
55 const pinocchio::CollisionObjectPtr_t obstacle);
58 const pinocchio::CollisionObjectPtr_t obstacle);
64 void LockJointRec(
const std::string& spared,
const pinocchio::JointPtr_t joint,
65 core::ConfigProjectorPtr_t projector);
72 const pinocchio::JointPtr_t joint,
73 core::ConfigProjectorPtr_t projector);
81 core::ConfigProjectorPtr_t projector,
82 const bool constant =
true);
86 double StrToD(
const std::string& str);
90 std::vector<std::string>
splitString(
const std::string& s,
const char sep);
91 void writeMatrix(
const Eigen::MatrixXd& mat, std::ostream& output);
92 void writeVecFCL(
const fcl::Vec3f& vec, std::ostream& output);
97 Eigen::MatrixXd
readMatrix(std::ifstream& myfile, std::string& line);
99 fcl::Vec3f
readVecFCL(std::ifstream& myfile, std::string& line);
102 template <
typename T>
104 T& validation, pinocchio::JointPtr_t joint,
105 const pinocchio::CollisionObjectPtr_t obstacle) {
107 validation.removeObstacleFromJoint(joint, obstacle);
108 }
catch (
const std::runtime_error& e) {
109 std::cout <<
"WARNING: " << e.what() << std::endl;
113 for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
114 RemoveEffectorCollisionRec<T>(validation, joint->childJoint(i), obstacle);
118 template <
typename T>
120 const core::ObjectStdVector_t& obstacles) {
121 for (core::ObjectStdVector_t::const_iterator cit = obstacles.begin();
122 cit != obstacles.end(); ++cit) {
123 RemoveEffectorCollision<T>(validation, effectorJoint, *cit);
127 template <
typename T>
129 const pinocchio::CollisionObjectPtr_t obstacle) {
132 validation.removeObstacleFromJoint(effectorJoint, obstacle);
133 }
catch (
const std::runtime_error& e) {
134 std::cout <<
"WARNING: " << e.what() << std::endl;
138 for (std::size_t i = 0; i < effectorJoint->numberChildJoints(); ++i) {
139 RemoveEffectorCollisionRec<T>(validation, effectorJoint->childJoint(i),
144 template <
typename T>
146 const pinocchio::Frame& effector,
147 const core::ObjectStdVector_t& collisionObjects,
148 T& collisionValidation,
149 const bool disableEffectorCollision) {
150 if (disableEffectorCollision) {
151 if (joint->name() == effector.name())
153 else if (joint->name() == effector.joint()->name())
155 else if (joint->numberChildJoints() == 0)
158 for (core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin();
159 cit != collisionObjects.end(); ++cit)
160 collisionValidation.addObstacleToJoint(*cit, joint,
false);
161 for (std::size_t i = 0; i < joint->numberChildJoints(); ++i)
162 addLimbCollisionRec<T>(joint->childJoint(i), effector, collisionObjects,
163 collisionValidation, disableEffectorCollision);
166 template <
typename T>
168 const std::string& limbname,
169 const core::ObjectStdVector_t& collisionObjects,
170 T& collisionValidation) {
171 if (joint->name() == limbname)
return;
172 for (core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin();
173 cit != collisionObjects.end(); ++cit) {
175 if (joint->linkedBody()) {
176 std::cout <<
"remiove obstacle: " << limbname <<
" " << joint->name()
177 <<
" " << (*cit)->name() << std::endl;
178 collisionValidation.removeObstacleFromJoint(joint, *cit);
180 }
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) {
187 collisionValidation);
191 template <
typename T>
193 const std::vector<std::string>& keepers,
194 const core::ObjectStdVector_t& collisionObjects,
195 T& collisionValidation) {
196 if (std::find(keepers.begin(), keepers.end(), joint->name()) != keepers.end())
198 for (core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin();
199 cit != collisionObjects.end(); ++cit) {
201 collisionValidation.removeObstacleFromJoint(joint, *cit);
202 }
catch (
const std::runtime_error& e) {
203 std::cout <<
"WARNING: " << e.what() << std::endl;
207 for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
209 collisionValidation);
213 template <
typename K,
typename V>
214 void addToMap(std::map<K, V>& map,
const K& key,
const V& value) {
215 if (map.find(key) != map.end())
218 map.insert(std::make_pair(key, value));
221 template <
typename T>
223 if (std::find(data.begin(), data.end(), value) == data.end()) {
224 data.push_back(value);
Definition: algorithm.hh:26