Commit c2378767 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Fix bug in Frame

parent 0f43c88f
......@@ -79,16 +79,22 @@ namespace hpp {
return pinocchio().name;
}
const Transform3f& Frame::currentTransformation () const
Transform3f Frame::currentTransformation () const
{
selfAssert();
return data().oMf[frameIndex_];
const Data & d = data();
const se3::Frame f = model().frames[frameIndex_];
if (f.type == se3::JOINT)
return d.oMi[f.parent];
else
return d.oMi[f.parent] * f.placement;
}
void Frame::setChildList()
{
assert(devicePtr_->modelPtr()); assert(devicePtr_->dataPtr());
children_.clear();
if (!isFixed()) return;
const Model& model = devicePtr_->model();
std::vector<bool> visited (model.frames.size(), false);
......@@ -104,11 +110,8 @@ namespace hpp {
for (FrameIndex i = model.frames.size() - 1; i > 0; --i) {
if (visited[i]) continue;
visited[i] = true;
if ( model.frames[i].type != se3::FIXED_JOINT
&& model.frames[i].type != se3::JOINT)
continue;
k = model.frames[i].previousFrame;
while (model.frames[k].type == se3::FIXED_JOINT) {
while (model.frames[k].type != se3::JOINT) {
if (k == frameIndex_ || k == 0) break;
visited[k] = true;
k = model.frames[k].previousFrame;
......@@ -135,8 +138,10 @@ namespace hpp {
Transform3f Frame::positionInParentFrame () const
{
selfAssert();
return model().frames[pinocchio().previousFrame].placement.inverse()
* model().frames[index() ].placement;
const Model& m = model();
const se3::Frame f = m.frames[index()];
return m.frames[f.previousFrame].placement.inverse()
* ((f.type == se3::FIXED_JOINT) ? f.placement : m.jointPlacements[f.parent]);
}
void Frame::positionInParentFrame (const Transform3f& p)
......@@ -145,8 +150,12 @@ namespace hpp {
devicePtr_->invalidate();
Model& model = devicePtr_->model();
se3::Frame& me = pinocchio();
Transform3f fMj = me.placement.inverse();
me.placement = model.frames[me.previousFrame].placement * p;
bool isJoint = (me.type == se3::JOINT);
Transform3f fMj = (isJoint ? model.jointPlacements[me.parent].inverse() : me.placement.inverse());
if (isJoint)
model.jointPlacements[me.parent] = model.frames[me.previousFrame].placement * p;
else
me.placement = model.frames[me.previousFrame].placement * p;
std::vector<bool> visited (model.frames.size(), false);
for (std::size_t i = 0; i < children_.size(); ++i) {
......
......@@ -20,6 +20,8 @@
#include <hpp/model/device.hh>
#include <pinocchio/algorithm/frames.hpp>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/frame.hh>
......@@ -27,35 +29,87 @@
static bool verbose = false;
#define CHECK_TRANSFORM(M, Mexp) BOOST_CHECK((M.inverse() * Mexp).isIdentity())
using namespace hpp::pinocchio;
typedef std::vector<std::string> Strings_t;
void check_children(const Model& model, const Frame& f, Strings_t expected_children)
{
const std::vector<FrameIndex>& children = f.children();
BOOST_CHECK(children.size() == expected_children.size());
for (std::size_t i = 0; i < children.size(); ++i) {
std::string name = model.frames[children[i]].name;
Strings_t::iterator _found = std::find(
expected_children.begin(), expected_children.end(), name);
BOOST_CHECK_MESSAGE(_found != expected_children.end(), "Child frame " << name << " not found");
if (_found != expected_children.end())
expected_children.erase(_found);
}
for (std::size_t i = 0; i < expected_children.size(); ++i) {
BOOST_CHECK_MESSAGE(false, "Frame " << expected_children[i] << " not found");
}
}
BOOST_AUTO_TEST_CASE (frame)
{
DevicePtr_t pinocchio = hppPinocchio();
Configuration_t q = pinocchio->neutralConfiguration();
pinocchio->currentConfiguration(q);
pinocchio->computeForwardKinematics();
Frame root = pinocchio->getFrameByName("root_joint");
Frame waist = pinocchio->getFrameByName("waist");
BOOST_CHECK(!root.isFixed());
BOOST_CHECK(waist.isFixed());
BOOST_CHECK(waist.parentFrame().name() == "root_joint");
BOOST_CHECK(!waist.parentFrame().isFixed());
std::vector<std::string> expected_children;
expected_children.push_back("ImuTorsoAccelerometer_joint");
expected_children.push_back("ImuTorsoGyrometer_joint");
Strings_t expected_children;
expected_children.push_back("ImuTorsoAccelerometer_frame");
expected_children.push_back("ImuTorsoGyrometer_frame");
expected_children.push_back("TrunkYaw");
expected_children.push_back("LHipYaw");
expected_children.push_back("RHipYaw");
expected_children.push_back("body");
const std::vector<FrameIndex>& children = waist.children();
BOOST_CHECK(children.size() == expected_children.size());
for (std::size_t i = 0; i < children.size(); ++i) {
std::string name = pinocchio->model().frames[children[i]].name;
std::vector<std::string>::iterator _found = std::find(
expected_children.begin(), expected_children.end(), name);
BOOST_CHECK_MESSAGE(_found != expected_children.end(), "Child frame " << name << " not found");
if (_found != expected_children.end())
expected_children.erase(_found);
}
for (std::size_t i = 0; i < expected_children.size(); ++i) {
BOOST_CHECK_MESSAGE(false, "Frame " << expected_children[i] << " not found");
}
// Check that the children are properly set.
check_children(pinocchio->model(), waist, expected_children);
check_children(pinocchio->model(), root , Strings_t());
// Check position in parent frame.
Joint root_j = waist.joint();
BOOST_CHECK(root_j.name() == "root_joint");
Frame
ImuTorsoAcc_F = pinocchio->getFrameByName("ImuTorsoAccelerometer_joint"),
ImuTorsoGyr_F = pinocchio->getFrameByName("ImuTorsoGyrometer_joint"),
TrunkYaw_F = pinocchio->getFrameByName("TrunkYaw"),
LHipYaw_F = pinocchio->getFrameByName("LHipYaw"),
RHipYaw_F = pinocchio->getFrameByName("RHipYaw");
Transform3f
ImuTorsoAcc_M = ImuTorsoAcc_F.positionInParentFrame(),
ImuTorsoGyr_M = ImuTorsoGyr_F.positionInParentFrame(),
TrunkYaw_M = TrunkYaw_F .positionInParentFrame(),
LHipYaw_M = LHipYaw_F .positionInParentFrame(),
RHipYaw_M = RHipYaw_F .positionInParentFrame(),
waist_M = waist .positionInParentFrame();
Transform3f shift = Transform3f::Random();
waist.positionInParentFrame(shift);
pinocchio->computeForwardKinematics();
CHECK_TRANSFORM(ImuTorsoAcc_F.positionInParentFrame(), ImuTorsoAcc_M);
CHECK_TRANSFORM(ImuTorsoGyr_F.positionInParentFrame(), ImuTorsoGyr_M);
CHECK_TRANSFORM(TrunkYaw_F .positionInParentFrame(), TrunkYaw_M );
CHECK_TRANSFORM(LHipYaw_F .positionInParentFrame(), LHipYaw_M );
CHECK_TRANSFORM(RHipYaw_F .positionInParentFrame(), RHipYaw_M );
CHECK_TRANSFORM(waist .positionInParentFrame(), shift );
CHECK_TRANSFORM(ImuTorsoAcc_F.currentTransformation(), shift * ImuTorsoAcc_M);
CHECK_TRANSFORM(ImuTorsoGyr_F.currentTransformation(), shift * ImuTorsoGyr_M);
CHECK_TRANSFORM(TrunkYaw_F .currentTransformation(), shift * TrunkYaw_M );
CHECK_TRANSFORM(LHipYaw_F .currentTransformation(), shift * LHipYaw_M );
CHECK_TRANSFORM(RHipYaw_F .currentTransformation(), shift * RHipYaw_M );
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment