void iDynTree_print_velocity_acceleration(DynTree & icub_idyntree, const std::string link_name)
{
std::cout <<"Velocity of the " << link_name << endl;
cout << icub_idyntree.getVel(icub_idyntree.getLinkIndex((link_name))).toString() << endl;
cout <<"Acceleration of " << link_name << endl;
cout << icub_idyntree.getAcc(icub_idyntree.getLinkIndex(link_name)).toString() << endl;
}
void set_random_q_dq_ddq(yarp::os::Random & rng, DynTree & 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 = 0.0;
vel_c = 0.0;
acc_c = 0.0;
double world_orient = 1.0;
double world_pos = 1.0;
double base_vel_c = 1.0;
double base_acc_c = 1.0;
yAssert(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;
yAssert(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;
}
void set_random_IMU_q_dq_ddq(yarp::os::Random & rng, DynTree & icub_tree)
{
double pos_c = 0.0,vel_c = 0.0,acc_c =0.0;
pos_c = 2.0;
vel_c = 1.0;
acc_c = 4.0;
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 imu_ang_vel(3), imu_ang_acc(3), imu_lin_acc(3);
set_random_vector(imu_ang_vel,rng,vel_c);
set_random_vector(imu_ang_acc,rng,acc_c);
set_random_vector(imu_lin_acc,rng,acc_c);
icub_tree.setInertialMeasure(imu_ang_vel,imu_ang_acc,imu_lin_acc);
return;
}
