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> 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,
39 const pinocchio::value_type& u);
40 pinocchio::value_type
distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2);
42 template <
typename K,
typename V>
43 void addToMap(
const K& key,
const V& value);
46 bool insertIfNew(std::vector<T>& data,
const T& value);
50 const core::ObjectStdVector_t& obstacles);
53 const pinocchio::CollisionObjectPtr_t obstacle);
56 const pinocchio::CollisionObjectPtr_t obstacle);
62 void LockJointRec(
const std::string& spared,
const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector);
68 void LockJointRec(
const std::vector<std::string>& spared,
const pinocchio::JointPtr_t joint,
69 core::ConfigProjectorPtr_t projector);
75 void LockJoint(
const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector,
const bool constant =
true);
79 double StrToD(
const std::string& str);
80 int StrToI(
const std::string& str);
81 double StrToD(std::ifstream& input);
82 int StrToI(std::ifstream& input);
83 std::vector<std::string>
splitString(
const std::string& s,
const char sep);
84 void writeMatrix(
const Eigen::MatrixXd& mat, std::ostream& output);
85 void writeVecFCL(
const fcl::Vec3f& vec, std::ostream& output);
87 Eigen::MatrixXd
readMatrix(std::ifstream& myfile);
90 Eigen::MatrixXd
readMatrix(std::ifstream& myfile, std::string& line);
92 fcl::Vec3f
readVecFCL(std::ifstream& myfile, std::string& line);
97 const pinocchio::CollisionObjectPtr_t obstacle) {
99 validation.removeObstacleFromJoint(joint, obstacle);
100 }
catch (
const std::runtime_error& e) {
101 std::cout <<
"WARNING: " << e.what() << std::endl;
105 for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
106 RemoveEffectorCollisionRec<T>(validation, joint->childJoint(i), obstacle);
110 template <
typename T>
112 const core::ObjectStdVector_t& obstacles) {
113 for (core::ObjectStdVector_t::const_iterator cit = obstacles.begin(); cit != obstacles.end(); ++cit) {
114 RemoveEffectorCollision<T>(validation, effectorJoint, *cit);
118 template <
typename T>
120 const pinocchio::CollisionObjectPtr_t obstacle) {
123 validation.removeObstacleFromJoint(effectorJoint, obstacle);
124 }
catch (
const std::runtime_error& e) {
125 std::cout <<
"WARNING: " << e.what() << std::endl;
129 for (std::size_t i = 0; i < effectorJoint->numberChildJoints(); ++i) {
130 RemoveEffectorCollisionRec<T>(validation, effectorJoint->childJoint(i), obstacle);
134 template <
typename T>
136 const core::ObjectStdVector_t& collisionObjects, T& collisionValidation,
137 const bool disableEffectorCollision) {
138 if (disableEffectorCollision) {
139 if (joint->name() == effector.name())
141 else if (joint->name() == effector.joint()->name())
143 else if (joint->numberChildJoints() == 0)
146 for (core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin(); cit != collisionObjects.end(); ++cit)
147 collisionValidation.addObstacleToJoint(*cit, joint,
false);
148 for (std::size_t i = 0; i < joint->numberChildJoints(); ++i)
149 addLimbCollisionRec<T>(joint->childJoint(i), effector, collisionObjects, collisionValidation,
150 disableEffectorCollision);
153 template <
typename T>
155 const core::ObjectStdVector_t& collisionObjects, T& collisionValidation) {
156 if (joint->name() == limbname)
return;
157 for (core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin(); cit != collisionObjects.end(); ++cit) {
159 if (joint->linkedBody()) {
160 std::cout <<
"remiove obstacle: " << limbname <<
" " << joint->name() <<
" " << (*cit)->name() << std::endl;
161 collisionValidation.removeObstacleFromJoint(joint, *cit);
163 }
catch (
const std::runtime_error& e) {
164 std::cout <<
"WARNING: " << e.what() << std::endl;
168 for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
173 template <
typename T>
175 const core::ObjectStdVector_t& collisionObjects, T& collisionValidation) {
176 if (std::find(keepers.begin(), keepers.end(), joint->name()) != keepers.end())
return;
177 for (core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin(); cit != collisionObjects.end(); ++cit) {
179 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) {
190 template <
typename K,
typename V>
191 void addToMap(std::map<K, V>& map,
const K& key,
const V& value) {
192 if (map.find(key) != map.end())
195 map.insert(std::make_pair(key, value));
198 template <
typename T>
200 if (std::find(data.begin(), data.end(), value) == data.end()) {
201 data.push_back(value);
210 #endif // HPP_RBPRM_TOOLS_HH
Definition: algorithm.hh:27