collision_matrix.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, E. Gil Jones */
36 
38 #include <boost/bind.hpp>
39 #include <iomanip>
40 
41 namespace collision_detection
42 {
44 {
45 }
46 
47 AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed)
48 {
49  for (std::size_t i = 0; i < names.size(); ++i)
50  for (std::size_t j = i; j < names.size(); ++j)
51  setEntry(names[i], names[j], allowed);
52 }
53 
54 AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg)
55 {
56  if (msg.entry_names.size() != msg.entry_values.size() ||
57  msg.default_entry_names.size() != msg.default_entry_values.size())
58  ROS_ERROR_NAMED("collision_detection", "The number of links does not match the number of entries "
59  "in AllowedCollisionMatrix message");
60  else
61  {
62  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
63  if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
64  ROS_ERROR_NAMED("collision_detection",
65  "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
66  msg.entry_names[i].c_str());
67  else
68  for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j)
69  setEntry(msg.entry_names[i], msg.entry_names[j], msg.entry_values[i].enabled[j]);
70 
71  for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
72  setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]);
73  }
74 }
75 
76 bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const
77 {
78  auto it1 = allowed_contacts_.find(name1);
79  if (it1 == allowed_contacts_.end())
80  return false;
81  auto it2 = it1->second.find(name2);
82  if (it2 == it1->second.end())
83  return false;
84  fn = it2->second;
85  return true;
86 }
87 
88 bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2,
89  AllowedCollision::Type& allowed_collision) const
90 {
91  auto it1 = entries_.find(name1);
92  if (it1 == entries_.end())
93  return false;
94  auto it2 = it1->second.find(name2);
95  if (it2 == it1->second.end())
96  return false;
97  allowed_collision = it2->second;
98  return true;
99 }
100 
101 bool AllowedCollisionMatrix::hasEntry(const std::string& name) const
102 {
103  return entries_.find(name) != entries_.end();
104 }
105 
106 bool AllowedCollisionMatrix::hasEntry(const std::string& name1, const std::string& name2) const
107 {
108  auto it1 = entries_.find(name1);
109  if (it1 == entries_.end())
110  return false;
111  auto it2 = it1->second.find(name2);
112  return it2 != it1->second.end();
113 }
114 
115 void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, bool allowed)
116 {
118  entries_[name1][name2] = entries_[name2][name1] = v;
119 
120  // remove boost::function pointers, if any
121  auto it = allowed_contacts_.find(name1);
122  if (it != allowed_contacts_.end())
123  {
124  auto jt = it->second.find(name2);
125  if (jt != it->second.end())
126  it->second.erase(jt);
127  }
128  it = allowed_contacts_.find(name2);
129  if (it != allowed_contacts_.end())
130  {
131  auto jt = it->second.find(name1);
132  if (jt != it->second.end())
133  it->second.erase(jt);
134  }
135 }
136 
137 void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn)
138 {
139  entries_[name1][name2] = entries_[name2][name1] = AllowedCollision::CONDITIONAL;
140  allowed_contacts_[name1][name2] = allowed_contacts_[name2][name1] = fn;
141 }
142 
143 void AllowedCollisionMatrix::removeEntry(const std::string& name)
144 {
145  entries_.erase(name);
146  allowed_contacts_.erase(name);
147  for (auto& entry : entries_)
148  entry.second.erase(name);
149  for (auto& allowed_contact : allowed_contacts_)
150  allowed_contact.second.erase(name);
151 }
152 
153 void AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::string& name2)
154 {
155  auto jt = entries_.find(name1);
156  if (jt != entries_.end())
157  {
158  auto it = jt->second.find(name2);
159  if (it != jt->second.end())
160  jt->second.erase(it);
161  }
162  jt = entries_.find(name2);
163  if (jt != entries_.end())
164  {
165  auto it = jt->second.find(name1);
166  if (it != jt->second.end())
167  jt->second.erase(it);
168  }
169 
170  auto it = allowed_contacts_.find(name1);
171  if (it != allowed_contacts_.end())
172  {
173  auto jt = it->second.find(name2);
174  if (jt != it->second.end())
175  it->second.erase(jt);
176  }
177  it = allowed_contacts_.find(name2);
178  if (it != allowed_contacts_.end())
179  {
180  auto jt = it->second.find(name1);
181  if (jt != it->second.end())
182  it->second.erase(jt);
183  }
184 }
185 
186 void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed)
187 {
188  for (const auto& other_name : other_names)
189  if (other_name != name)
190  setEntry(other_name, name, allowed);
191 }
192 
193 void AllowedCollisionMatrix::setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2,
194  bool allowed)
195 {
196  for (const auto& name1 : names1)
197  setEntry(name1, names2, allowed);
198 }
199 
200 void AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
201 {
202  std::string last = name;
203  for (auto& entry : entries_)
204  if (name != entry.first && last != entry.first)
205  {
206  last = entry.first;
207  setEntry(name, entry.first, allowed);
208  }
209 }
210 
212 {
214  for (auto& entry : entries_)
215  for (auto& it2 : entry.second)
216  it2.second = v;
217 }
218 
219 void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, bool allowed)
220 {
222  default_entries_[name] = v;
223  default_allowed_contacts_.erase(name);
224 }
225 
226 void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, const DecideContactFn& fn)
227 {
229  default_allowed_contacts_[name] = fn;
230 }
231 
232 bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const
233 {
234  auto it = default_entries_.find(name);
235  if (it == default_entries_.end())
236  return false;
237  allowed_collision = it->second;
238  return true;
239 }
240 
241 bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name, DecideContactFn& fn) const
242 {
243  auto it = default_allowed_contacts_.find(name);
244  if (it == default_allowed_contacts_.end())
245  return false;
246  fn = it->second;
247  return true;
248 }
249 
250 static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f2, Contact& contact)
251 {
252  return f1(contact) && f2(contact);
253 }
254 
255 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
256  DecideContactFn& fn) const
257 {
258  DecideContactFn fn1, fn2;
259  bool found1 = getDefaultEntry(name1, fn1);
260  bool found2 = getDefaultEntry(name2, fn2);
261 
262  if (!found1 && !found2)
263  return getEntry(name1, name2, fn);
264  else
265  {
266  if (found1 && !found2)
267  fn = fn1;
268  else if (!found1 && found2)
269  fn = fn2;
270  else if (found1 && found2)
271  fn = boost::bind(&andDecideContact, fn1, fn2, _1);
272  else
273  return false;
274  return true;
275  }
276 }
277 
278 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
279  AllowedCollision::Type& allowed_collision) const
280 {
281  AllowedCollision::Type t1, t2;
282  bool found1 = getDefaultEntry(name1, t1);
283  bool found2 = getDefaultEntry(name2, t2);
284 
285  if (!found1 && !found2)
286  return getEntry(name1, name2, allowed_collision);
287  else
288  {
289  if (found1 && !found2)
290  allowed_collision = t1;
291  else if (!found1 && found2)
292  allowed_collision = t2;
293  else if (found1 && found2)
294  {
296  allowed_collision = AllowedCollision::NEVER;
298  allowed_collision = AllowedCollision::CONDITIONAL;
299  else // ALWAYS is the only remaining case
300  allowed_collision = AllowedCollision::ALWAYS;
301  }
302  else
303  return false;
304  return true;
305  }
306 }
307 
309 {
310  entries_.clear();
311  allowed_contacts_.clear();
312  default_entries_.clear();
314 }
315 
316 void AllowedCollisionMatrix::getAllEntryNames(std::vector<std::string>& names) const
317 {
318  names.clear();
319  for (const auto& entry : entries_)
320  if (!names.empty() && names.back() == entry.first)
321  continue;
322  else
323  names.push_back(entry.first);
324 }
325 
326 void AllowedCollisionMatrix::getMessage(moveit_msgs::AllowedCollisionMatrix& msg) const
327 {
328  msg.entry_names.clear();
329  msg.entry_values.clear();
330  msg.default_entry_names.clear();
331  msg.default_entry_values.clear();
332 
333  getAllEntryNames(msg.entry_names);
334  std::sort(msg.entry_names.begin(), msg.entry_names.end());
335 
336  msg.entry_values.resize(msg.entry_names.size());
337  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
338  msg.entry_values[i].enabled.resize(msg.entry_names.size(), false);
339 
340  // there is an approximation here: if we use a boost function to decide
341  // whether a collision is allowed or not, we just assume the collision is not allowed.
342  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
343  {
345  bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
346  if (dfound)
347  {
348  msg.default_entry_names.push_back(msg.entry_names[i]);
349  msg.default_entry_values.push_back(dtype == AllowedCollision::ALWAYS);
350  }
351 
352  for (std::size_t j = i; j < msg.entry_names.size(); ++j)
353  {
355  bool found = getEntry(msg.entry_names[i], msg.entry_names[j], type);
356  if (found)
357  msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = type == AllowedCollision::ALWAYS;
358  }
359  }
360 }
361 
362 void AllowedCollisionMatrix::print(std::ostream& out) const
363 {
364  std::vector<std::string> names;
365  getAllEntryNames(names);
366  std::sort(names.begin(), names.end());
367 
368  std::size_t spacing = 4;
369  for (auto& name : names)
370  {
371  std::size_t length = name.length();
372  if (length > spacing)
373  spacing = length;
374  }
375  ++spacing;
376 
377  std::size_t number_digits = 2;
378  while (names.size() > pow(10, number_digits) - 1)
379  number_digits++;
380 
381  // print indices along the top of the matrix
382  for (std::size_t j = 0; j < number_digits; ++j)
383  {
384  out << std::setw(spacing + number_digits + 4) << "";
385  for (std::size_t i = 0; i < names.size(); ++i)
386  {
387  std::stringstream ss;
388  ss << std::setw(number_digits) << i;
389  out << std::setw(3) << ss.str().c_str()[j];
390  }
391  out << std::endl;
392  }
393 
394  for (std::size_t i = 0; i < names.size(); ++i)
395  {
396  out << std::setw(spacing) << names[i];
397  out << std::setw(number_digits + 1) << i;
398  out << " | ";
399  for (std::size_t j = 0; j < names.size(); ++j)
400  {
402  bool found = getAllowedCollision(names[i], names[j], type);
403  if (found)
404  out << std::setw(3) << (type == AllowedCollision::ALWAYS ? '1' : (type == AllowedCollision::NEVER ? '0' : '?'));
405  else
406  out << std::setw(3) << '-';
407  }
408  out << std::endl;
409  }
410 }
411 
412 } // end of namespace collision_detection
std::map< std::string, std::map< std::string, AllowedCollision::Type > > entries_
bool getEntry(const std::string &name1, const std::string &name2, AllowedCollision::Type &allowed_collision_type) const
Get the type of the allowed collision between two elements. Return true if the entry is included in t...
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
static bool andDecideContact(const DecideContactFn &f1, const DecideContactFn &f2, Contact &contact)
bool getDefaultEntry(const std::string &name, AllowedCollision::Type &allowed_collision) const
Get the type of the allowed collision between to be considered by default for an element. Return true if a default value was found for the specified element. Return false otherwise.
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
std::map< std::string, AllowedCollision::Type > default_entries_
Generic interface to collision detection.
std::map< std::string, DecideContactFn > default_allowed_contacts_
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
The collision is allowed depending on a predicate evaluated on the produced contact. If the predicate returns true, this particular contact is deemed ok (or allowed), i.e., the contact does not imply that the two bodies are in collision.
void print(std::ostream &out) const
Print the allowed collision matrix.
std::map< std::string, std::map< std::string, DecideContactFn > > allowed_contacts_
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
bool hasEntry(const std::string &name) const
Check if the allowed collision matrix has an entry at all for an element. Returns true if the element...
void setDefaultEntry(const std::string &name, bool allowed)
Set the default value for entries that include name. If such a default value is set, queries to getAllowedCollision() that include name will return this value instead, even if a pair that includes name was previously specified with setEntry().
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
#define ROS_ERROR_NAMED(name,...)
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
void clear()
Clear the allowed collision matrix.
bool getAllowedCollision(const std::string &name1, const std::string &name2, DecideContactFn &fn) const
Get the allowed collision predicate between two elements. Return true if a predicate for entry is inc...
void removeEntry(const std::string &name1, const std::string &name2)
Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in th...
void getAllEntryNames(std::vector< std::string > &names) const
Get all the names known to the collision matrix.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Nov 23 2020 03:52:30