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