hpp-rbprm  4.11.0
Implementation of RB-PRM planner using hpp.
tools.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Steve Tonneau (steve.tonneau@laas.fr)
4 //
5 // This file is part of hpp-rbprm.
6 // hpp-rbprm is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-rbprm is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_RBPRM_TOOLS_HH
20 #define HPP_RBPRM_TOOLS_HH
21 
22 #include <iostream>
23 
24 #include <hpp/core/config-validation.hh>
25 #include <hpp/pinocchio/joint.hh>
26 #include <hpp/rbprm/config.hh>
27 #include <hpp/pinocchio/collision-object.hh>
28 #include <hpp/pinocchio/frame.hh>
29 #include <Eigen/Core>
30 
31 namespace hpp {
32 namespace tools {
34 Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d& from, const Eigen::Vector3d& to);
35 fcl::Matrix3f GetZRotMatrix(const core::value_type theta);
36 fcl::Matrix3f GetYRotMatrix(const core::value_type theta);
37 fcl::Matrix3f GetXRotMatrix(const core::value_type theta);
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);
41 
42 template <typename K, typename V>
43 void addToMap(const K& key, const V& value);
44 
45 template <typename T>
46 bool insertIfNew(std::vector<T>& data, const T& value);
47 
48 template <typename T>
49 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
50  const core::ObjectStdVector_t& obstacles);
51 template <typename T>
52 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
53  const pinocchio::CollisionObjectPtr_t obstacle);
54 template <typename T>
55 void RemoveEffectorCollisionRec(T& validation, pinocchio::JointPtr_t joint,
56  const pinocchio::CollisionObjectPtr_t obstacle);
57 
62 void LockJointRec(const std::string& spared, const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector);
63 
68 void LockJointRec(const std::vector<std::string>& spared, const pinocchio::JointPtr_t joint,
69  core::ConfigProjectorPtr_t projector);
70 
75 void LockJoint(const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector, const bool constant = true);
76 
78 namespace io {
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);
86 void writeRotMatrixFCL(const fcl::Matrix3f& mat, std::ostream& output);
87 Eigen::MatrixXd readMatrix(std::ifstream& myfile);
88 fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile);
89 fcl::Vec3f readVecFCL(std::ifstream& myfile);
90 Eigen::MatrixXd readMatrix(std::ifstream& myfile, std::string& line);
91 fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile, std::string& line);
92 fcl::Vec3f readVecFCL(std::ifstream& myfile, std::string& line);
93 } // namespace io
94 
95 template <typename T>
96 void RemoveEffectorCollisionRec(T& validation, pinocchio::JointPtr_t joint,
97  const pinocchio::CollisionObjectPtr_t obstacle) {
98  try {
99  validation.removeObstacleFromJoint(joint, obstacle);
100  } catch (const std::runtime_error& e) {
101  std::cout << "WARNING: " << e.what() << std::endl;
102  return;
103  }
104  // then sons
105  for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
106  RemoveEffectorCollisionRec<T>(validation, joint->childJoint(i), obstacle);
107  }
108 }
109 
110 template <typename T>
111 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
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);
115  }
116 }
117 
118 template <typename T>
119 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
120  const pinocchio::CollisionObjectPtr_t obstacle) {
121  try {
122  // remove actual effector or not ?
123  validation.removeObstacleFromJoint(effectorJoint, obstacle);
124  } catch (const std::runtime_error& e) {
125  std::cout << "WARNING: " << e.what() << std::endl;
126  return;
127  }
128  // then sons
129  for (std::size_t i = 0; i < effectorJoint->numberChildJoints(); ++i) {
130  RemoveEffectorCollisionRec<T>(validation, effectorJoint->childJoint(i), obstacle);
131  }
132 }
133 
134 template <typename T>
135 void addLimbCollisionRec(pinocchio::JointPtr_t joint, const pinocchio::Frame& effector,
136  const core::ObjectStdVector_t& collisionObjects, T& collisionValidation,
137  const bool disableEffectorCollision) {
138  if (disableEffectorCollision) {
139  if (joint->name() == effector.name())
140  return;
141  else if (joint->name() == effector.joint()->name())
142  return; // TODO only disable collision for frame
143  else if (joint->numberChildJoints() == 0)
144  return; // TODO only disable collision for frame
145  }
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);
151 }
152 
153 template <typename T>
154 void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint, const std::string& limbname,
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) {
158  try {
159  if (joint->linkedBody()) {
160  std::cout << "remiove obstacle: " << limbname << " " << joint->name() << " " << (*cit)->name() << std::endl;
161  collisionValidation.removeObstacleFromJoint(joint, *cit);
162  }
163  } catch (const std::runtime_error& e) {
164  std::cout << "WARNING: " << e.what() << std::endl;
165  // return;
166  }
167  }
168  for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
169  RemoveNonLimbCollisionRec(joint->childJoint(i), limbname, collisionObjects, collisionValidation);
170  }
171 }
172 
173 template <typename T>
174 void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint, const std::vector<std::string>& keepers,
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) {
178  try {
179  collisionValidation.removeObstacleFromJoint(joint, *cit);
180  } catch (const std::runtime_error& e) {
181  std::cout << "WARNING: " << e.what() << std::endl;
182  return;
183  }
184  }
185  for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
186  RemoveNonLimbCollisionRec(joint->childJoint(i), keepers, collisionObjects, collisionValidation);
187  }
188 }
189 
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())
193  map[key] = value;
194  else
195  map.insert(std::make_pair(key, value));
196 }
197 
198 template <typename T>
199 bool insertIfNew(std::vector<T>& data, const T& value) {
200  if (std::find(data.begin(), data.end(), value) == data.end()) {
201  data.push_back(value);
202  return true;
203  }
204  return false;
205 }
206 
207 } // namespace tools
208 } // namespace hpp
209 
210 #endif // HPP_RBPRM_TOOLS_HH
fcl::Vec3f readVecFCL(std::ifstream &myfile)
void LockJoint(const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector, const bool constant=true)
Definition: algorithm.hh:27
double StrToD(const std::string &str)
bool insertIfNew(std::vector< T > &data, const T &value)
Definition: tools.hh:199
pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2)
void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint, const std::string &limbname, const core::ObjectStdVector_t &collisionObjects, T &collisionValidation)
Definition: tools.hh:154
int StrToI(const std::string &str)
fcl::Matrix3f readRotMatrixFCL(std::ifstream &myfile)
void RemoveEffectorCollisionRec(T &validation, pinocchio::JointPtr_t joint, const pinocchio::CollisionObjectPtr_t obstacle)
Definition: tools.hh:96
void addToMap(const K &key, const V &value)
void writeMatrix(const Eigen::MatrixXd &mat, std::ostream &output)
void writeRotMatrixFCL(const fcl::Matrix3f &mat, std::ostream &output)
std::vector< std::string > splitString(const std::string &s, const char sep)
fcl::Matrix3f GetXRotMatrix(const core::value_type theta)
Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d &from, const Eigen::Vector3d &to)
Uses Rodriguez formula to find transformation between two vectors.
fcl::Matrix3f GetZRotMatrix(const core::value_type theta)
Eigen::MatrixXd readMatrix(std::ifstream &myfile)
void RemoveEffectorCollision(T &validation, pinocchio::JointPtr_t effectorJoint, const core::ObjectStdVector_t &obstacles)
Definition: tools.hh:111
void addLimbCollisionRec(pinocchio::JointPtr_t joint, const pinocchio::Frame &effector, const core::ObjectStdVector_t &collisionObjects, T &collisionValidation, const bool disableEffectorCollision)
Definition: tools.hh:135
fcl::Matrix3f GetYRotMatrix(const core::value_type theta)
pinocchio::Configuration_t interpolate(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2, const pinocchio::value_type &u)
void writeVecFCL(const fcl::Vec3f &vec, std::ostream &output)
void LockJointRec(const std::string &spared, const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector)