Exemple #1
0
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 testJacobianConsistency(iDynTree::HighLevel::DynamicsComputations & dynComp,
                             iCub::iDynTree::DynTree & dynTree)
{
    size_t dofs = dynComp.getNrOfDegreesOfFreedom();
    MatrixDynSize dynTreeJacobian(6,6+dofs),dynCompJacobian(6,6+dofs);
    yarp::sig::Matrix        dynTreeJacobianYarp(6,6+dofs);

    for(size_t frame=0; frame < dynComp.getNrOfFrames(); frame++ )
    {
        dynTree.getJacobian(frame,dynTreeJacobianYarp);
        yarp2idyntree(dynTreeJacobianYarp,dynTreeJacobian);

        dynComp.getFrameJacobian(frame,dynCompJacobian);

        ASSERT_EQUAL_MATRIX(dynTreeJacobian,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);
    }
}