Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix the initialization for FreeJoint::convertToTransform() #338

Merged
merged 1 commit into from Feb 23, 2015
Merged

Fix the initialization for FreeJoint::convertToTransform() #338

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 1 addition & 1 deletion dart/dynamics/FreeJoint.cpp
View file
Open in desktop
Expand Up @@ -70,7 +70,7 @@ Eigen::Vector6d FreeJoint::convertToPositions(const Eigen::Isometry3d& _tf)
Eigen::Isometry3d FreeJoint::convertToTransform(
const Eigen::Vector6d& _positions)
{
Eigen::Isometry3d tf;
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.linear() = math::expMapRot(_positions.head<3>());
tf.translation() = _positions.tail<3>();
return tf;
Expand Down
116 changes: 75 additions & 41 deletions unittests/testJoints.cpp
View file
Open in desktop
Expand Up @@ -558,8 +558,6 @@ TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)

freejoint->setTransformFromParentBodyNode(random_transform());
freejoint->setTransformFromChildBodyNode(random_transform());
Eigen::Isometry3d freejoint_tf = random_transform();
freejoint->setPositions(FreeJoint::convertToPositions(freejoint_tf));

// -- set up the EulerJoint
BodyNode* eulerjoint_bn = new BodyNode("eulerjoint_bn");
Expand All @@ -570,10 +568,6 @@ TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)

eulerjoint->setTransformFromParentBodyNode(random_transform());
eulerjoint->setTransformFromChildBodyNode(random_transform());
Eigen::Isometry3d eulerjoint_tf = random_transform();
eulerjoint_tf.translation() = Eigen::Vector3d::Zero();
eulerjoint->setPositions(
eulerjoint->convertToPositions(eulerjoint_tf.linear()));

// -- set up the BallJoint
BodyNode* balljoint_bn = new BodyNode("balljoint_bn");
Expand All @@ -584,10 +578,6 @@ TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)

balljoint->setTransformFromParentBodyNode(random_transform());
balljoint->setTransformFromChildBodyNode(random_transform());
Eigen::Isometry3d balljoint_tf = random_transform();
balljoint_tf.translation() = Eigen::Vector3d::Zero();
balljoint->setPositions(
BallJoint::convertToPositions(balljoint_tf.linear()));

// -- set up Skeleton and compute forward kinematics
Skeleton* skel = new Skeleton;
Expand All @@ -596,40 +586,84 @@ TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)
skel->addBodyNode(eulerjoint_bn);
skel->addBodyNode(balljoint_bn);
skel->init();
skel->computeForwardKinematics(true, false, false);

std::vector<Joint*> joints;
std::vector<BodyNode*> bns;
std::vector<Eigen::Isometry3d> tfs;

joints.push_back(freejoint);
bns.push_back(freejoint_bn);
tfs.push_back(freejoint_tf);

joints.push_back(eulerjoint);
bns.push_back(eulerjoint_bn);
tfs.push_back(eulerjoint_tf);

joints.push_back(balljoint);
bns.push_back(balljoint_bn);
tfs.push_back(balljoint_tf);

for(size_t i=0; i<joints.size(); ++i)
// Test a hundred times
for(size_t n=0; n<100; ++n)
{
Joint* joint = joints[i];
BodyNode* bn = bns[i];
Eigen::Isometry3d tf = tfs[i];
EXPECT_TRUE(equals(predict_joint_transform(joint, tf).matrix(),
get_relative_transform(bn, bn->getParentBodyNode()).matrix()));

if(!equals(predict_joint_transform(joint, tf).matrix(),
get_relative_transform(bn, bn->getParentBodyNode()).matrix()))
// -- convert transforms to positions and then positions back to transforms
Eigen::Isometry3d desired_freejoint_tf = random_transform();
freejoint->setPositions(FreeJoint::convertToPositions(desired_freejoint_tf));
Eigen::Isometry3d actual_freejoint_tf = FreeJoint::convertToTransform(
freejoint->getPositions());

Eigen::Isometry3d desired_eulerjoint_tf = random_transform();
desired_eulerjoint_tf.translation() = Eigen::Vector3d::Zero();
eulerjoint->setPositions(
eulerjoint->convertToPositions(desired_eulerjoint_tf.linear()));
Eigen::Isometry3d actual_eulerjoint_tf = eulerjoint->convertToTransform(
eulerjoint->getPositions());

Eigen::Isometry3d desired_balljoint_tf = random_transform();
desired_balljoint_tf.translation() = Eigen::Vector3d::Zero();
balljoint->setPositions(
BallJoint::convertToPositions(desired_balljoint_tf.linear()));
Eigen::Isometry3d actual_balljoint_tf = BallJoint::convertToTransform(
balljoint->getPositions());

skel->computeForwardKinematics(true, false, false);

// -- collect everything so we can cycle through the tests
std::vector<Joint*> joints;
std::vector<BodyNode*> bns;
std::vector<Eigen::Isometry3d> desired_tfs;
std::vector<Eigen::Isometry3d> actual_tfs;

joints.push_back(freejoint);
bns.push_back(freejoint_bn);
desired_tfs.push_back(desired_freejoint_tf);
actual_tfs.push_back(actual_freejoint_tf);

joints.push_back(eulerjoint);
bns.push_back(eulerjoint_bn);
desired_tfs.push_back(desired_eulerjoint_tf);
actual_tfs.push_back(actual_eulerjoint_tf);

joints.push_back(balljoint);
bns.push_back(balljoint_bn);
desired_tfs.push_back(desired_balljoint_tf);
actual_tfs.push_back(actual_balljoint_tf);

for(size_t i=0; i<joints.size(); ++i)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix()
<< "\n\nActual:\n"
<< get_relative_transform(bn, bn->getParentBodyNode()).matrix()
<< "\n\n";
Joint* joint = joints[i];
BodyNode* bn = bns[i];
Eigen::Isometry3d tf = desired_tfs[i];

bool check_transform_conversion =
equals(predict_joint_transform(joint, tf).matrix(),
get_relative_transform(bn, bn->getParentBodyNode()).matrix());
EXPECT_TRUE(check_transform_conversion);

if(!check_transform_conversion)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix()
<< "\n\nActual:\n"
<< get_relative_transform(bn, bn->getParentBodyNode()).matrix()
<< "\n\n";
}

bool check_full_cycle = equals(desired_tfs[i].matrix(),
actual_tfs[i].matrix());
EXPECT_TRUE(check_full_cycle);

if(!check_full_cycle)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Desired:\n" << desired_tfs[i].matrix()
<< "\n\nActual:\n" << actual_tfs[i].matrix()
<< "\n\n";
}
}
}
}
Expand Down