void testJacobianConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree,
iDynTree::KinDynComputations & kinDynComp)
{
size_t dofs = dynComp.getNrOfDegreesOfFreedom();
MatrixDynSize dynTreeJacobian(6,6+dofs),dynCompJacobian(6,6+dofs);
yarp::sig::Matrix dynTreeJacobianYarp(6,6+dofs);
FrameFreeFloatingJacobian kinDynCompJacobian(kinDynComp.getRobotModel());
for(size_t frame=0; frame < dynComp.getNrOfFrames(); frame++ )
{
dynTree.getJacobian(frame,dynTreeJacobianYarp);
yarp2idyntree(dynTreeJacobianYarp,dynTreeJacobian);
dynComp.getFrameJacobian(frame,dynCompJacobian);
std::string frameName = dynComp.getFrameName(frame);
ASSERT_EQUAL_MATRIX(dynTreeJacobian,dynCompJacobian);
std::cerr << "Testing frame " << frameName << std::endl;
bool ok = kinDynComp.getFrameFreeFloatingJacobian(frameName,kinDynCompJacobian);
std::cerr << "DynamicsComputations: " << std::endl;
std::cerr << dynCompJacobian.toString() << std::endl;
std::cerr << "KinamicsDynamicsComputations : " << std::endl;
std::cerr << kinDynCompJacobian.toString() << std::endl;
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_MATRIX(kinDynCompJacobian,dynCompJacobian);
}
}
void testTransformsConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
iCub::iDynTree::DynTree & dynTree)
{
for(size_t frame=0; frame < dynComp.getNrOfFrames(); frame++ )
{
std::string frameNameDynComp;
std::string frameNameDynTree;
dynTree.getFrameName(frame,frameNameDynTree);
frameNameDynComp = dynComp.getFrameName(frame);
ASSERT_EQUAL_STRING(frameNameDynComp,frameNameDynTree);
// todo assert equal string
Transform dynTreeTransform = yarpTransform2idyntree(dynTree.getPosition(frame));
Transform dynCompTransform = dynComp.getWorldTransform(frame);
ASSERT_EQUAL_TRANSFORM(dynCompTransform,dynTreeTransform);
}
}