test_fcl_collision_detection.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, 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: E. Gil Jones */
36 
42 
43 #include <urdf_parser/urdf_parser.h>
45 
46 #include <gtest/gtest.h>
47 #include <sstream>
48 #include <algorithm>
49 #include <ctype.h>
50 #include <fstream>
51 
54 
55 class FclCollisionDetectionTester : public testing::Test
56 {
57 protected:
58  void SetUp() override
59  {
61  robot_model_ok_ = static_cast<bool>(robot_model_);
62  kinect_dae_resource_ = "package://moveit_resources/pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae";
63 
64  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
65 
67  cworld_.reset(new DefaultCWorldType());
68  }
69 
70  void TearDown() override
71  {
72  }
73 
74 protected:
76 
77  robot_model::RobotModelPtr robot_model_;
78 
79  collision_detection::CollisionRobotPtr crobot_;
80  collision_detection::CollisionWorldPtr cworld_;
81 
82  collision_detection::AllowedCollisionMatrixPtr acm_;
83 
84  std::string kinect_dae_resource_;
85 };
86 
88 {
89  ASSERT_TRUE(robot_model_ok_);
90 }
91 
92 TEST_F(FclCollisionDetectionTester, DefaultNotInCollision)
93 {
95  robot_state.setToDefaultValues();
96  robot_state.update();
97 
100  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
101  ASSERT_FALSE(res.collision);
102 }
103 
105 {
110  // req.contacts = true;
111  // req.max_contacts = 100;
112 
114  robot_state.setToDefaultValues();
115  robot_state.update();
116 
117  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
118  offset.translation().x() = .01;
119 
120  // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity());
121  // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
122  robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
123  robot_state.updateStateWithLinkAt("base_bellow_link", offset);
124  robot_state.update();
125 
126  acm_->setEntry("base_link", "base_bellow_link", false);
127  crobot_->checkSelfCollision(req, res1, robot_state, *acm_);
128  ASSERT_TRUE(res1.collision);
129 
130  acm_->setEntry("base_link", "base_bellow_link", true);
131  crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
132  ASSERT_FALSE(res2.collision);
133 
134  // req.verbose = true;
135  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity());
136  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
137  robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
138  robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
139  robot_state.update();
140 
141  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
142  crobot_->checkSelfCollision(req, res3, robot_state, *acm_);
143  ASSERT_TRUE(res3.collision);
144 }
145 
147 {
149  req.contacts = true;
150  req.max_contacts = 1;
151 
153  robot_state.setToDefaultValues();
154  robot_state.update();
155 
156  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
157  offset.translation().x() = .01;
158 
159  // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity());
160  // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
161  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity());
162  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
163 
164  robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
165  robot_state.updateStateWithLinkAt("base_bellow_link", offset);
166  robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
167  robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
168  robot_state.update();
169 
170  acm_->setEntry("base_link", "base_bellow_link", false);
171  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
172 
174  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
175  ASSERT_TRUE(res.collision);
176  EXPECT_EQ(res.contacts.size(), 1u);
177  EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
178 
179  res.clear();
180  req.max_contacts = 2;
181  req.max_contacts_per_pair = 1;
182  // req.verbose = true;
183  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
184  ASSERT_TRUE(res.collision);
185  EXPECT_EQ(res.contacts.size(), 2u);
186  EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
187 
188  res.contacts.clear();
189  res.contact_count = 0;
190 
191  req.max_contacts = 10;
192  req.max_contacts_per_pair = 2;
193  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false));
194  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
195  ASSERT_TRUE(res.collision);
196  EXPECT_LE(res.contacts.size(), 10u);
197  EXPECT_LE(res.contact_count, 10u);
198 }
199 
201 {
203  req.contacts = true;
204  req.max_contacts = 1;
205 
207  robot_state.setToDefaultValues();
208  robot_state.update();
209 
210  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
211  Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
212 
213  pos1.translation().x() = 5.0;
214  pos2.translation().x() = 5.01;
215 
216  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
217  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
218  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
219  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
220  robot_state.update();
221 
222  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
223 
225  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
226  ASSERT_TRUE(res.collision);
227  ASSERT_EQ(res.contacts.size(), 1u);
228  ASSERT_EQ(res.contacts.begin()->second.size(), 1u);
229 
230  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
231  it != res.contacts.end(); it++)
232  {
233  EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
234  }
235 
236  pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
237  pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
238  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
239  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
240  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
241  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
242  robot_state.update();
243 
245  crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
246  ASSERT_TRUE(res2.collision);
247  ASSERT_EQ(res2.contacts.size(), 1u);
248  ASSERT_EQ(res2.contacts.begin()->second.size(), 1u);
249 
250  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
251  it != res2.contacts.end(); it++)
252  {
253  EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
254  }
255 
256  pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
257  pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0));
258  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
259  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
260  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
261  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
262  robot_state.update();
263 
265  crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
266  ASSERT_FALSE(res3.collision);
267 }
268 
270 {
273 
274  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
275 
277  robot_state.setToDefaultValues();
278  robot_state.update();
279 
280  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
281  pos1.translation().x() = 5.0;
282 
283  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
284  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
285  robot_state.update();
286  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
287  ASSERT_FALSE(res.collision);
288 
289  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
290  cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
291 
293  cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
294  ASSERT_TRUE(res.collision);
295 
296  // deletes shape
297  cworld_->getWorld()->removeObject("box");
298 
299  shape = new shapes::Box(.1, .1, .1);
300  std::vector<shapes::ShapeConstPtr> shapes;
302  shapes.push_back(shapes::ShapeConstPtr(shape));
303  poses.push_back(Eigen::Isometry3d::Identity());
304  std::vector<std::string> touch_links;
305  robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
306 
308  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
309  ASSERT_TRUE(res.collision);
310 
311  // deletes shape
312  robot_state.clearAttachedBody("box");
313 
314  touch_links.push_back("r_gripper_palm_link");
315  touch_links.push_back("r_gripper_motor_accelerometer_link");
316  shapes[0].reset(new shapes::Box(.1, .1, .1));
317  robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
318  robot_state.update();
319 
321  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
322  ASSERT_FALSE(res.collision);
323 
324  pos1.translation().x() = 5.01;
325  shapes::Shape* coll = new shapes::Box(.1, .1, .1);
326  cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
328  cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
329  ASSERT_TRUE(res.collision);
330 
331  acm_->setEntry("coll", "r_gripper_palm_link", true);
333  cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
334  ASSERT_TRUE(res.collision);
335 }
336 
338 {
340  robot_state.setToDefaultValues();
341  robot_state.update();
342 
345 
347  *(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
348 
350  new_crobot.checkSelfCollision(req, res, robot_state);
351  double first_check = (ros::WallTime::now() - before).toSec();
352  before = ros::WallTime::now();
353  new_crobot.checkSelfCollision(req, res, robot_state);
354  double second_check = (ros::WallTime::now() - before).toSec();
355 
356  EXPECT_LT(fabs(first_check - second_check), .05);
357 
358  std::vector<shapes::ShapeConstPtr> shapes;
359  shapes.resize(1);
360 
362 
364  poses.push_back(Eigen::Isometry3d::Identity());
365 
366  std::vector<std::string> touch_links;
367  robot_state.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link");
368 
369  before = ros::WallTime::now();
370  new_crobot.checkSelfCollision(req, res, robot_state);
371  first_check = (ros::WallTime::now() - before).toSec();
372  before = ros::WallTime::now();
373  new_crobot.checkSelfCollision(req, res, robot_state);
374  second_check = (ros::WallTime::now() - before).toSec();
375 
376  // the first check is going to take a while, as data must be constructed
377  EXPECT_LT(second_check, .1);
378 
380  *(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
381  before = ros::WallTime::now();
382  new_crobot.checkSelfCollision(req, res, robot_state);
383  first_check = (ros::WallTime::now() - before).toSec();
384  before = ros::WallTime::now();
385  new_crobot.checkSelfCollision(req, res, robot_state);
386  second_check = (ros::WallTime::now() - before).toSec();
387 
388  EXPECT_LT(fabs(first_check - second_check), .05);
389 }
390 
391 TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached)
392 {
395 
397  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
398  Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
399  pos2.translation().x() = 10.0;
400 
401  cworld_->getWorld()->addToObject("kinect", shape, pos1);
402 
404  robot_state.setToDefaultValues();
405  robot_state.update();
406 
408  cworld_->checkRobotCollision(req, res, *crobot_, robot_state);
409  double first_check = (ros::WallTime::now() - before).toSec();
410  before = ros::WallTime::now();
411  cworld_->checkRobotCollision(req, res, *crobot_, robot_state);
412  double second_check = (ros::WallTime::now() - before).toSec();
413 
414  EXPECT_LT(second_check, .05);
415 
416  collision_detection::CollisionWorld::ObjectConstPtr object = cworld_->getWorld()->getObject("kinect");
417  cworld_->getWorld()->removeObject("kinect");
418 
421  robot_state1.setToDefaultValues();
422  robot_state2.setToDefaultValues();
423  robot_state1.update();
424  robot_state2.update();
425 
426  std::vector<std::string> touch_links;
427  robot_state1.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
428 
429  EigenSTL::vector_Isometry3d other_poses;
430  other_poses.push_back(pos2);
431 
432  // This creates a new set of constant properties for the attached body, which happens to be the same as the one above;
433  robot_state2.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
434 
435  // going to take a while, but that's fine
437  crobot_->checkSelfCollision(req, res, robot_state1);
438 
439  EXPECT_TRUE(res.collision);
440 
441  before = ros::WallTime::now();
442  crobot_->checkSelfCollision(req, res, robot_state1, *acm_);
443  first_check = (ros::WallTime::now() - before).toSec();
444  before = ros::WallTime::now();
445  req.verbose = true;
447  crobot_->checkSelfCollision(req, res, robot_state2, *acm_);
448  second_check = (ros::WallTime::now() - before).toSec();
449 
450  EXPECT_LT(first_check, .05);
451  EXPECT_LT(fabs(first_check - second_check), .1);
452 }
453 
454 TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed)
455 {
457  std::vector<shapes::ShapeConstPtr> shapes;
458  for (unsigned int i = 0; i < 10000; i++)
459  {
460  poses.push_back(Eigen::Isometry3d::Identity());
461  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
462  }
464  cworld_->getWorld()->addToObject("map", shapes, poses);
465  double t = (ros::WallTime::now() - start).toSec();
466  EXPECT_GE(1.0, t);
467  // this is not really a failure; it is just that slow;
468  // looking into doing collision checking with a voxel grid.
469  ROS_INFO_NAMED("collision_detection.fcl", "Adding boxes took %g", t);
470 }
471 
473 {
475  robot_state1.setToDefaultValues();
476  robot_state1.update();
477 
478  Eigen::Isometry3d kinect_pose;
479  kinect_pose.setIdentity();
480  shapes::ShapePtr kinect_shape;
482 
483  cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
484 
485  Eigen::Isometry3d np;
486  for (unsigned int i = 0; i < 5; i++)
487  {
488  np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity();
489  cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np);
492  cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_);
493  }
494 }
495 
496 TEST_F(FclCollisionDetectionTester, TestChangingShapeSize)
497 {
499  robot_state1.setToDefaultValues();
500  robot_state1.update();
501 
504 
505  ASSERT_FALSE(res1.collision);
506 
508  std::vector<shapes::ShapeConstPtr> shapes;
509  for (unsigned int i = 0; i < 5; i++)
510  {
511  cworld_->getWorld()->removeObject("shape");
512  shapes.clear();
513  poses.clear();
514  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
515  poses.push_back(Eigen::Isometry3d::Identity());
516  cworld_->getWorld()->addToObject("shape", shapes, poses);
519  cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_);
520  ASSERT_TRUE(res.collision);
521  }
522 
523  Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity();
524  shapes::ShapePtr kinect_shape;
526  cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
529  cworld_->checkCollision(req2, res2, *crobot_, robot_state1, *acm_);
530  ASSERT_TRUE(res2.collision);
531  for (unsigned int i = 0; i < 5; i++)
532  {
533  cworld_->getWorld()->removeObject("shape");
534  shapes.clear();
535  poses.clear();
536  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
537  poses.push_back(Eigen::Isometry3d::Identity());
538  cworld_->getWorld()->addToObject("shape", shapes, poses);
541  cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_);
542  ASSERT_TRUE(res.collision);
543  }
544 }
545 
546 int main(int argc, char** argv)
547 {
548  testing::InitGoogleTest(&argc, argv);
549  return RUN_ALL_TESTS();
550 }
collision_detection::CollisionRobotFCL DefaultCRobotType
int main(int argc, char **argv)
Core components of MoveIt!
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
#define ROS_INFO_NAMED(name,...)
ROSCPP_DECL void start()
TEST_F(FclCollisionDetectionTester, InitOK)
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
#define M_PI
#define EXPECT_NEAR(a, b, prec)
geometry_msgs::TransformStamped t
std::shared_ptr< Shape > ShapePtr
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
collision_detection::CollisionRobotPtr crobot_
bool collision
True if collision was found, false otherwise.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
World::ObjectConstPtr ObjectConstPtr
collision_detection::AllowedCollisionMatrixPtr acm_
#define EXPECT_EQ(a, b)
std::size_t max_contacts
Overall maximum number of contacts to compute.
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
void update(bool force=false)
Update all transforms.
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
collision_detection::CollisionWorldPtr cworld_
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Isometry3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
Definition: robot_state.h:1411
collision_detection::CollisionWorldFCL DefaultCWorldType
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
Mesh * createMeshFromResource(const std::string &resource)
static WallTime now()
robot_model::RobotModelPtr robot_model_
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
#define EXPECT_TRUE(args)
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)...
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Apr 14 2019 02:52:24