From 2cc6e9844f2b455425e7d36ded0d4819407e24b6 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 24 Feb 2015 11:38:20 +0100
Subject: [PATCH] Fix bug: add collisions between robots from different URDF.
---
include/hpp/manipulation/device.hh | 29 +++++++++++++++-
src/CMakeLists.txt | 1 +
src/device.cc | 53 ++++++++++++++++++++++++++++++
3 files changed, 82 insertions(+), 1 deletion(-)
create mode 100644 src/device.cc
diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
index 1f3febf..c956855 100644
--- a/include/hpp/manipulation/device.hh
+++ b/include/hpp/manipulation/device.hh
@@ -63,6 +63,9 @@ namespace hpp {
return os;
}
+ /// \name Accessors to container elements
+ /// \{
+
/// Get an element of a container
template <typename Element>
const Element& get (const std::string& name) const
@@ -84,13 +87,34 @@ namespace hpp {
Container <Element>::add (name, element);
}
+ /// \}
+
+ /// \name Collisions
+ /// \{
+
+ /// Cache joint vector
+ void prepareInsertRobot ()
+ {
+ didPrepare_ = true;
+ jointCache_ = getJointVector ();
+ }
+
+ /// Add collisions
+ /// between the joint vector cache initialized by prepareInsertRobot()
+ /// add the current Robot.
+ /// When creating a robot from several URDF files, this enables
+ /// collisions between joints from different files.
+ void didInsertRobot ();
+
+ /// \}
+
protected:
/// Constructor
/// \param name of the new instance,
/// \param robot Robots that manipulate objects,
/// \param objects Set of objects manipulated by the robot.
Device (const std::string& name) :
- Parent_t (name)
+ Parent_t (name), jointCache_ (), didPrepare_ (false)
{}
void init (const DeviceWkPtr_t& self)
@@ -101,6 +125,9 @@ namespace hpp {
private:
DeviceWkPtr_t self_;
+
+ model::JointVector_t jointCache_;
+ bool didPrepare_;
}; // class Device
} // namespace manipulation
} // namespace hpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 22cb69d..9d0bd2e 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -25,6 +25,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
manipulation-planner.cc
problem-solver.cc
roadmap.cc
+ device.cc
graph-path-validation.cc
graph-steering-method.cc
diff --git a/src/device.cc b/src/device.cc
new file mode 100644
index 0000000..9d73727
--- /dev/null
+++ b/src/device.cc
@@ -0,0 +1,53 @@
+///
+/// Copyright (c) 2015 CNRS
+/// Authors: Joseph Mirabel
+///
+///
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include <hpp/manipulation/device.hh>
+
+#include <hpp/model/joint.hh>
+
+namespace hpp {
+ namespace manipulation {
+ void Device::didInsertRobot ()
+ {
+ if (!didPrepare_) {
+ hppDout (error, "You must call prepareInsertRobot before.");
+ }
+ didPrepare_ = false;
+ /// Build list of new joints
+ const model::JointVector_t jv = getJointVector ();
+ model::JointVector_t newj;
+ model::JointVector_t::const_iterator retFind, it1, it2;
+ for (it1 = jv.begin (); it1 != jv.end (); ++it1) {
+ retFind = find (jointCache_.begin (), jointCache_.end (), *it1);
+ if (retFind == jointCache_.end ())
+ newj.push_back (*it1);
+ }
+ /// Add collision between old joints and new ones.
+ for (it1 = newj.begin (); it1 != newj.end (); ++it1) {
+ if (!(*it1)->linkedBody ()) continue;
+ for (it2 = jointCache_.begin (); it2 != jointCache_.end (); ++it2) {
+ if (!(*it2)->linkedBody ()) continue;
+ addCollisionPairs (*it1, *it2, model::COLLISION);
+ addCollisionPairs (*it1, *it2, model::DISTANCE);
+ }
+ }
+ jointCache_.clear ();
+ }
+ } // namespace manipulation
+} // namespace hpp
--
GitLab