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 
42 {
43 }
44 
45 collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed)
46 {
47  for (std::size_t i = 0; i < names.size(); ++i)
48  for (std::size_t j = i; j < names.size(); ++j)
49  setEntry(names[i], names[j], allowed);
50 }
51 
52 collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg)
53 {
54  if (msg.entry_names.size() != msg.entry_values.size() ||
55  msg.default_entry_names.size() != msg.default_entry_values.size())
56  CONSOLE_BRIDGE_logError("The number of links does not match the number of entries in AllowedCollisionMatrix "
57  "message");
58  else
59  {
60  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
61  if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
62  CONSOLE_BRIDGE_logError("Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
63  msg.entry_names[i].c_str());
64  else
65  for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j)
66  setEntry(msg.entry_names[i], msg.entry_names[j], msg.entry_values[i].enabled[j]);
67 
68  for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
69  setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]);
70  }
71 }
72 
74 {
75  entries_ = acm.entries_;
79 }
80 
81 bool collision_detection::AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2,
82  DecideContactFn& fn) const
83 {
84  auto it1 = allowed_contacts_.find(name1);
85  if (it1 == allowed_contacts_.end())
86  return false;
87  auto it2 = it1->second.find(name2);
88  if (it2 == it1->second.end())
89  return false;
90  fn = it2->second;
91  return true;
92 }
93 
94 bool collision_detection::AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2,
95  AllowedCollision::Type& allowed_collision) const
96 {
97  auto it1 = entries_.find(name1);
98  if (it1 == entries_.end())
99  return false;
100  auto it2 = it1->second.find(name2);
101  if (it2 == it1->second.end())
102  return false;
103  allowed_collision = it2->second;
104  return true;
105 }
106 
107 bool collision_detection::AllowedCollisionMatrix::hasEntry(const std::string& name) const
108 {
109  return entries_.find(name) != entries_.end();
110 }
111 
112 bool collision_detection::AllowedCollisionMatrix::hasEntry(const std::string& name1, const std::string& name2) const
113 {
114  auto it1 = entries_.find(name1);
115  if (it1 == entries_.end())
116  return false;
117  auto it2 = it1->second.find(name2);
118  if (it2 == it1->second.end())
119  return false;
120  return true;
121 }
122 
123 void collision_detection::AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2,
124  bool allowed)
125 {
127  entries_[name1][name2] = entries_[name2][name1] = v;
128 
129  // remove boost::function pointers, if any
130  auto it = allowed_contacts_.find(name1);
131  if (it != allowed_contacts_.end())
132  {
133  auto jt = it->second.find(name2);
134  if (jt != it->second.end())
135  it->second.erase(jt);
136  }
137  it = allowed_contacts_.find(name2);
138  if (it != allowed_contacts_.end())
139  {
140  auto jt = it->second.find(name1);
141  if (jt != it->second.end())
142  it->second.erase(jt);
143  }
144 }
145 
146 void collision_detection::AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2,
147  const DecideContactFn& fn)
148 {
149  entries_[name1][name2] = entries_[name2][name1] = AllowedCollision::CONDITIONAL;
150  allowed_contacts_[name1][name2] = allowed_contacts_[name2][name1] = fn;
151 }
152 
154 {
155  entries_.erase(name);
156  allowed_contacts_.erase(name);
157  for (auto& entry : entries_)
158  entry.second.erase(name);
159  for (auto& allowed_contact : allowed_contacts_)
160  allowed_contact.second.erase(name);
161 }
162 
163 void collision_detection::AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::string& name2)
164 {
165  auto jt = entries_.find(name1);
166  if (jt != entries_.end())
167  {
168  auto it = jt->second.find(name2);
169  if (it != jt->second.end())
170  jt->second.erase(it);
171  }
172  jt = entries_.find(name2);
173  if (jt != entries_.end())
174  {
175  auto it = jt->second.find(name1);
176  if (it != jt->second.end())
177  jt->second.erase(it);
178  }
179 
180  auto it = allowed_contacts_.find(name1);
181  if (it != allowed_contacts_.end())
182  {
183  auto jt = it->second.find(name2);
184  if (jt != it->second.end())
185  it->second.erase(jt);
186  }
187  it = allowed_contacts_.find(name2);
188  if (it != allowed_contacts_.end())
189  {
190  auto jt = it->second.find(name1);
191  if (jt != it->second.end())
192  it->second.erase(jt);
193  }
194 }
195 
197  const std::vector<std::string>& other_names, bool allowed)
198 {
199  for (const auto& other_name : other_names)
200  if (other_name != name)
201  setEntry(other_name, name, allowed);
202 }
203 
204 void collision_detection::AllowedCollisionMatrix::setEntry(const std::vector<std::string>& names1,
205  const std::vector<std::string>& names2, bool allowed)
206 {
207  for (const auto& name1 : names1)
208  setEntry(name1, names2, allowed);
209 }
210 
211 void collision_detection::AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
212 {
213  std::string last = name;
214  for (auto& entry : entries_)
215  if (name != entry.first && last != entry.first)
216  {
217  last = entry.first;
218  setEntry(name, entry.first, allowed);
219  }
220 }
221 
223 {
225  for (auto& entry : entries_)
226  for (auto& it2 : entry.second)
227  it2.second = v;
228 }
229 
230 void collision_detection::AllowedCollisionMatrix::setDefaultEntry(const std::string& name, bool allowed)
231 {
233  default_entries_[name] = v;
234  default_allowed_contacts_.erase(name);
235 }
236 
238 {
240  default_allowed_contacts_[name] = fn;
241 }
242 
244  AllowedCollision::Type& allowed_collision) const
245 {
246  auto it = default_entries_.find(name);
247  if (it == default_entries_.end())
248  return false;
249  allowed_collision = it->second;
250  return true;
251 }
252 
254 {
255  auto it = default_allowed_contacts_.find(name);
256  if (it == default_allowed_contacts_.end())
257  return false;
258  fn = it->second;
259  return true;
260 }
261 
262 namespace collision_detection
263 {
264 static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f2,
266 {
267  return f1(contact) && f2(contact);
268 }
269 }
270 
272  const std::string& name2,
273  DecideContactFn& fn) const
274 {
275  DecideContactFn fn1, fn2;
276  bool found1 = getDefaultEntry(name1, fn1);
277  bool found2 = getDefaultEntry(name2, fn2);
278 
279  if (!found1 && !found2)
280  return getEntry(name1, name2, fn);
281  else
282  {
283  if (found1 && !found2)
284  fn = fn1;
285  else if (!found1 && found2)
286  fn = fn2;
287  else if (found1 && found2)
288  fn = boost::bind(&andDecideContact, fn1, fn2, _1);
289  else
290  return false;
291  return true;
292  }
293 }
294 
296  const std::string& name2,
297  AllowedCollision::Type& allowed_collision) const
298 {
299  AllowedCollision::Type t1, t2;
300  bool found1 = getDefaultEntry(name1, t1);
301  bool found2 = getDefaultEntry(name2, t2);
302 
303  if (!found1 && !found2)
304  return getEntry(name1, name2, allowed_collision);
305  else
306  {
307  if (found1 && !found2)
308  allowed_collision = t1;
309  else if (!found1 && found2)
310  allowed_collision = t2;
311  else if (found1 && found2)
312  {
314  allowed_collision = AllowedCollision::NEVER;
316  allowed_collision = AllowedCollision::CONDITIONAL;
317  else // ALWAYS is the only remaining case
318  allowed_collision = AllowedCollision::ALWAYS;
319  }
320  else
321  return false;
322  return true;
323  }
324 }
325 
327 {
328  entries_.clear();
329  allowed_contacts_.clear();
330  default_entries_.clear();
332 }
333 
334 void collision_detection::AllowedCollisionMatrix::getAllEntryNames(std::vector<std::string>& names) const
335 {
336  names.clear();
337  for (const auto& entry : entries_)
338  if (!names.empty() && names.back() == entry.first)
339  continue;
340  else
341  names.push_back(entry.first);
342 }
343 
344 void collision_detection::AllowedCollisionMatrix::getMessage(moveit_msgs::AllowedCollisionMatrix& msg) const
345 {
346  msg.entry_names.clear();
347  msg.entry_values.clear();
348  msg.default_entry_names.clear();
349  msg.default_entry_values.clear();
350 
351  getAllEntryNames(msg.entry_names);
352  std::sort(msg.entry_names.begin(), msg.entry_names.end());
353 
354  msg.entry_values.resize(msg.entry_names.size());
355  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
356  msg.entry_values[i].enabled.resize(msg.entry_names.size(), false);
357 
358  // there is an approximation here: if we use a boost function to decide
359  // whether a collision is allowed or not, we just assume the collision is not allowed.
360  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
361  {
363  bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
364  if (dfound)
365  {
366  msg.default_entry_names.push_back(msg.entry_names[i]);
367  msg.default_entry_values.push_back(dtype == AllowedCollision::ALWAYS);
368  }
369 
370  for (std::size_t j = i; j < msg.entry_names.size(); ++j)
371  {
373  bool found = getEntry(msg.entry_names[i], msg.entry_names[j], type);
374  if (found)
375  msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = type == AllowedCollision::ALWAYS;
376  }
377  }
378 }
379 
381 {
382  std::vector<std::string> names;
383  getAllEntryNames(names);
384  std::sort(names.begin(), names.end());
385 
386  std::size_t L = 4;
387  for (auto& name : names)
388  {
389  std::size_t l = name.length();
390  if (l > L)
391  L = l;
392  }
393  ++L;
394 
395  std::size_t D = 2;
396  while (names.size() > pow(10, D) - 1)
397  D++;
398 
399  // print indices along the top of the matrix
400  for (std::size_t j = 0; j < D; ++j)
401  {
402  out << std::setw(L + D + 4) << "";
403  for (std::size_t i = 0; i < names.size(); ++i)
404  {
405  std::stringstream ss;
406  ss << std::setw(D) << i;
407  out << std::setw(3) << ss.str().c_str()[j];
408  }
409  out << std::endl;
410  }
411 
412  for (std::size_t i = 0; i < names.size(); ++i)
413  {
414  out << std::setw(L) << names[i];
415  out << std::setw(D + 1) << i;
416  out << " | ";
417  for (std::size_t j = 0; j < names.size(); ++j)
418  {
420  bool found = getAllowedCollision(names[i], names[j], type);
421  if (found)
422  out << std::setw(3) << (type == AllowedCollision::ALWAYS ? '1' : (type == AllowedCollision::NEVER ? '0' : '?'));
423  else
424  out << std::setw(3) << '-';
425  }
426  out << std::endl;
427  }
428 }
std::map< std::string, std::map< std::string, AllowedCollision::Type > > entries_
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
void print(std::ostream &out) const
Print the allowed collision matrix.
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...
static bool andDecideContact(const DecideContactFn &f1, const DecideContactFn &f2, collision_detection::Contact &contact)
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...
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
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...
std::map< std::string, AllowedCollision::Type > default_entries_
Generic interface to collision detection.
std::map< std::string, DecideContactFn > default_allowed_contacts_
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.
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
std::map< std::string, std::map< std::string, DecideContactFn > > allowed_contacts_
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.
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)
#define CONSOLE_BRIDGE_logError(fmt,...)
void getAllEntryNames(std::vector< std::string > &names) const
Get all the names known to the collision matrix.
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
void clear()
Clear the allowed collision matrix.
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...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Apr 18 2018 02:49:03