From 216c211d480e7e531f7a42ec085bf4f4d7059689 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sun, 18 Sep 2016 10:51:25 +0200
Subject: [PATCH] Fix construction of constraint graph.
Device now derives from Container <FrameIndexes_t> and stores vectors of
frame indices instead of joint indices. Update construction of graph
accordingly.
---
include/hpp/manipulation/fwd.hh | 1 +
src/graph/helper.cc | 26 ++++++++++++++++++--------
2 files changed, 19 insertions(+), 8 deletions(-)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index eaf85b5..67976d0 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -32,6 +32,7 @@ namespace hpp {
typedef pinocchio::JointPtr_t JointPtr_t;
typedef pinocchio::JointIndex JointIndex;
typedef std::vector<JointIndex> JointIndexes_t;
+ typedef se3::FrameIndex FrameIndex;
typedef std::vector<se3::FrameIndex> FrameIndexes_t;
typedef pinocchio::Configuration_t Configuration_t;
typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 0b4d83b..2ac0da9 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -27,6 +27,7 @@
#include <hpp/util/debug.hh>
#include <hpp/pinocchio/gripper.hh>
+#include <pinocchio/multibody/model.hpp>
#include <hpp/constraints/differentiable-function.hh>
@@ -1035,14 +1036,23 @@ namespace hpp {
}
}
// Create object lock
- assert (robot.has <JointIndexes_t> (od.name));
- BOOST_FOREACH (const JointIndex& j, robot.get<JointIndexes_t> (od.name)) {
- JointPtr_t oj (new Joint (ps->robot(), j));
- LockedJointPtr_t lj = core::LockedJoint::create (oj,
- robot.currentConfiguration()
- .segment (oj->rankInConfiguration (), oj->configSize ()));
- ps->ProblemSolver::ThisC_t::add <LockedJointPtr_t> ("lock_" + oj->name (), lj);
- objects[i].get<0> ().get<2> ().push_back (lj);
+ // Loop over all frames of object, detect joint and create locked
+ // joint.
+ assert (robot.has <FrameIndexes_t> (od.name));
+ BOOST_FOREACH (const FrameIndex& j, robot.get<FrameIndexes_t> (od.name)) {
+ const se3::Frame& frame (robot.model ().frames [j]);
+ if (frame.type == se3::JOINT) {
+ JointIndex jointId (robot.model ().getJointId (frame.name));
+ hppDout (info, "frame " << j << " with name " << od.name
+ << " is joint with id " << jointId);
+ JointPtr_t oj (new Joint (ps->robot(), jointId));
+ LockedJointPtr_t lj = core::LockedJoint::create
+ (oj, robot.currentConfiguration().segment
+ (oj->rankInConfiguration (), oj->configSize ()));
+ ps->ProblemSolver::ThisC_t::add <LockedJointPtr_t>
+ ("lock_" + oj->name (), lj);
+ objects[i].get<0> ().get<2> ().push_back (lj);
+ }
}
++i;
}
--
GitLab