hpp-rbprm  4.14.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 <Eigen/Core>
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>
27 #include <hpp/rbprm/config.hh>
28 #include <iostream>
29 
30 namespace hpp {
31 namespace tools {
33 Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d& from,
34  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,
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);
43 
44 template <typename K, typename V>
45 void addToMap(const K& key, const V& value);
46 
47 template <typename T>
48 bool insertIfNew(std::vector<T>& data, const T& value);
49 
50 template <typename T>
51 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
52  const core::ObjectStdVector_t& obstacles);
53 template <typename T>
54 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
55  const pinocchio::CollisionObjectPtr_t obstacle);
56 template <typename T>
57 void RemoveEffectorCollisionRec(T& validation, pinocchio::JointPtr_t joint,
58  const pinocchio::CollisionObjectPtr_t obstacle);
59 
64 void LockJointRec(const std::string& spared, const pinocchio::JointPtr_t joint,
65  core::ConfigProjectorPtr_t projector);
66 
71 void LockJointRec(const std::vector<std::string>& spared,
72  const pinocchio::JointPtr_t joint,
73  core::ConfigProjectorPtr_t projector);
74 
80 void LockJoint(const pinocchio::JointPtr_t joint,
81  core::ConfigProjectorPtr_t projector,
82  const bool constant = true);
83 
85 namespace io {
86 double StrToD(const std::string& str);
87 int StrToI(const std::string& str);
88 double StrToD(std::ifstream& input);
89 int StrToI(std::ifstream& input);
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);
93 void writeRotMatrixFCL(const fcl::Matrix3f& mat, std::ostream& output);
94 Eigen::MatrixXd readMatrix(std::ifstream& myfile);
95 fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile);
96 fcl::Vec3f readVecFCL(std::ifstream& myfile);
97 Eigen::MatrixXd readMatrix(std::ifstream& myfile, std::string& line);
98 fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile, std::string& line);
99 fcl::Vec3f readVecFCL(std::ifstream& myfile, std::string& line);
100 } // namespace io
101 
102 template <typename T>
104  T& validation, pinocchio::JointPtr_t joint,
105  const pinocchio::CollisionObjectPtr_t obstacle) {
106  try {
107  validation.removeObstacleFromJoint(joint, obstacle);
108  } catch (const std::runtime_error& e) {
109  std::cout << "WARNING: " << e.what() << std::endl;
110  return;
111  }
112  // then sons
113  for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
114  RemoveEffectorCollisionRec<T>(validation, joint->childJoint(i), obstacle);
115  }
116 }
117 
118 template <typename T>
119 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
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);
124  }
125 }
126 
127 template <typename T>
128 void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
129  const pinocchio::CollisionObjectPtr_t obstacle) {
130  try {
131  // remove actual effector or not ?
132  validation.removeObstacleFromJoint(effectorJoint, obstacle);
133  } catch (const std::runtime_error& e) {
134  std::cout << "WARNING: " << e.what() << std::endl;
135  return;
136  }
137  // then sons
138  for (std::size_t i = 0; i < effectorJoint->numberChildJoints(); ++i) {
139  RemoveEffectorCollisionRec<T>(validation, effectorJoint->childJoint(i),
140  obstacle);
141  }
142 }
143 
144 template <typename T>
145 void addLimbCollisionRec(pinocchio::JointPtr_t joint,
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())
152  return;
153  else if (joint->name() == effector.joint()->name())
154  return; // TODO only disable collision for frame
155  else if (joint->numberChildJoints() == 0)
156  return; // TODO only disable collision for frame
157  }
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);
164 }
165 
166 template <typename T>
167 void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint,
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) {
174  try {
175  if (joint->linkedBody()) {
176  std::cout << "remiove obstacle: " << limbname << " " << joint->name()
177  << " " << (*cit)->name() << std::endl;
178  collisionValidation.removeObstacleFromJoint(joint, *cit);
179  }
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), limbname, collisionObjects,
187  collisionValidation);
188  }
189 }
190 
191 template <typename T>
192 void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint,
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())
197  return;
198  for (core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin();
199  cit != collisionObjects.end(); ++cit) {
200  try {
201  collisionValidation.removeObstacleFromJoint(joint, *cit);
202  } catch (const std::runtime_error& e) {
203  std::cout << "WARNING: " << e.what() << std::endl;
204  return;
205  }
206  }
207  for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
208  RemoveNonLimbCollisionRec(joint->childJoint(i), keepers, collisionObjects,
209  collisionValidation);
210  }
211 }
212 
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())
216  map[key] = value;
217  else
218  map.insert(std::make_pair(key, value));
219 }
220 
221 template <typename T>
222 bool insertIfNew(std::vector<T>& data, const T& value) {
223  if (std::find(data.begin(), data.end(), value) == data.end()) {
224  data.push_back(value);
225  return true;
226  }
227  return false;
228 }
229 
230 } // namespace tools
231 } // namespace hpp
232 
233 #endif // HPP_RBPRM_TOOLS_HH
double StrToD(const std::string &str)
void writeMatrix(const Eigen::MatrixXd &mat, std::ostream &output)
fcl::Vec3f readVecFCL(std::ifstream &myfile)
int StrToI(const std::string &str)
void writeVecFCL(const fcl::Vec3f &vec, std::ostream &output)
fcl::Matrix3f readRotMatrixFCL(std::ifstream &myfile)
std::vector< std::string > splitString(const std::string &s, const char sep)
Eigen::MatrixXd readMatrix(std::ifstream &myfile)
void writeRotMatrixFCL(const fcl::Matrix3f &mat, std::ostream &output)
void RemoveEffectorCollision(T &validation, pinocchio::JointPtr_t effectorJoint, const core::ObjectStdVector_t &obstacles)
Definition: tools.hh:119
fcl::Matrix3f GetZRotMatrix(const core::value_type theta)
void LockJoint(const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector, const bool constant=true)
void RemoveEffectorCollisionRec(T &validation, pinocchio::JointPtr_t joint, const pinocchio::CollisionObjectPtr_t obstacle)
Definition: tools.hh:103
bool insertIfNew(std::vector< T > &data, const T &value)
Definition: tools.hh:222
void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint, const std::string &limbname, const core::ObjectStdVector_t &collisionObjects, T &collisionValidation)
Definition: tools.hh:167
pinocchio::Configuration_t interpolate(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2, const pinocchio::value_type &u)
Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d &from, const Eigen::Vector3d &to)
Uses Rodriguez formula to find transformation between two vectors.
void addLimbCollisionRec(pinocchio::JointPtr_t joint, const pinocchio::Frame &effector, const core::ObjectStdVector_t &collisionObjects, T &collisionValidation, const bool disableEffectorCollision)
Definition: tools.hh:145
pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2)
fcl::Matrix3f GetYRotMatrix(const core::value_type theta)
void addToMap(const K &key, const V &value)
fcl::Matrix3f GetXRotMatrix(const core::value_type theta)
void LockJointRec(const std::string &spared, const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector)
Definition: algorithm.hh:26