From 07cf9be0ab3a3cee18d3a124f2714a24552b2c7b Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Sun, 14 Feb 2016 16:49:41 +0100
Subject: [PATCH] Device stores the joint list of each objects
---
include/hpp/manipulation/device.hh | 5 +++--
include/hpp/manipulation/graph/helper.hh | 1 -
src/device.cc | 4 +++-
src/graph/helper.cc | 5 +++--
4 files changed, 9 insertions(+), 6 deletions(-)
diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
index fec0b6ac..6dc1156e 100644
--- a/include/hpp/manipulation/device.hh
+++ b/include/hpp/manipulation/device.hh
@@ -37,7 +37,8 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI Device : public model::HumanoidRobot,
public core::Container <HandlePtr_t>,
public core::Container <model::GripperPtr_t>,
- public core::Container <JointAndShapes_t>
+ public core::Container <JointAndShapes_t>,
+ public core::Container <model::JointVector_t>
{
public:
typedef model::HumanoidRobot Parent_t;
@@ -112,7 +113,7 @@ namespace hpp {
/// add the current Robot.
/// When creating a robot from several URDF files, this enables
/// collisions between joints from different files.
- void didInsertRobot ();
+ void didInsertRobot (const std::string& name);
/// \}
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index eadb1317..267dc6bc 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -171,7 +171,6 @@ namespace hpp {
struct ObjectDef_t {
std::string name;
- JointPtr_t lastJoint;
StringList_t handles, shapes;
};
diff --git a/src/device.cc b/src/device.cc
index 392d89f3..c8f320fe 100644
--- a/src/device.cc
+++ b/src/device.cc
@@ -25,7 +25,7 @@
namespace hpp {
namespace manipulation {
- void Device::didInsertRobot ()
+ void Device::didInsertRobot (const std::string& name)
{
if (!didPrepare_) {
hppDout (error, "You must call prepareInsertRobot before.");
@@ -34,6 +34,7 @@ namespace hpp {
/// Build list of new joints
const model::JointVector_t jv = getJointVector ();
model::JointVector_t newj;
+ newj.reserve (jv.size () - jointCache_.size ());
model::JointVector_t::const_iterator retFind, it1, it2;
for (it1 = jv.begin (); it1 != jv.end (); ++it1) {
retFind = find (jointCache_.begin (), jointCache_.end (), *it1);
@@ -50,6 +51,7 @@ namespace hpp {
}
}
jointCache_.clear ();
+ add (name, newj);
}
std::ostream& Device::print (std::ostream& os) const
{
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index c25f8a94..10eb08b6 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -871,8 +871,9 @@ namespace hpp {
}
}
// Create object lock
- for (JointPtr_t oj = od.lastJoint;
- oj != NULL; oj = oj->parentJoint ()) {
+ using model::JointVector_t;
+ assert (robot.has <JointVector_t> (od.name));
+ BOOST_FOREACH (const JointPtr_t& oj, robot.get<JointVector_t> (od.name)) {
LockedJointPtr_t lj = core::LockedJoint::create (oj,
robot.currentConfiguration()
.segment (oj->rankInConfiguration (), oj->configSize ()));
--
GitLab