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); } }