hpp-rbprm  4.10.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, const pinocchio::value_type& u);
39  pinocchio::value_type distance (pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2);
40 
41  template<typename K, typename V>
42  void addToMap(const K& key, const V& value);
43 
44  template<typename T>
45  bool insertIfNew(std::vector<T>& data, const T& value);
46 
47  template<typename T>
48  void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint, const core::ObjectStdVector_t& obstacles);
49  template<typename T>
50  void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint, const pinocchio::CollisionObjectPtr_t obstacle);
51  template<typename T>
52  void RemoveEffectorCollisionRec(T& validation, pinocchio::JointPtr_t joint, const pinocchio::CollisionObjectPtr_t obstacle);
53 
58  void LockJointRec(const std::string& spared, const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector);
59 
64  void LockJointRec(const std::vector<std::string>& spared, const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector);
65 
70  void LockJoint(const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector, const bool constant=true);
71 
73  namespace io
74  {
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);
82  void writeRotMatrixFCL(const fcl::Matrix3f& mat , std::ostream& output);
83  Eigen::MatrixXd readMatrix (std::ifstream& myfile);
84  fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile);
85  fcl::Vec3f readVecFCL (std::ifstream& myfile);
86  Eigen::MatrixXd readMatrix (std::ifstream& myfile, std::string& line);
87  fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile, std::string& line);
88  fcl::Vec3f readVecFCL (std::ifstream& myfile, std::string& line);
89  } // namespace io
90 
91  template<typename T>
92  void RemoveEffectorCollisionRec(T& validation, pinocchio::JointPtr_t joint, const pinocchio::CollisionObjectPtr_t obstacle)
93  {
94  try
95  {
96  validation.removeObstacleFromJoint(joint,obstacle);
97  }
98  catch(const std::runtime_error& e)
99  {
100  std::cout << "WARNING: " << e.what() << std::endl;
101  return;
102  }
103  //then sons
104  for(std::size_t i =0; i < joint->numberChildJoints(); ++i)
105  {
106  RemoveEffectorCollisionRec<T>(validation, joint->childJoint(i), obstacle);
107  }
108  }
109 
110  template<typename T>
111  void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint, const core::ObjectStdVector_t &obstacles)
112  {
113  for(core::ObjectStdVector_t::const_iterator cit = obstacles.begin();
114  cit != obstacles.end(); ++cit)
115  {
116  RemoveEffectorCollision<T>(validation,effectorJoint,*cit);
117  }
118  }
119 
120  template<typename T>
121  void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint, const pinocchio::CollisionObjectPtr_t obstacle)
122  {
123  try
124  {
125  //remove actual effector or not ?
126  validation.removeObstacleFromJoint(effectorJoint,obstacle);
127  }
128  catch(const std::runtime_error& e)
129  {
130  std::cout << "WARNING: " << e.what() << std::endl;
131  return;
132  }
133  //then sons
134  for(std::size_t i =0; i < effectorJoint->numberChildJoints(); ++i)
135  {
136  RemoveEffectorCollisionRec<T>(validation, effectorJoint->childJoint(i), obstacle);
137  }
138  }
139 
140  template<typename T>
141  void addLimbCollisionRec(pinocchio::JointPtr_t joint, const pinocchio::Frame& effector,
142  const core::ObjectStdVector_t &collisionObjects,
143  T& collisionValidation, const bool disableEffectorCollision)
144  {
145  if(disableEffectorCollision)
146  {
147  if (joint->name() == effector.name())
148  return;
149  else if(joint->name() == effector.joint()->name() )
150  return; // TODO only disable collision for frame
151  else if(joint->numberChildJoints() == 0)
152  return; // TODO only disable collision for frame
153  }
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);
159  }
160 
161 
162  template<typename T>
163  void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint, const std::string& limbname,
164  const core::ObjectStdVector_t &collisionObjects,
165  T& collisionValidation)
166  {
167  if(joint->name() == limbname) return;
168  for(core::ObjectStdVector_t::const_iterator cit = collisionObjects.begin();
169  cit != collisionObjects.end(); ++cit)
170  {
171  try
172  {
173  if(joint->linkedBody ())
174  {
175  std::cout << "remiove obstacle: " << limbname << " "<< joint->name() << " " << (*cit)->name() << std::endl;
176  collisionValidation.removeObstacleFromJoint(joint, *cit);
177  }
178  }
179  catch(const std::runtime_error& e)
180  {
181  std::cout << "WARNING: "<< e.what() << std::endl;
182  //return;
183  }
184  }
185  for(std::size_t i=0; i<joint->numberChildJoints(); ++i)
186  {
187  RemoveNonLimbCollisionRec(joint->childJoint(i), limbname, collisionObjects, collisionValidation);
188  }
189  }
190 
191 
192  template<typename T>
193  void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint, const std::vector<std::string>& keepers,
194  const core::ObjectStdVector_t &collisionObjects,
195  T& collisionValidation)
196  {
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)
200  {
201  try
202  {
203  collisionValidation.removeObstacleFromJoint(joint, *cit);
204  }
205  catch(const std::runtime_error& e)
206  {
207  std::cout << "WARNING: "<< e.what() << std::endl;
208  return;
209  }
210  }
211  for(std::size_t i=0; i<joint->numberChildJoints(); ++i)
212  {
213  RemoveNonLimbCollisionRec(joint->childJoint(i), keepers, collisionObjects, collisionValidation);
214  }
215  }
216 
217 
218  template<typename K, typename V>
219  void addToMap(std::map<K,V>& map, const K& key, const V& value)
220  {
221  if(map.find(key) != map.end())
222  map[key] = value;
223  else
224  map.insert(std::make_pair(key,value));
225  }
226 
227  template<typename T>
228  bool insertIfNew(std::vector<T>& data, const T& value)
229  {
230  if(std::find(data.begin(), data.end(), value) == data.end())
231  {
232  data.push_back(value);
233  return true;
234  }
235  return false;
236  }
237 
238 
239  } // namespace tools
240 } // namespace hpp
241 
242 #endif // HPP_RBPRM_TOOLS_HH
hpp::tools::insertIfNew
bool insertIfNew(std::vector< T > &data, const T &value)
Definition: tools.hh:228
hpp::tools::io::readVecFCL
fcl::Vec3f readVecFCL(std::ifstream &myfile)
hpp::tools::distance
pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2)
hpp::tools::LockJointRec
void LockJointRec(const std::string &spared, const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector)
hpp::tools::RemoveNonLimbCollisionRec
void RemoveNonLimbCollisionRec(const pinocchio::JointPtr_t joint, const std::string &limbname, const core::ObjectStdVector_t &collisionObjects, T &collisionValidation)
Definition: tools.hh:163
hpp::tools::addToMap
void addToMap(const K &key, const V &value)
hpp::tools::io::readRotMatrixFCL
fcl::Matrix3f readRotMatrixFCL(std::ifstream &myfile)
hpp::tools::io::StrToD
double StrToD(const std::string &str)
hpp::tools::GetXRotMatrix
fcl::Matrix3f GetXRotMatrix(const core::value_type theta)
hpp::tools::GetRotationMatrix
Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d &from, const Eigen::Vector3d &to)
Uses Rodriguez formula to find transformation between two vectors.
hpp::tools::interpolate
pinocchio::Configuration_t interpolate(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2, const pinocchio::value_type &u)
hpp::tools::RemoveEffectorCollision
void RemoveEffectorCollision(T &validation, pinocchio::JointPtr_t effectorJoint, const core::ObjectStdVector_t &obstacles)
Definition: tools.hh:111
hpp::tools::io::splitString
std::vector< std::string > splitString(const std::string &s, const char sep)
hpp::tools::RemoveEffectorCollisionRec
void RemoveEffectorCollisionRec(T &validation, pinocchio::JointPtr_t joint, const pinocchio::CollisionObjectPtr_t obstacle)
Definition: tools.hh:92
hpp
Definition: algorithm.hh:27
hpp::tools::io::writeMatrix
void writeMatrix(const Eigen::MatrixXd &mat, std::ostream &output)
hpp::tools::io::StrToI
int StrToI(const std::string &str)
hpp::tools::LockJoint
void LockJoint(const pinocchio::JointPtr_t joint, core::ConfigProjectorPtr_t projector, const bool constant=true)
hpp::tools::GetYRotMatrix
fcl::Matrix3f GetYRotMatrix(const core::value_type theta)
hpp::tools::io::writeRotMatrixFCL
void writeRotMatrixFCL(const fcl::Matrix3f &mat, std::ostream &output)
hpp::tools::addLimbCollisionRec
void addLimbCollisionRec(pinocchio::JointPtr_t joint, const pinocchio::Frame &effector, const core::ObjectStdVector_t &collisionObjects, T &collisionValidation, const bool disableEffectorCollision)
Definition: tools.hh:141
hpp::tools::io::writeVecFCL
void writeVecFCL(const fcl::Vec3f &vec, std::ostream &output)
hpp::tools::GetZRotMatrix
fcl::Matrix3f GetZRotMatrix(const core::value_type theta)
hpp::tools::io::readMatrix
Eigen::MatrixXd readMatrix(std::ifstream &myfile)
config.hh