Commit 25f48b04 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

BUG FIX: clone method for joint does not initialize initial transform

parent 8b735321
......@@ -53,10 +53,12 @@ namespace hpp {
Joint::Joint (const Joint& joint) :
configuration_ (joint.configuration_),
currentTransformation_ (joint.initialPosition_),
positionInParentFrame_ (joint.positionInParentFrame_),
linkInJointFrame_ (joint.linkInJointFrame_),
maximalDistanceToParent_ (joint.maximalDistanceToParent_),
configSize_ (joint.configSize_), numberDof_ (joint.numberDof_),
initialPosition_(joint.initialPosition_),
robot_ (), body_ (joint.body_ ? joint.body_->clone (this) : 0x0),
name_ (joint.name_), linkName_ (joint.linkName_),
children_ (), parent_ (), rankInConfiguration_ (-1), rankInVelocity_ (-1),
......
......@@ -77,6 +77,19 @@ BOOST_AUTO_TEST_CASE(clone_device)
d1->computeForwardKinematics();
d2->computeForwardKinematics();
// now verifying that transforms are correctly updated as well
fcl::Vec3f x(0,0,1);
fcl::Transform3f r1, r2;
for(std::size_t i =0; i != jv1.size(); ++i)
{
r1 = jv1[i]->currentTransformation();
r2 = jv2[i]->currentTransformation();
BOOST_CHECK_MESSAGE((r1.getRotation() * x + r1.getTranslation() - r2.getRotation() * x + r2.getTranslation()).norm() < 10e-2,
"Transformation not matching for joint " +jv1[i]->name()+ " and " +jv2[i]->name() );
}
//collision checking with objects
fcl::CollisionObjectPtr_t c1 = d1->rootJoint()->linkedBody()->innerObjects(hpp::model::COLLISION).front()->fcl();
fcl::CollisionObjectPtr_t c2 = d2->rootJoint()->linkedBody()->innerObjects(hpp::model::COLLISION).front()->fcl();
......@@ -96,4 +109,5 @@ BOOST_AUTO_TEST_CASE(clone_device)
fcl::collide(c1.get(),c2.get(),colReq2,colRes2);
BOOST_CHECK_MESSAGE ( !colRes2.isCollision(),
"Device robot and clone should not be colliding after collision");
}
Markdown is supported
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