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 
211 void AllowedCollisionMatrix::setEntry(bool allowed)
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;
224 }
225 
226 void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, const DecideContactFn& fn)
227 {
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
collision_detection::AllowedCollisionMatrix::setDefaultEntry
void setDefaultEntry(const std::string &name, bool allowed)
Set the default value for entries that include name. If such a default value is set,...
Definition: collision_matrix.cpp:251
f2
const T1 const T2 & f2
collision_detection::AllowedCollisionMatrix::hasEntry
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...
Definition: collision_matrix.cpp:133
collision_detection::AllowedCollisionMatrix::entries_
std::map< std::string, std::map< std::string, AllowedCollision::Type > > entries_
Definition: collision_matrix.h:305
collision_detection::AllowedCollisionMatrix::getDefaultEntry
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....
Definition: collision_matrix.cpp:264
collision_detection::AllowedCollisionMatrix::getAllowedCollision
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...
Definition: collision_matrix.cpp:287
collision_detection::AllowedCollisionMatrix::getEntry
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...
Definition: collision_matrix.cpp:120
collision_detection::AllowedCollisionMatrix::removeEntry
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...
Definition: collision_matrix.cpp:185
collision_detection::AllowedCollision::ALWAYS
@ ALWAYS
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
Definition: collision_matrix.h:157
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
collision_detection::AllowedCollisionMatrix::getMessage
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
Definition: collision_matrix.cpp:358
collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix
AllowedCollisionMatrix()
Definition: collision_matrix.cpp:75
name
std::string name
collision_detection::AllowedCollision::Type
Type
Definition: collision_matrix.h:117
collision_matrix.h
collision_detection::AllowedCollision::CONDITIONAL
@ CONDITIONAL
The collision is allowed depending on a predicate evaluated on the produced contact....
Definition: collision_matrix.h:162
collision_detection::andDecideContact
static bool andDecideContact(const DecideContactFn &f1, const DecideContactFn &f2, Contact &contact)
Definition: collision_matrix.cpp:282
collision_detection::AllowedCollisionMatrix::print
void print(std::ostream &out) const
Print the allowed collision matrix.
Definition: collision_matrix.cpp:394
collision_detection::AllowedCollisionMatrix::clear
void clear()
Clear the allowed collision matrix.
Definition: collision_matrix.cpp:340
collision_detection::AllowedCollisionMatrix::allowed_contacts_
std::map< std::string, std::map< std::string, DecideContactFn > > allowed_contacts_
Definition: collision_matrix.h:306
collision_detection::AllowedCollisionMatrix::default_entries_
std::map< std::string, AllowedCollision::Type > default_entries_
Definition: collision_matrix.h:308
collision_detection::AllowedCollisionMatrix::default_allowed_contacts_
std::map< std::string, DecideContactFn > default_allowed_contacts_
Definition: collision_matrix.h:309
length
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
collision_detection::AllowedCollisionMatrix::getAllEntryNames
void getAllEntryNames(std::vector< std::string > &names) const
Get all the names known to the collision matrix.
Definition: collision_matrix.cpp:348
f1
const T1 & f1
collision_detection::DecideContactFn
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
Definition: collision_matrix.h:104
collision_detection::AllowedCollision::NEVER
@ NEVER
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
Definition: collision_matrix.h:153
collision_detection::AllowedCollisionMatrix::setEntry
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
Definition: collision_matrix.cpp:147
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 21 2021 02:23:41