diff --git a/src/ur5_rtt_gazebo_component.cpp b/src/ur5_rtt_gazebo_component.cpp index cb5dce6..5e110e0 100644 --- a/src/ur5_rtt_gazebo_component.cpp +++ b/src/ur5_rtt_gazebo_component.cpp @@ -48,7 +48,7 @@ class UR5RttGazeboComponent: public RTT::TaskContext { UR5RttGazeboComponent(std::string const& name) : /* RTT::TaskContext(name), nb_static_joints(0) , jnt_it(0) , jnt_width(0) , nb_links(0), inter_torque({{0} , {0} , {0} , {0} , {0} , {0}}) , jnt_effort(0), torque_difference(0), control_value(0) , target_value(0), error_value(0),errorI(0), cumulative_error(0), error_derivative(0), last_error(0), dynStepSize(5) , pid_it(5) ,Kp({2000 , 3000 , 3000 , 540 , 540 , 1000 }) , Ki({0.4 , 0.4 , 0.4 , 0.2 , 0.2 , 0.4 }) , Kd({41850 ,41850 , 41850 , 41850 , 41850 , 37850}) , Ks({0,0,0,0,0,0}) */// HACK: The urdf has static tag for base_link, which makes it appear in gazebo as a joint. - RTT::TaskContext(name), nb_static_joints(0) , ee_mass({5, 0.00001 , 1 , 3}) , mass_id(0) , elm_id(0) , random_pos(true) , jnt_it(0) , jnt_width(0) , nb_links(0), inter_torque({{0} , {0} , {0} , {0} , {0} , {0}}) , inter_torque_elm({{0} , {0} , {0} , {0} , {0} , {0}}) ,nb_recording(0), jnt_effort(0), torque_difference(0) , torque_difference_0(0) , torque_difference_1(0), torque_difference_5(0), control_value(0) , target_value(0), error_value(0),errorI(0), cumulative_error(0), error_derivative(0), last_error(0), dynStepSize(5) , pid_it(5) ,Kp({2000 , 3000 , 3000 , 540 , 540 , 70 }) , Ki({0.4 , 0.4 , 0.4 , 0.0 , 0.0 , 0.4 }) , Kd({41850 ,41850 , 11850 , 41850 , 51850 , 2850}) , Ks({0,0,0,0,0,0}) // HACK: The urdf has static tag for base_link, which makes it appear in gazebo as a joint. + RTT::TaskContext(name), nb_static_joints(0),nearest_mass(0) ,wait(false),temp_wait(false), nb_wait(0), nb_measure_trq(20), wait_step(800), mean_trq_step(200), elm_step(200), ee_mass({5, 0.00001 , 1 , 3 , 4 , 2}) , mass_id(0) , elm_id(0) , random_pos(true) , jnt_it(0) , jnt_width(0) , nb_links(0), inter_torque({{0} , {0} , {0} , {0} , {0} , {0}}) ,add_trq(0), inter_torque_elm({{0} , {0} , {0} , {0} , {0} , {0}}) ,nb_recording(0), jnt_effort(0), torque_difference(0) , torque_difference_0(0) , torque_difference_1(0), torque_difference_3(0), torque_difference_5(0), payload_index({0.00001 , 1 , 3 , 5}), payload_error(0), control_value(0) , target_value(0), error_value(0),errorI(0), cumulative_error(0), error_derivative(0), last_error(0), dynStepSize(5) , pid_it(5) ,Kp({2000 , 3000 , 3000 , 540 , 540 , 70 }) , Ki({0.4 , 0.4 , 0.4 , 0.0 , 0.0 , 0.4 }) , Kd({41850 ,41850 , 11850 , 41850 , 51850 , 2850}) , Ks({0,0,0,0,0,0}) // HACK: The urdf has static tag for base_link, which makes it appear in gazebo as a joint. { // Add required gazebo interfaces. this->provides("gazebo")->addOperation("configure", @@ -105,16 +105,18 @@ class UR5RttGazeboComponent: public RTT::TaskContext { // ELM model creation. - std::string infile("/homes/abalayn/workspace/rtt-gazebo-ur5-integration/elmmodel_mass_0/data"); + + std::string infile("/homes/abalayn/workspace/rtt-gazebo-ur5-integration/models/elmmodel_mass_0/data"); elm_0=ExtremeLearningMachine::create(infile); RTT::log(RTT::Warning) << "Generating testdata with dimensionality: " << elm_0->getInputDimension() << RTT::endlog(); RTT::log(RTT::Warning) << "Expecting results with dimensionality: " << elm_0->getOutputDimension() << RTT::endlog(); RTT::log(RTT::Error) << "ELM loaded" << RTT::endlog(); - std::string infile("/homes/abalayn/workspace/rtt-gazebo-ur5-integration/models/elmmodel_mass_1/data"); - elm_1=ExtremeLearningMachine::create(infile); - std::string infile("/homes/abalayn/workspace/rtt-gazebo-ur5-integration/models/elmmodel_mass_5/data"); - elm_5=ExtremeLearningMachine::create(infile); - + std::string infile1("/homes/abalayn/workspace/rtt-gazebo-ur5-integration/models/elmmodel_mass_1/data"); + elm_1=ExtremeLearningMachine::create(infile1); + std::string infile5("/homes/abalayn/workspace/rtt-gazebo-ur5-integration/models/elmmodel_mass_5/data"); + elm_5=ExtremeLearningMachine::create(infile5); + std::string infile3("/homes/abalayn/workspace/rtt-gazebo-ur5-integration/models/elmmodel_mass_3/data"); + elm_3=ExtremeLearningMachine::create(infile3); // Get the joints @@ -195,12 +197,14 @@ class UR5RttGazeboComponent: public RTT::TaskContext { torque_difference.push_back(0); torque_difference_0.push_back(0); torque_difference_1.push_back(0); + torque_difference_3.push_back(0); torque_difference_5.push_back(0); jnt_it.push_back(1); jnt_width.push_back(0); thresholds.push_back(0); jnt_effort.push_back(0); nb_recording.push_back(0); + add_trq.push_back(0); } @@ -214,21 +218,21 @@ class UR5RttGazeboComponent: public RTT::TaskContext { RTT::log(RTT::Error) << "The file could not be open." << RTT::endlog(); +//For recording data + //target_value[0] = 0; + //target_value[1] = -0.1; + //target_value[2] = 3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4; + //target_value[3] = -3.14; + //target_value[4] = -1.4; + //target_value[5] = -1.57; - target_value[0] = 0; - target_value[1] = -0.1; - target_value[2] = 3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4; - target_value[3] = -3.14; - target_value[4] = -1.4; - target_value[5] = -1.57; - - - // target_value[0] = 0.0221602379990538; - // target_value[1] = -1.30987684310836; - // target_value[2] = 1.53518890257943; - // target_value[3] = -1.96273703325866; - // target_value[4] = -1.60073869056189; - // target_value[5] = 0.914073649111683; +//For testing positions + target_value[0] = 0.5927; + target_value[1] = -1.5942; + target_value[2] = 1.4871; + target_value[3] = -1.4064; + target_value[4] = -1.5318; + target_value[5] = -0.3283; jnt_width[0] = 6.28; jnt_width[1] = abs(-2.3 - -0.1); @@ -246,12 +250,20 @@ class UR5RttGazeboComponent: public RTT::TaskContext { RTT::log(RTT::Warning) << "Test trgt value 0: " << target_value[0] << RTT::endlog(); */ - thresholds[0] = 2;//31; - thresholds[1] = 8;//12; //12; - thresholds[2] = 2;//159; //30; // see if smaller could be ok. (if the position difference is not high). - thresholds[3] = 2;//30;//29; - thresholds[4] = 2;//29;//29; - thresholds[5] = 10;//40;// 40; + thresholds[0] = 1.2;//31; + thresholds[1] = 1.0;//12; //12; + thresholds[2] = 1.0;//159; //30; // see if smaller could be ok. (if the position difference is not high). + thresholds[3] = 1;//30;//29; + thresholds[4] = 1;//29;//29; + thresholds[5] = 1;//40;// 40; + + add_trq[0] = 0.05; + add_trq[1] = 0.05; + add_trq[2] = 0.02; + add_trq[3] = 0.02; + add_trq[4] = 0.02; + add_trq[5] = 0.02; + jnt_effort[0] = 150; jnt_effort[1] = 150; @@ -270,10 +282,17 @@ class UR5RttGazeboComponent: public RTT::TaskContext { jnt_it[1] = -1; jnt_it[2] = -1; + nb_wait = wait_step; + + // To test guessing of the payload. + eeMass(5, model); + //eeMass(0.00001 , model); - eeMass(0.00001 , model); - mass_id = 1; + mass_id = 4; // For data recording. + nearest_mass = 0; + + RTT::log(RTT::Warning) << "Configure hook finished." << RTT::endlog(); return true; } @@ -287,169 +306,169 @@ class UR5RttGazeboComponent: public RTT::TaskContext { /***************************************************************************************************************************************************/ - - /* - * Data recording part - */ - - if ((nb_iteration == 3410) || (nb_iteration == 3420) || (nb_iteration == 3430) || (nb_iteration == 3440) || (nb_iteration == 3450) || (nb_iteration == 3460) || (nb_iteration == 3470) || (nb_iteration == 3480) || (nb_iteration == 3490) || (nb_iteration == 3500) ) // To check if position is stable. - { - for (unsigned j = 0; j < joints_idx.size(); j++) - { - gazebo::physics::JointWrench w1 = gazebo_joints_[joints_idx[j]]->GetForceTorque(0u); - gazebo::math::Vector3 a1 = gazebo_joints_[joints_idx[j]]->GetLocalAxis(0u); - inter_torque[j].push_back(a1.Dot(w1.body1Torque)); - } - } - - - - if (nb_iteration >= 3500) // For stabilisation of the torque. - { - - - data_file << "{ sim_id = " << sim_id << " ; "; - double mean_of_torques = 0; - for (unsigned j = 0; j < joints_idx.size(); j++) - { - data_file << "jnt " << j << " ; "; - - // Computing the mean of the torques. - - mean_of_torques = (std::accumulate((inter_torque[j]).begin(),(inter_torque[j]).end(), 0.0))/10.0; - - data_file << "trq "<< mean_of_torques << " ; "; - - data_file << "agl " << model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian() << " ; "; - data_file << "trg_agl " < -(nb_recording[2]))&&(target_value[1] < 1.57)) - { - if (random_pos) - target_value[2] = jnt_it[2]*jnt_width[2]/nb_recording[2] + (jnt_width[2]/nb_recording[2]) * ((((float) rand()) / (float) RAND_MAX)) + 3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4; - else - target_value[2] = jnt_it[2]*jnt_width[2]/nb_recording[2] + 3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4; - jnt_it[2]--; - } - else if (( jnt_it[2] > -(nb_recording[2]))&&(target_value[1] > 1.57)) - { - if (random_pos) - target_value[2] = -jnt_it[2]*jnt_width[2]/nb_recording[2] - (jnt_width[2]/nb_recording[2]) * ((((float) rand()) / (float) RAND_MAX)) -3.14 + (- target_value[1] + 1.7 - acos(cos(-target_value[1]+1.7)*l1/l2)) + 0.3 +0.4; - else - target_value[2] = -jnt_it[2]*jnt_width[2]/nb_recording[2] -3.14 + (- target_value[1] + 1.7 - acos(cos(-target_value[1]+1.7)*l1/l2)) + 0.3 +0.4; - jnt_it[2]--; - } - else - { - if ((jnt_it[1] ) > -(nb_recording[1])) - { - if (random_pos) - target_value[1] = jnt_it[1]*jnt_width[1]/nb_recording[1] - (jnt_width[1]/nb_recording[1]) * ((((float) rand()) / (float) RAND_MAX)) -0.1; - else - target_value[1] = jnt_it[1]*jnt_width[1]/nb_recording[1] -0.1; - - jnt_width[2] = abs(3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4 - (3.14 - (+target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 3.14 + 0.8)); - jnt_it[1]--; - } - else - { - target_value[1] = -0.1; - jnt_it[1] = -1; - jnt_width[2] = abs(3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4 - (3.14 - (+target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 3.14 + 0.8)); - - if (jnt_it[0] >= nb_recording[0]) - { - target_value[0] = 0; - jnt_it[0] = 1; - - mass_id++; - if (mass_id >= ee_mass.size()) - { - mass_id = 0; - } - eeMass(ee_mass[mass_id] , model); - RTT::log(RTT::Error) << "Mass set to " << ee_mass[mass_id] << " kg. " << RTT::endlog(); - - } - else - { - if (random_pos) - target_value[0] = jnt_it[0]*jnt_width[0]/nb_recording[0] + (jnt_width[0]/nb_recording[0]) * ((((float) rand()) / (float) RAND_MAX)); - else - target_value[0] = jnt_it[0]*jnt_width[0]/nb_recording[0]; - jnt_it[0]++; - } - } - if (target_value[1] < 1.57) - target_value[2] = 3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4; - else - target_value[2] = -3.14 + (- target_value[1] + 1.7 - acos(cos(-target_value[1]+1.7)*l1/l2)) + 0.3 +0.4; - jnt_it[2] = -1; - } - target_value[3] = -3.14; - jnt_it[3] = 1; - } - target_value[4] = -1.4; - jnt_it[4] = 1; - } - target_value[5] = -1.57; - jnt_it[5] = 1; - } - - - - } - +// +// /* +// * Data recording part +// */ +// +// if ((nb_iteration == 3410) || (nb_iteration == 3420) || (nb_iteration == 3430) || (nb_iteration == 3440) || (nb_iteration == 3450) || (nb_iteration == 3460) || (nb_iteration == 3470) || (nb_iteration == 3480) || (nb_iteration == 3490) || (nb_iteration == 3500) ) // To check if position is stable. +// { +// for (unsigned j = 0; j < joints_idx.size(); j++) +// { +// gazebo::physics::JointWrench w1 = gazebo_joints_[joints_idx[j]]->GetForceTorque(0u); +// gazebo::math::Vector3 a1 = gazebo_joints_[joints_idx[j]]->GetLocalAxis(0u); +// inter_torque[j].push_back(a1.Dot(w1.body1Torque)); +// } +// } +// +// +// +// if (nb_iteration >= 3500) // For stabilisation of the torque. +// { +// +// +// data_file << "{ sim_id = " << sim_id << " ; "; +// double mean_of_torques = 0; +// for (unsigned j = 0; j < joints_idx.size(); j++) +// { +// data_file << "jnt " << j << " ; "; +// +// // Computing the mean of the torques. +// +// mean_of_torques = (std::accumulate((inter_torque[j]).begin(),(inter_torque[j]).end(), 0.0))/10.0; +// +// data_file << "trq "<< mean_of_torques << " ; "; +// +// data_file << "agl " << model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian() << " ; "; +// data_file << "trg_agl " < -(nb_recording[2]))&&(target_value[1] < 1.57)) +// { +// if (random_pos) +// target_value[2] = jnt_it[2]*jnt_width[2]/nb_recording[2] + (jnt_width[2]/nb_recording[2]) * ((((float) rand()) / (float) RAND_MAX)) + 3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4; +// else +// target_value[2] = jnt_it[2]*jnt_width[2]/nb_recording[2] + 3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4; +// jnt_it[2]--; +// } +// else if (( jnt_it[2] > -(nb_recording[2]))&&(target_value[1] > 1.57)) +// { +// if (random_pos) +// target_value[2] = -jnt_it[2]*jnt_width[2]/nb_recording[2] - (jnt_width[2]/nb_recording[2]) * ((((float) rand()) / (float) RAND_MAX)) -3.14 + (- target_value[1] + 1.7 - acos(cos(-target_value[1]+1.7)*l1/l2)) + 0.3 +0.4; +// else +// target_value[2] = -jnt_it[2]*jnt_width[2]/nb_recording[2] -3.14 + (- target_value[1] + 1.7 - acos(cos(-target_value[1]+1.7)*l1/l2)) + 0.3 +0.4; +// jnt_it[2]--; +// } +// else +// { +// if ((jnt_it[1] ) > -(nb_recording[1])) +// { +// if (random_pos) +// target_value[1] = jnt_it[1]*jnt_width[1]/nb_recording[1] - (jnt_width[1]/nb_recording[1]) * ((((float) rand()) / (float) RAND_MAX)) -0.1; +// else +// target_value[1] = jnt_it[1]*jnt_width[1]/nb_recording[1] -0.1; +// +// jnt_width[2] = abs(3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4 - (3.14 - (+target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 3.14 + 0.8)); +// jnt_it[1]--; +// } +// else +// { +// target_value[1] = -0.1; +// jnt_it[1] = -1; +// jnt_width[2] = abs(3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4 - (3.14 - (+target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 3.14 + 0.8)); +// +// if (jnt_it[0] >= nb_recording[0]) +// { +// target_value[0] = 0; +// jnt_it[0] = 1; +// +// mass_id++; +// if (mass_id >= ee_mass.size()) +// { +// mass_id = 0; +// } +// eeMass(ee_mass[mass_id] , model); +// RTT::log(RTT::Error) << "Mass set to " << ee_mass[mass_id] << " kg. " << RTT::endlog(); +// +// } +// else +// { +// if (random_pos) +// target_value[0] = jnt_it[0]*jnt_width[0]/nb_recording[0] + (jnt_width[0]/nb_recording[0]) * ((((float) rand()) / (float) RAND_MAX)); +// else +// target_value[0] = jnt_it[0]*jnt_width[0]/nb_recording[0]; +// jnt_it[0]++; +// } +// } +// if (target_value[1] < 1.57) +// target_value[2] = 3.14 - (+ target_value[1] + acos(sin(-target_value[1])*l1/l2) + 1.57) - 0.3 -0.4; +// else +// target_value[2] = -3.14 + (- target_value[1] + 1.7 - acos(cos(-target_value[1]+1.7)*l1/l2)) + 0.3 +0.4; +// jnt_it[2] = -1; +// } +// target_value[3] = -3.14; +// jnt_it[3] = 1; +// } +// target_value[4] = -1.4; +// jnt_it[4] = 1; +// } +// target_value[5] = -1.57; +// jnt_it[5] = 1; +// } +// +// +// +// } +// @@ -503,16 +522,16 @@ class UR5RttGazeboComponent: public RTT::TaskContext { /***************************************************************************************************************************************************/ -// /* -// * ELM part: -// * Computing the difference between current torque and awaited torque for each joint. -// */ -// -// -// + /* + * ELM part: + * Computing the difference between current torque and awaited torque for each joint. + */ + + + // elm_id++; // -// if ((elm_id == 910) || (elm_id == 920) || (elm_id == 930) || (elm_id == 940) || (elm_id == 950) || (elm_id == 960) || (elm_id == 970) || (elm_id == 980) || (elm_id == 990) || (elm_id == 1000) ) // To check if position is stable. +// if ((elm_id == (mean_trq_step-19*mean_trq_step/nb_measure_trq)) ||(elm_id == (mean_trq_step-18*mean_trq_step/nb_measure_trq)) ||(elm_id == (mean_trq_step-17*mean_trq_step/nb_measure_trq)) ||(elm_id == (mean_trq_step-16*mean_trq_step/nb_measure_trq)) ||(elm_id == (mean_trq_step-15*mean_trq_step/nb_measure_trq)) ||(elm_id == (mean_trq_step-14*mean_trq_step/nb_measure_trq)) ||(elm_id == (mean_trq_step-13*mean_trq_step/nb_measure_trq)) ||(elm_id == (mean_trq_step-12*mean_trq_step/nb_measure_trq)) ||(elm_id == (mean_trq_step-11*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-10*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-9*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-8*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-7*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-6*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-5*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-4*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-3*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-2*mean_trq_step/nb_measure_trq)) || (elm_id == (mean_trq_step-mean_trq_step/nb_measure_trq)) || (elm_id == mean_trq_step) ) // To check if position is stable. // { // for (unsigned j = 0; j < joints_idx.size(); j++) // { @@ -521,183 +540,273 @@ class UR5RttGazeboComponent: public RTT::TaskContext { // inter_torque_elm[j].push_back(a1.Dot(w1.body1Torque)); // } // } -// /* -// // Create vector containing awaited position. -// RealVectorPtr inputdata = RealVector::create(elm_0->getInputDimension(), 0.0); -// for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()); -// -// // Create vector containing the torque which should be applied. -// RealVectorPtr result = elm_0->evaluate(inputdata); -// //RTT::log(RTT::Warning) << "Evaluating [" << inputdata << "] -> [" << result << "]" << RTT::endlog(); -// -// error_file << "{" ; -// -// for (unsigned j = 0; j < joints_idx.size(); j++) -// { -// // Compute current torque. -// gazebo::physics::JointWrench w1 = gazebo_joints_[joints_idx[j]]->GetForceTorque(0u); -// gazebo::math::Vector3 a1 = gazebo_joints_[joints_idx[j]]->GetLocalAxis(0u); -// -// torque_difference[j] = result->getValue(j) - (a1.Dot(w1.body1Torque)); -// //RTT::log(RTT::Warning) << "Torque difference: " << torque_difference[j] << RTT::endlog(); -// error_file << "joint " << j << ": " << torque_difference[j] << " dsrTrq: " << result->getValue(j) << " realTrq: " << (a1.Dot(w1.body1Torque)) << " ;"; -// } -// error_file << "}" << std::endl; -// */ -// -// /***************************************************************************************************************************************************/ -// -// -// -// /***************************************************************************************************************************************************/ -// /* -// * Compliancy of the robot. -// */ -// if ((sim_id > 3500)&&(elm_id >= 1000)) -// { + //if (elm_id == 1000) + //{ + // elm_id =0; + // // Create vector containing awaited position. + // RealVectorPtr inputdata = RealVector::create(elm_0->getInputDimension(), 0.0); + // for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()); + // + // // Create vector containing the torque which should be applied. + // RealVectorPtr result = elm_0->evaluate(inputdata); + // //RTT::log(RTT::Warning) << "Evaluating [" << inputdata << "] -> [" << result << "]" << RTT::endlog(); + // + // error_file << "{" ; + // + // for (unsigned j = 0; j < joints_idx.size(); j++) + // { + // double mean_of_torques_elm = 0; + // mean_of_torques_elm = (std::accumulate((inter_torque_elm[j]).begin(),(inter_torque_elm[j]).end(), 0))/10.0; + // + // + // torque_difference[j] = result->getValue(j) - mean_of_torques_elm; + // //RTT::log(RTT::Warning) << "Torque difference: " << torque_difference[j] << RTT::endlog(); + // error_file << "joint " << j << ": " << torque_difference[j] << " dsrTrq: " << result->getValue(j) << " realTrq: " << mean_of_torques_elm << " ;"; + // mean_of_torques_elm = 0; + // inter_torque_elm[j] = {0}; + // } + // error_file << "}" << std::endl; + //} + + + /***************************************************************************************************************************************************/ + + + + /***************************************************************************************************************************************************/ + /* + * Compliance of the robot. + */ + + + + + + + + + + //if ((sim_id % 2000)&&(elm_id >= 1000)&&(sim_id>14000)) +//if (elm_id == mean_trq_step) +//{ // elm_id = 0; // // // // -// // ******************************************************************************************************************************** -// // * ELM part: -// // * Computing the difference between current torque and awaited torque for each joint. -// // Create vector containing awaited position. -// -// RealVectorPtr inputdata = RealVector::create(elm_0->getInputDimension(), 0.0); -// for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()); +// // ******************************************************************************************************************************** +// // * ELM part: +// // * Computing the difference between current torque and awaited torque for each joint. +// // Create vector containing awaited position. // -// // Create vector containing the torque which should be applied. -// RealVectorPtr result = elm_0->evaluate(inputdata); +//if (nearest_mass == 0) +//{ +// RealVectorPtr inputdata = RealVector::create(elm_0->getInputDimension(), 0.0); +// for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,target_value[j]/*model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()*/); // -// error_file << "{" ; +// // Create vector containing the torque which should be applied. +// RealVectorPtr result = elm_0->evaluate(inputdata); +//} +//else if (nearest_mass == 1) +//{ +// RealVectorPtr inputdata = RealVector::create(elm_1->getInputDimension(), 0.0); +// for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,target_value[j]/*model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()*/); // -// for (unsigned j = 0; j < joints_idx.size(); j++) -// { +// // Create vector containing the torque which should be applied. +// RealVectorPtr result = elm_1->evaluate(inputdata); +//} +//else if (nearest_mass == 3) +//{ +// RealVectorPtr inputdata = RealVector::create(elm_3->getInputDimension(), 0.0); +// for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,target_value[j]/*model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()*/); // +// // Create vector containing the torque which should be applied. +// RealVectorPtr result = elm_3->evaluate(inputdata); +//} +//else if (nearest_mass == 5) +//{ +// RealVectorPtr inputdata = RealVector::create(elm_5->getInputDimension(), 0.0); +// for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,target_value[j]/*model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()*/); // -// double mean_of_torques_elm = 0; -// mean_of_torques_elm = (std::accumulate((inter_torque_elm[j]).begin(),(inter_torque_elm[j]).end(), 0))/10.0; +// // Create vector containing the torque which should be applied. +// RealVectorPtr result = elm_5->evaluate(inputdata); +//} // // -// torque_difference[j] = result->getValue(j) - mean_of_torques_elm; -// //RTT::log(RTT::Warning) << "Torque difference: " << torque_difference[j] << RTT::endlog(); -// error_file << "joint " << j << ": " << torque_difference[j] << " dsrTrq: " << result->getValue(j) << " realTrq: " << mean_of_torques_elm << " ;"; // -// // ******************************************************************************************************************************** // +// error_file << "{" ; // +// for (unsigned j = 0; j < joints_idx.size(); j++) +// { // // +// double mean_of_torques_elm = 0; +// mean_of_torques_elm = (std::accumulate((inter_torque_elm[j]).begin(),(inter_torque_elm[j]).end(), 0.0))/nb_measure_trq; // -// // ******************************************************************************************************************************** -// // * Compliance part: // +// torque_difference[j] = result->getValue(j) - mean_of_torques_elm; +// //RTT::log(RTT::Warning) << "Torque difference: " << torque_difference[j] << RTT::endlog(); +// error_file << "joint " << j << ": " << torque_difference[j] << " dsrTrq: " << result->getValue(j) << " realTrq: " << mean_of_torques_elm << " ;"; // -// if (abs(torque_difference[j]) > thresholds[j]) -// { -// // gazebo::physics::JointWrench w1 = gazebo_joints_[joints_idx[j]]->GetForceTorque(0u); -// // gazebo::math::Vector3 a1 = gazebo_joints_[joints_idx[j]]->GetLocalAxis(0u); -// // error_file << "{" ; -// // error_file << "joint " << j << ": " << torque_difference[j] << " dsrTrq: " << result->getValue(j) << " realTrq: " << (a1.Dot(w1.body1Torque)) << " ;"; -// // error_file << "tgtPos" << target_value[j]; -// // Test: the new target value is the current position. To be evaluated because there is no deformation compared to soft robots! - // target_value[j] = model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian(); -// // error_file << "newPos" << target_value[j]; -// // error_file << "}" << std::endl; -// -// - // Test: the new target value: we add an angle: has to be set for each joint> -// if (torque_difference[j] > 0) -// target_value[j] = target_value[j] + 0.2; -// else -// target_value[j] = target_value[j] - 0.2; +// // ******************************************************************************************************************************** // -// RTT::log(RTT::Warning) << "Joint " << j << "set to " << target_value[j] << RTT::endlog(); -// } -// inter_torque_elm[j] = {0}; -// mean_of_torques_elm = 0; // -// } -// error_file << "}" << std::endl; // -// } - - /***************************************************************************************************************************************************/ - - - - /***************************************************************************************************************************************************/ - -// /* -// * Try to guess which payload is currently at the end-effector. -// */ // // +// // ******************************************************************************************************************************** +// // * Compliance part: only every T seconds - to be decided. // -// if ((sim_id == 3410) || (sim_id == 3420) || (sim_id == 3430) || (sim_id == 3440) || (sim_id == 3450) || (sim_id == 3460) || (sim_id == 3470) || (sim_id == 3480) || (sim_id == 3490) || (sim_id == 3500) ) // To check if position is stable. +// if ((sim_id >= 10000)&&(sim_id%elm_step == 0)) +// { +// if (!wait) +// { +// if (abs(torque_difference[j]) > thresholds[j]) // { -// for (unsigned j = 0; j < joints_idx.size(); j++) +// // gazebo::physics::JointWrench w1 = gazebo_joints_[joints_idx[j]]->GetForceTorque(0u); +// // gazebo::math::Vector3 a1 = gazebo_joints_[joints_idx[j]]->GetLocalAxis(0u); +// // error_file << "{" ; +// // error_file << "joint " << j << ": " << torque_difference[j] << " dsrTrq: " << result->getValue(j) << " realTrq: " << (a1.Dot(w1.body1Torque)) << " ;"; +// // error_file << "tgtPos" << target_value[j]; +// // Test: the new target value is the current position. To be evaluated because there is no deformation compared to soft robots! +// // target_value[j] = model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian(); +// // error_file << "newPos" << target_value[j]; +// // error_file << "}" << std::endl; +// +// +// // Test: the new target value: we add an angle: has to be set for each joint> +// if (torque_difference[j] > 0) +// { +// target_value[j] = target_value[j] + add_trq[j]; +// } +// else // { -// gazebo::physics::JointWrench w1 = gazebo_joints_[joints_idx[j]]->GetForceTorque(0u); -// gazebo::math::Vector3 a1 = gazebo_joints_[joints_idx[j]]->GetLocalAxis(0u); -// inter_torque[j].push_back(a1.Dot(w1.body1Torque)); +// target_value[j] = target_value[j] - add_trq[j]; // } +// error_file << "joint " << j << "modified!"; +// temp_wait = true; +// RTT::log(RTT::Warning) << "Joint " << j << "set to " << target_value[j] << RTT::endlog(); // } -// if (sim_id == 3500) -// { -// RealVectorPtr inputdata = RealVector::create(elm_0->getInputDimension(), 0.0); -// for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()); -// // Create vector containing the torque which should be applied. -// RealVectorPtr result_0 = elm_0->evaluate(inputdata); -// RealVectorPtr result_1 = elm_1->evaluate(inputdata); -// RealVectorPtr result_5 = elm_5->evaluate(inputdata); +// } // -// for (unsigned j = 0; j < joints_idx.size(); j++) -// { -// double mean_of_torques = 0; -// mean_of_torques= (std::accumulate((inter_torque[j]).begin(),(inter_torque[j]).end(), 0))/10.0; -// torque_difference_0[j] = result_0->getValue(j) - mean_of_torques; -// torque_difference_1[j] = result_1->getValue(j) - mean_of_torques; -// torque_difference_5[j] = result_5->getValue(j) - mean_of_torques; // -// mean_of_torques = 0; -// inter_torque[j] = {0}; -// } -// double error_0 = std::inner_product( torque_difference_0.begin(), torque_difference_0.end(), torque_difference_0.begin(), 0.0 ); -// double error_1 = std::inner_product( torque_difference_1.begin(), torque_difference_1.end(), torque_difference_1.begin(), 0.0 ); -// double error_5 = std::inner_product( torque_difference_5.begin(), torque_difference_5.end(), torque_difference_5.begin(), 0.0 ); // -// if (std::min(error_0 , error_1) == error_0) -// { -// if (std::min(error_0 , error_2) == error_0) -// { -// RTT::log(RTT::Warning) << "Payload is the nearest to " << 0 << RTT::endlog(); -// } -// else -// { -// RTT::log(RTT::Warning) << "Payload is the nearest to " << 2 << RTT::endlog(); -// } -// } -// else -// { -// if (std::min(error_1 , error_2) == error_1) -// { -// RTT::log(RTT::Warning) << "Payload is the nearest to " << 1 << RTT::endlog(); -// } -// else -// { -// RTT::log(RTT::Warning) << "Payload is the nearest to " << 2 << RTT::endlog(); -// } -// } // } +// inter_torque_elm[j] = {0}; +// mean_of_torques_elm = 0; +// } +// if (temp_wait) +// { +// wait = true; +// temp_wait = false; +// } +// error_file << "}" << std::endl; // +//} // +//if (wait) +//{ +// nb_wait = nb_wait-1; +// if (nb_wait<=0) +// { +// wait = false; +// nb_wait = wait_step; +// } +//} // + /***************************************************************************************************************************************************/ + + + + /***************************************************************************************************************************************************/ + + /* + * Try to guess which payload is currently at the end-effector. + */ + + + + if ((sim_id == 3410) || (sim_id == 3420) || (sim_id == 3430) || (sim_id == 3440) || (sim_id == 3450) || (sim_id == 3460) || (sim_id == 3470) || (sim_id == 3480) || (sim_id == 3490) || (sim_id == 3500) ) // To check if position is stable. + { + for (unsigned j = 0; j < joints_idx.size(); j++) + { + gazebo::physics::JointWrench w1 = gazebo_joints_[joints_idx[j]]->GetForceTorque(0u); + gazebo::math::Vector3 a1 = gazebo_joints_[joints_idx[j]]->GetLocalAxis(0u); + inter_torque[j].push_back(a1.Dot(w1.body1Torque)); + } + } + + if (sim_id == 3500) + { + RealVectorPtr inputdata = RealVector::create(elm_0->getInputDimension(), 0.0); + for (int j=0; jgetDimension(); j++) inputdata->setValueEquals(j,model->GetJoints()[joints_idx[j]]->GetAngle(0).Radian()); + // Create vector containing the torque which should be applied. + RealVectorPtr result_0 = elm_0->evaluate(inputdata); + RealVectorPtr result_1 = elm_1->evaluate(inputdata); + RealVectorPtr result_3 = elm_3->evaluate(inputdata); + RealVectorPtr result_5 = elm_5->evaluate(inputdata); + + + for (unsigned j = 0; j < joints_idx.size(); j++) + { + double mean_of_torques = 0; + mean_of_torques= (std::accumulate((inter_torque[j]).begin(),(inter_torque[j]).end(), 0))/10.0; + torque_difference_0[j] = result_0->getValue(j) - mean_of_torques; + torque_difference_1[j] = result_1->getValue(j) - mean_of_torques; + torque_difference_3[j] = result_3->getValue(j) - mean_of_torques; + torque_difference_5[j] = result_5->getValue(j) - mean_of_torques; + + mean_of_torques = 0; + inter_torque[j] = {0}; + } + double error_0 = std::inner_product( torque_difference_0.begin(), torque_difference_0.end(), torque_difference_0.begin(), 0.0 ); + double error_1 = std::inner_product( torque_difference_1.begin(), torque_difference_1.end(), torque_difference_1.begin(), 0.0 ); + double error_3 = std::inner_product( torque_difference_3.begin(), torque_difference_3.end(), torque_difference_3.begin(), 0.0 ); + double error_5 = std::inner_product( torque_difference_5.begin(), torque_difference_5.end(), torque_difference_5.begin(), 0.0 ); + + payload_error.push_back(error_0); + payload_error.push_back(error_1); + payload_error.push_back(error_3); + payload_error.push_back(error_5); + + + // Find index of min value of the vector + std::size_t min_mass_idx = std::distance(payload_error.begin(), std::min_element(payload_error.begin(), payload_error.end())); + RTT::log(RTT::Warning) << "Payload is the nearest to " << payload_index[min_mass_idx] << RTT::endlog(); + + /* + if (std::min(error_0 , error_1) == error_0) + { + if (std::min(error_0 , error_5) == error_0) + { + RTT::log(RTT::Warning) << "Payload is the nearest to " << 0 << RTT::endlog(); + } + else + { + RTT::log(RTT::Warning) << "Payload is the nearest to " << 5 << RTT::endlog(); + } + } + else + { + if (std::min(error_1 , error_5) == error_1) + { + RTT::log(RTT::Warning) << "Payload is the nearest to " << 1 << RTT::endlog(); + } + else + { + RTT::log(RTT::Warning) << "Payload is the nearest to " << 5 << RTT::endlog(); + } + } + */ + } + + + + @@ -745,6 +854,7 @@ class UR5RttGazeboComponent: public RTT::TaskContext { // RTT::log(RTT::Error) << j << " " << error_value[j] << " " << control_value[j] << " " << errorI[j] << RTT::endlog() ; } + error_file << "cmd 1 " << control_value[1] << std::endl ; // For tuning PID. @@ -769,8 +879,18 @@ class UR5RttGazeboComponent: public RTT::TaskContext { /* if (sim_id == 8000) { - RTT::log(RTT::Error) << "entering mass modification " << RTT::endlog() ; eeMass(5.0 , model); + RTT::log(RTT::Error) << "Mass set to 5 " << RTT::endlog() ; + } + if (sim_id == 12000) + { + RTT::log(RTT::Error) << "Mass set to 1 " << RTT::endlog() ; + eeMass(1.0 , model); + } + if (sim_id == 16000) + { + RTT::log(RTT::Error) << "Mass set to 0 " << RTT::endlog() ; + eeMass(0.001 , model); } */ @@ -802,6 +922,7 @@ class UR5RttGazeboComponent: public RTT::TaskContext { std::vector nb_recording; std::vector ee_mass; int mass_id; + int nearest_mass; // To indicate the model used for compliance mode. int nb_iteration; // number of hook iterations for one tested position. int sim_id; // number of angle positions tested. @@ -855,20 +976,31 @@ class UR5RttGazeboComponent: public RTT::TaskContext { ExtremeLearningMachinePtr elm_0; ExtremeLearningMachinePtr elm_1; + ExtremeLearningMachinePtr elm_3; ExtremeLearningMachinePtr elm_5; std::vector torque_difference; std::vector torque_difference_0; std::vector torque_difference_1; + std::vector torque_difference_3; std::vector torque_difference_5; + std::vector payload_error; // To compare the differences between current torque and awaited torque or different payloads. + std::vector payload_index; // contains payloads in correct order to find current payload. // Variable to save intermediate robot position - to decide if the data will be written in the file. std::vector< std::vector > inter_torque_elm; - // Compliancy + // Compliance std::vector thresholds; std::vector jnt_effort; - + std::vector add_trq; // Torque which is added when difference overshoots the threshold. + bool wait; + bool temp_wait; // Used because all joints must be updated before waiting. + int nb_wait; + int wait_step; + int elm_step; + int mean_trq_step; + int nb_measure_trq; };