Exemple #1
0
void set_random_vector(yarp::sig::Vector & vec, yarp::os::Random & rng, double coeff=1.0)
{
    for( int i=0; i < (int)vec.size(); i++ ) {
        vec[i] =  coeff*M_PI*rng.uniform();
    }
}
Exemple #2
0
void set_random_q_dq_ddq(yarp::os::Random & rng, iCubTree & icub_tree)
{
    double pos_c = 0.0,vel_c = 0.0,acc_c =0.0;

    yarp::sig::Matrix H_w2b(4,4);
    H_w2b.eye();

    pos_c = 1.0;
    vel_c = 1.0;
    acc_c = 1.0;

    double world_orient = 1.0;
    double world_pos = 1.0;

    double base_vel_c = 1.0;
    double base_acc_c = 1.0;

    KDL::Frame H_w2b_kdl;

    H_w2b_kdl.M = KDL::Rotation::EulerZYX(world_orient*rng.uniform(),world_orient*rng.uniform(),world_orient*rng.uniform());

    H_w2b_kdl.p[0] = world_pos*M_PI*rng.uniform();
    H_w2b_kdl.p[1] = world_pos*M_PI*rng.uniform();
    H_w2b_kdl.p[2] = world_pos*M_PI*rng.uniform();


    H_w2b_kdl.Make4x4(H_w2b.data());

    YARP_ASSERT(icub_tree.setWorldBasePose(H_w2b));

    std::cout << "iDynTree Jacobian test world pose " << icub_tree.getWorldBasePose().toString() << std::endl;


    Vector q(icub_tree.getNrOfDOFs());
    set_random_vector(q,rng,pos_c);
    icub_tree.setAng(q);

    Vector dq(icub_tree.getNrOfDOFs());
    set_random_vector(dq,rng,vel_c);
    //dq[1] = 1000.0;
    icub_tree.setDAng(dq);

    Vector ddq(icub_tree.getNrOfDOFs());
    set_random_vector(ddq,rng,acc_c);
    icub_tree.setD2Ang(ddq);

    Vector base_vel(6,0.0), base_acc(6,0.0);

    set_random_vector(base_vel,rng,base_vel_c);
    set_random_vector(base_acc,rng,base_acc_c);

    base_acc[3] = base_acc[4] = base_acc[5] = 0.0;

    icub_tree.setKinematicBaseVelAcc(base_vel,base_acc);

    //std::cout << "iDynTreeJacobianTest: Setting base_acc " << base_acc.toString() << " ( " << norm(base_acc) << " ) " <<  std::endl;

    YARP_ASSERT(icub_tree.kinematicRNEA());

    //std::cout << "iDynTreeJacobianTest: Acc at the base " << icub_tree.getAcc(icub_tree.getLinkIndex("root_link")).toString() << " ( " << norm(icub_tree.getAcc(icub_tree.getLinkIndex("root_link"))) << " ) " << std::endl;


    return;
}