From bfd77d62f0f64ca9a4d97cdfc45d177a687ef352 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 29 Mar 2019 11:45:04 +0100
Subject: [PATCH] Remove Device container of frame indices.
---
include/hpp/manipulation/device.hh | 17 ++-------
src/device.cc | 56 ++++++++++++++++++++----------
src/graph/helper.cc | 2 +-
3 files changed, 41 insertions(+), 34 deletions(-)
diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
index 8834f25b..3dd67fab 100644
--- a/include/hpp/manipulation/device.hh
+++ b/include/hpp/manipulation/device.hh
@@ -58,22 +58,13 @@ namespace hpp {
virtual pinocchio::DevicePtr_t clone () const;
- /// \name Collisions
- /// \{
+ std::vector<std::string> robotNames () const;
- /// 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 (const std::string& name);
-
- /// \}
+ FrameIndices_t robotFrames (const std::string& robotName) const;
core::Container <HandlePtr_t> handles;
core::Container <GripperPtr_t> grippers;
core::Container <JointAndShapes_t> jointAndShapes;
- core::Container <FrameIndices_t> frameIndices;
protected:
/// Constructor
@@ -81,7 +72,7 @@ namespace hpp {
/// \param robot Robots that manipulate objects,
/// \param objects Set of objects manipulated by the robot.
Device (const std::string& name) :
- Parent_t (name), frameCacheSize_ (1)
+ Parent_t (name)
{}
void init (const DeviceWkPtr_t& self)
@@ -98,8 +89,6 @@ namespace hpp {
private:
DeviceWkPtr_t self_;
-
- std::size_t frameCacheSize_;
}; // class Device
} // namespace manipulation
} // namespace hpp
diff --git a/src/device.cc b/src/device.cc
index 680a5eac..36b8a011 100644
--- a/src/device.cc
+++ b/src/device.cc
@@ -23,6 +23,7 @@
#include <pinocchio/multibody/geometry.hpp>
#include <hpp/pinocchio/joint.hh>
+#include <hpp/pinocchio/joint-collection.hh>
#include <hpp/pinocchio/gripper.hh>
#include <hpp/manipulation/handle.hh>
@@ -42,7 +43,10 @@ namespace hpp {
void Device::setRobotRootPosition (const std::string& rn,
const Transform3f& t)
{
- const FrameIndices_t& idxs = frameIndices.get (rn);
+ FrameIndices_t idxs = robotFrames (rn);
+ if (idxs.size() == 0)
+ throw std::invalid_argument ("No frame for robot name " + rn);
+
pinocchio::Model& m = model();
pinocchio::GeomModel& gm = geomModel();
// The root frame should be the first frame.
@@ -77,28 +81,42 @@ namespace hpp {
invalidate();
}
- void Device::didInsertRobot (const std::string& name)
+ std::vector<std::string> Device::robotNames () const
{
- /// Build list of new joints
- std::size_t fvSize = model().frames.size();
- assert (fvSize >= frameCacheSize_);
- FrameIndices_t newF (fvSize - frameCacheSize_);
- for (std::size_t i = frameCacheSize_; i < fvSize; ++i) {
- assert (
- (model().frames[i].name.compare(0, name.size(), name) == 0)
- && (model().frames[i].name[name.size()] == '/')
- && "Frames have been reordered.");
- newF[i - frameCacheSize_] = i;
+ const pinocchio::Model& model = this->model();
+ std::vector<std::string> names;
+
+ for (pinocchio::FrameIndex fi = 1; fi < model.frames.size(); ++fi)
+ {
+ const Frame& frame = model.frames[fi];
+ std::size_t sep = frame.name.find ('/');
+ if (sep == std::string::npos) {
+ hppDout (warning, "Frame " << frame.name << " does not belong to any robots.");
+ continue;
+ }
+ std::string name = frame.name.substr(0,sep);
+
+ if (std::find(names.rbegin(), names.rend(), name) != names.rend())
+ names.push_back (name);
}
+ return names;
+ }
+
+ FrameIndices_t Device::robotFrames (const std::string& robotName) const
+ {
+ const pinocchio::Model& model = this->model();
+ FrameIndices_t frameIndices;
- frameCacheSize_ = model().frames.size();
- if (frameIndices.has(name)) {
- const FrameIndices_t& old = frameIndices.get(name);
- newF.insert(newF.begin(), old.begin(), old.end());
+ for (pinocchio::FrameIndex fi = 1; fi < model.frames.size(); ++fi)
+ {
+ const std::string& name = model.frames[fi].name;
+ if ( name.size() > robotName.size()
+ && name.compare (0, robotName.size(), robotName) == 0
+ && name[robotName.size()] == '/') {
+ frameIndices.push_back (fi);
+ }
}
- frameIndices.add (name, newF);
- createData();
- createGeomData();
+ return frameIndices;
}
std::ostream& Device::print (std::ostream& os) const
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 35291767..a9132c9b 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -1103,7 +1103,7 @@ namespace hpp {
// Loop over all frames of object, detect joint and create locked
// joint.
assert (robot.frameIndices.has (od.name));
- BOOST_FOREACH (const FrameIndex& f, robot.frameIndices.get (od.name)) {
+ BOOST_FOREACH (const FrameIndex& f, robot.robotFrames (od.name)) {
if (model.frames[f].type != ::pinocchio::JOINT) continue;
const JointIndex j = model.frames[f].parent;
JointPtr_t oj (Joint::create (ps->robot(), j));
--
GitLab