hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
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
30namespace hpp {
31namespace tools {
33Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d& from,
34 const Eigen::Vector3d& to);
35fcl::Matrix3f GetZRotMatrix(const core::value_type theta);
36fcl::Matrix3f GetYRotMatrix(const core::value_type theta);
37fcl::Matrix3f GetXRotMatrix(const core::value_type theta);
38pinocchio::Configuration_t interpolate(pinocchio::ConfigurationIn_t q1,
39 pinocchio::ConfigurationIn_t q2,
40 const pinocchio::value_type& u);
41pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1,
42 pinocchio::ConfigurationIn_t q2);
43
44template <typename K, typename V>
45void addToMap(const K& key, const V& value);
46
47template <typename T>
48bool insertIfNew(std::vector<T>& data, const T& value);
49
50template <typename T>
51void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
52 const core::ObjectStdVector_t& obstacles);
53template <typename T>
54void RemoveEffectorCollision(T& validation, pinocchio::JointPtr_t effectorJoint,
55 const pinocchio::CollisionObjectPtr_t obstacle);
56template <typename T>
57void RemoveEffectorCollisionRec(T& validation, pinocchio::JointPtr_t joint,
58 const pinocchio::CollisionObjectPtr_t obstacle);
59
64void LockJointRec(const std::string& spared, const pinocchio::JointPtr_t joint,
65 core::ConfigProjectorPtr_t projector);
66
71void LockJointRec(const std::vector<std::string>& spared,
72 const pinocchio::JointPtr_t joint,
73 core::ConfigProjectorPtr_t projector);
74
80void LockJoint(const pinocchio::JointPtr_t joint,
81 core::ConfigProjectorPtr_t projector,
82 const bool constant = true);
83
85namespace io {
86double StrToD(const std::string& str);
87int StrToI(const std::string& str);
88double StrToD(std::ifstream& input);
89int StrToI(std::ifstream& input);
90std::vector<std::string> splitString(const std::string& s, const char sep);
91void writeMatrix(const Eigen::MatrixXd& mat, std::ostream& output);
92void writeVecFCL(const fcl::Vec3f& vec, std::ostream& output);
93void writeRotMatrixFCL(const fcl::Matrix3f& mat, std::ostream& output);
94Eigen::MatrixXd readMatrix(std::ifstream& myfile);
95fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile);
96fcl::Vec3f readVecFCL(std::ifstream& myfile);
97Eigen::MatrixXd readMatrix(std::ifstream& myfile, std::string& line);
98fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile, std::string& line);
99fcl::Vec3f readVecFCL(std::ifstream& myfile, std::string& line);
100} // namespace io
101
102template <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
118template <typename T>
119void 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
127template <typename T>
128void 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
144template <typename T>
145void 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
166template <typename T>
167void 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
191template <typename T>
192void 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
213template <typename K, typename V>
214void 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
221template <typename T>
222bool 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)
Eigen::MatrixXd readMatrix(std::ifstream &myfile)
void writeRotMatrixFCL(const fcl::Matrix3f &mat, std::ostream &output)
std::vector< std::string > splitString(const std::string &s, const char sep)
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