diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh
index e284d4788600b06f864622bb50b3394ab413f987..ac91cfdabfc4ec7d81ceef9a43f8bddde4f5ce07 100644
--- a/include/hpp/manipulation/device.hh
+++ b/include/hpp/manipulation/device.hh
@@ -41,7 +41,7 @@ namespace hpp {
boost::mpl::vector < HandlePtr_t,
GripperPtr_t,
JointAndShapes_t,
- JointIndexes_t> >
+ FrameIndexes_t> >
{
public:
typedef pinocchio::HumanoidRobot Parent_t;
@@ -50,7 +50,7 @@ namespace hpp {
boost::mpl::vector < HandlePtr_t,
pinocchio::GripperPtr_t,
JointAndShapes_t,
- JointIndexes_t> > Containers_t;
+ FrameIndexes_t> > Containers_t;
/// Constructor
/// \param name of the new instance,
@@ -65,6 +65,9 @@ namespace hpp {
/// Print object in a stream
virtual std::ostream& print (std::ostream& os) const;
+ void setRobotRootPosition (const std::string& robotName,
+ const Transform3f& positionWRTParentJoint);
+
/// \name Collisions
/// \{
@@ -83,7 +86,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), jointCacheSize_ (1)
+ Parent_t (name), frameCacheSize_ (1)
{}
void init (const DeviceWkPtr_t& self)
@@ -95,7 +98,7 @@ namespace hpp {
private:
DeviceWkPtr_t self_;
- std::size_t jointCacheSize_;
+ std::size_t frameCacheSize_;
}; // class Device
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 0fa89ecf0d787aa2ce8028a9fdab607e66bc9248..eaf85b5ea68674cb23419acb29ba6a7649c1299d 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 std::vector<se3::FrameIndex> FrameIndexes_t;
typedef pinocchio::Configuration_t Configuration_t;
typedef pinocchio::ConfigurationIn_t ConfigurationIn_t;
typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
diff --git a/src/device.cc b/src/device.cc
index 1c4fbd7d5dbecd7dae23e58283c796b28d6c0dd1..75e9c7202652cb30b11d029f1ba9c48d887ace9c 100644
--- a/src/device.cc
+++ b/src/device.cc
@@ -20,6 +20,7 @@
#include <hpp/manipulation/device.hh>
#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/multibody/geometry.hpp>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/gripper.hh>
@@ -28,16 +29,63 @@
namespace hpp {
namespace manipulation {
+ using se3::Frame;
+ using se3::FrameIndex;
+
+ void Device::setRobotRootPosition (const std::string& rn,
+ const Transform3f& t)
+ {
+ FrameIndexes_t idxs = get<JointIndexes_t> (rn);
+ pinocchio::Model& m = model();
+ pinocchio::GeomModel& gm = geomModel();
+ // The root frame should be the first frame.
+ Frame& rootFrame = m.frames[idxs[0]];
+ if (rootFrame.type == se3::JOINT) {
+ JointIndex jid = m.getJointId (rootFrame.name);
+ m.jointPlacements[jid] = t;
+ return;
+ }
+
+ Transform3f shift (t * rootFrame.placement.inverse());
+ // Find all the frames that have the same parent joint.
+ for (std::size_t i = 1; i < idxs.size(); ++i) {
+ Frame& frame = m.frames[idxs[i]];
+ if (frame.parent == rootFrame.parent) {
+ // frame is between rootFrame and next moving joints.
+ frame.placement = shift * frame.placement;
+ if (frame.type == se3::BODY) {
+ // Update the geometry object placement.
+ for (std::size_t k = 0; k < gm.geometryObjects.size(); ++k)
+ {
+ se3::GeometryObject& go = gm.geometryObjects[k];
+ if (go.parentFrame == idxs[i])
+ go.placement = shift * go.placement;
+ }
+ }
+ } else if ((frame.type == se3::JOINT) && (rootFrame.parent == m.parents[frame.parent])) {
+ // frame corresponds to a child joint of rootFrame.parent
+ m.jointPlacements[frame.parent] = shift * m.jointPlacements[frame.parent];
+ }
+ }
+ invalidate();
+ }
+
void Device::didInsertRobot (const std::string& name)
{
/// Build list of new joints
- std::size_t jvSize = model().joints.size();
- assert (jvSize >= jointCacheSize_);
- JointIndexes_t newJ (jvSize - jointCacheSize_);
- for (std::size_t i = jointCacheSize_; i < jvSize; ++i) newJ[i - jointCacheSize_] = i;
+ std::size_t fvSize = model().frames.size();
+ assert (fvSize >= frameCacheSize_);
+ FrameIndexes_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;
+ }
- jointCacheSize_ = model().joints.size();
- add (name, newJ);
+ frameCacheSize_ = model().frames.size();
+ add (name, newF);
}
std::ostream& Device::print (std::ostream& os) const
diff --git a/tests/path-projection.cc b/tests/path-projection.cc
index 103bdec70702f225acd162b3397d9071f3306bb3..ee77980cf197807590c57b3dfd682f49a0dcee8f 100644
--- a/tests/path-projection.cc
+++ b/tests/path-projection.cc
@@ -101,8 +101,10 @@ namespace hpp_test {
TV::Random() + TV::Constant(1),
CV::Random() - CV::Constant(1),
CV::Random() + CV::Constant(1));
+ model.addJointFrame (idx);
- model.appendBodyToJoint(idx,se3::Inertia::Random(),se3::SE3::Identity(),name + "_BODY");
+ model.appendBodyToJoint(idx,se3::Inertia::Random(),se3::SE3::Identity());
+ model.addBodyFrame(name + "_BODY", idx);
}
DevicePtr_t createRobot ()
@@ -113,7 +115,7 @@ namespace hpp_test {
// lleg
addJointAndBody(model,se3::JointModelRX(),vector3_t::Zero(), 0, "ARM");
addJointAndBody(model,se3::JointModelRX(),vector3_t(0, ARM_LENGTH, 0), 1, "FOREARM");
- model.addFrame(se3::Frame("EE", 2, se3::SE3::Identity(), se3::FIXED_JOINT));
+ model.addFrame(se3::Frame("EE", 2, model.getFrameId("FOREARM"), se3::SE3::Identity(), se3::FIXED_JOINT));
robot->createData();
robot->controlComputation((Device::Computation_t) (Device::JOINT_POSITION | Device::JACOBIAN));