void setRandomState(iDynTree::HighLevel::DynamicsComputations & dynComp,
                    iCub::iDynTree::DynTree & dynTree)
{
    size_t dofs = dynComp.getNrOfDegreesOfFreedom();
    Transform    worldTbase;
    Twist        baseVel;
    ClassicalAcc baseAcc;
    SpatialAcc gravity;
    Vector6    properAcc;

    iDynTree::VectorDynSize qj(dofs), dqj(dofs), ddqj(dofs);

    worldTbase = iDynTree::Transform(Rotation::RPY(actual_random_double(),random_double(),random_double()),
                                     Position(random_double(),random_double(),random_double()));
    for(int i=0; i < 3; i++)
    {
        gravity(i) = random_double();
    }

    gravity(2) = 10.0;

    for(int i=0; i < 6; i++)
    {
        baseVel(i) = random_double();
        baseAcc(i) = random_double();
        properAcc(i) = baseAcc(i) - gravity(i);
    }

    for(size_t dof=0; dof < dofs; dof++)

    {
        qj(dof) = random_double();
        dqj(dof) = random_double();
        ddqj(dof) = random_double();
    }

    bool ok = dynComp.setRobotState(qj,dqj,ddqj,worldTbase,baseVel,baseAcc,gravity);

    dynTree.setAng(idyntree2yarp(qj));
    dynTree.setDAng(idyntree2yarp(dqj));
    dynTree.setD2Ang(idyntree2yarp(ddqj));
    dynTree.setWorldBasePose(idyntreeMat2yarp(worldTbase.asHomogeneousTransform()));
    dynTree.setKinematicBaseVelAcc(idyntree2yarp(baseVel),idyntree2yarp(properAcc));

    ASSERT_EQUAL_DOUBLE(ok,true);
}
예제 #2
0
void setRandomState(iDynTree::HighLevel::DynamicsComputations & dynComp,
                    iCub::iDynTree::DynTree & dynTree,
                    iDynTree::KinDynComputations& kinDynComp,
                    iDynTree::Vector6& baseAccKinDyn,
                    iDynTree::JointDOFsDoubleArray& jointAccKinDyn)
{
   std::cerr << " setRandomState" << std::endl;
    
    size_t dofs = dynComp.getNrOfDegreesOfFreedom();
    Transform    worldTbase;
    Twist        baseVel;
    ClassicalAcc baseAcc;
    SpatialAcc gravity;
    Vector6    properAcc;

    iDynTree::VectorDynSize qj(dofs), dqj(dofs), ddqj(dofs);

    worldTbase = iDynTree::Transform(Rotation::RPY(random_double(),random_double(),random_double()),
                                     Position(random_double(),random_double(),random_double()));
    for(int i=0; i < 3; i++)
    {
        gravity(i) = random_double();
    }

    gravity(2) = 10.0;

    for(int i=0; i < 6; i++)
    {
        baseVel(i) = random_double();
        baseAcc(i) = random_double();
        properAcc(i) = baseAcc(i) - gravity(i);
    }

    jointAccKinDyn.resize(dofs);
    for(size_t dof=0; dof < dofs; dof++)

    {
        qj(dof) = random_double();
        dqj(dof) = random_double();
        ddqj(dof) = random_double();
    }

    bool ok = dynComp.setRobotState(qj,dqj,ddqj,worldTbase,baseVel,baseAcc,gravity);
    
    ASSERT_IS_TRUE(ok);

    dynTree.setAng(idyntree2yarp(qj));
    dynTree.setDAng(idyntree2yarp(dqj));
    dynTree.setD2Ang(idyntree2yarp(ddqj));
    dynTree.setWorldBasePose(idyntreeMat2yarp(worldTbase.asHomogeneousTransform()));
    dynTree.setKinematicBaseVelAcc(idyntree2yarp(baseVel),idyntree2yarp(properAcc));

    std::cerr << " Setting state in kinDyn" << std::endl;
    ASSERT_EQUAL_DOUBLE(qj.size(),kinDynComp.getRobotModel().getNrOfPosCoords());
    ASSERT_EQUAL_DOUBLE(dqj.size(),kinDynComp.getRobotModel().getNrOfDOFs());

    Vector3 grav3d = gravity.getLinearVec3();
    ok = kinDynComp.setRobotState(worldTbase,qj,baseVel,dqj,grav3d);
    toEigen(jointAccKinDyn) = toEigen(ddqj);
    toEigen(baseAccKinDyn)  = toEigen(baseAcc);
    ASSERT_IS_TRUE(ok);
    
    std::cerr << "state setted" << std::endl;
}