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