Skip to content

Commit

Permalink
comparison to real robot
Browse files Browse the repository at this point in the history
  • Loading branch information
@agathe-balayn
agathe-balayn committed Jul 29, 2016
1 parent d79233d commit c9cd1a1
Show file tree
Hide file tree
Showing 10 changed files with 101 additions and 72 deletions.
100 changes: 100 additions & 0 deletions angles.csv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
0.50001,-0.32004,-1.6863e-05,-3.14,-1.57,1.684e-05
0.50001,-0.13399,-1.6047e-05,-3.14,-1.57,1.9084e-05
0.50001,-1.186,1.6537e-05,-3.14,-1.57,3.6375e-06
0.50001,-0.38404,-1.6968e-05,-3.14,-1.57,1.9028e-05
0.50001,-0.5672,-1.6593e-05,-3.14,-1.57,1.9032e-05
0.50001,-0.072851,-1.6531e-05,-3.14,-1.57,1.9025e-05
0.50001,-1.4345,1.6571e-05,-3.14,-1.57,3.0293e-06
0.50001,-1.3133,5.4883e-06,-3.14,-1.57,-1.3036e-05
0.50001,-1.1273,-1.5402e-05,-3.14,-1.57,-1.2316e-05
0.50001,-1.2502,-8.2283e-06,-3.1399,-1.57,-1.2734e-05
0.50001,-1.56,1.6917e-05,-3.14,-1.57,-1.2739e-05
0.50002,-0.69435,-1.6067e-05,-3.14,-1.57,2.1983e-06
0.50001,-1.3729,2.034e-05,-3.1399,-1.57,-7.3648e-06
0.50001,-1.4986,6.8868e-06,-3.14,-1.57,-2.7011e-06
0.50002,-0.19796,-7.3197e-06,-3.14,-1.57,1.8838e-05
0.50002,-0.81431,1.3337e-05,-3.14,-1.57,1.2932e-05
0.50002,-1.0636,1.9393e-07,-3.1399,-1.57,3.1158e-06
0.50002,-0.50755,-2.0174e-05,-3.14,-1.57,1.8967e-05
0.50002,-1.0008,-4.2102e-06,-3.14,-1.57,3.6881e-06
0.50002,-0.75503,-2.0705e-05,-3.14,-1.57,2.66e-06
0.50002,-0.25927,-5.2724e-06,-3.14,-1.57,9.9748e-06
0.50002,-0.443,-2.0739e-05,-3.14,-1.57,1.2505e-05
0.50002,-0.93862,-1.6822e-05,-3.14,-1.57,3.0166e-06
0.50002,-0.87852,-2.0675e-05,-3.14,-1.57,3.0135e-06
0.50002,-0.6309,-2.0822e-05,-3.14,-1.57,3.0133e-06
0.5,-1.5697,2.5147,-3.141,-1.57,1.1679e-05
0.5,-1.57,0.6419,-3.14,-1.57,-2.0449e-05
0.50002,-1.5699,0.017079,-3.1401,-1.57,-2.4547e-07
0.50002,-1.57,1.2621,-3.1401,-1.57,1.1145e-05
0.50002,-1.5699,1.576,-3.1401,-1.57,1.0639e-05
0.50002,-1.5699,1.4734,-3.1401,-1.57,-8.2105e-06
0.50002,-1.5699,1.1617,-3.1401,-1.57,-2.0224e-05
0.50002,-1.57,1.8874,-3.1401,-1.57,-4.1603e-06
0.50001,-1.57,0.22567,-3.1401,-1.57,-2.0168e-05
0.50001,-1.57,0.84626,-3.1401,-1.57,-1.7461e-05
0.50001,-1.57,2.4078,-3.1402,-1.57,1.1857e-05
0.50001,-1.5699,0.53779,-3.1401,-1.57,-1.298e-05
0.50001,-1.57,2.0954,-3.1401,-1.57,1.1861e-05
0.50001,-1.57,0.95484,-3.1401,-1.57,-1.6916e-05
0.50002,-1.57,1.6792,-3.1401,-1.57,4.2397e-06
0.50002,-1.57,0.74647,-3.1401,-1.57,-2.0166e-05
0.50002,-1.57,1.9911,-3.1401,-1.57,1.1955e-05
0.50002,-1.5699,0.43409,-3.1401,-1.57,-2.025e-05
0.50002,-1.57,1.3665,-3.1401,-1.57,1.3949e-06
0.50001,-1.57,0.33006,-3.1401,-1.57,-2.032e-05
0.50001,-1.57,1.7828,-3.1401,-1.57,1.2581e-05
0.50002,-1.57,2.2004,-3.1401,-1.57,1.188e-05
0.50002,-1.57,2.3056,-3.1401,-1.57,1.1854e-05
0.50002,-1.5699,0.12086,-3.1401,-1.57,-2.0164e-05
0.50002,-1.5699,1.0541,-3.1401,-1.57,-4.4177e-06
0.49996,-1.5699,1.5678,1.6978,-1.5695,0.0012993
0.5,-1.57,1.5694,3.1839,-1.5697,0.00039583
0.50001,-1.57,1.5694,-0.041662,-1.5699,0.00053238
0.50001,-1.57,1.5694,-1.5294,-1.5697,0.00025744
0.50001,-1.57,1.5694,2.1935,-1.5699,0.00020722
0.50001,-1.57,1.5694,4.1778,-1.5697,0.00022876
0.50001,-1.57,1.5694,0.70291,-1.5698,0.00036805
0.50001,-1.57,1.5694,-1.033,-1.57,0.00036758
0.50001,-1.57,1.5694,0.45218,-1.57,0.00033554
0.50001,-1.57,1.5694,2.6882,-1.5698,0.00035164
0.50001,-1.57,1.5694,3.6805,-1.5698,0.00036759
0.50001,-1.57,1.5694,3.4356,-1.5698,0.00036758
0.50001,-1.57,1.5694,1.2012,-1.5698,0.00033554
0.50001,-1.57,1.5694,-1.2824,-1.5698,0.00036761
0.50001,-1.57,1.5695,2.4419,-1.5698,0.00033555
0.50001,-1.57,1.5694,2.9363,-1.5698,0.00033755
0.50001,-1.57,1.5694,4.4254,-1.5698,0.00036796
0.50001,-1.57,1.5694,1.9459,-1.5698,0.00033572
0.50001,-1.57,1.5694,3.9295,-1.5698,0.00036758
0.50001,-1.57,1.5694,0.95191,-1.5698,0.00033556
0.50001,-1.57,1.5694,-0.78475,-1.5698,0.00036763
0.50001,-1.57,1.5694,1.4466,-1.5698,0.00033549
0.50001,-1.57,1.5694,-0.53679,-1.5698,0.00036759
0.50001,-1.57,1.5694,-0.29114,-1.5698,0.00036746
0.50001,-1.57,1.5694,0.2048,-1.5698,0.00033791
0.50002,-1.57,1.57,4.71,0.20304,1.0851e-05
0.50002,-1.57,1.57,4.71,3.4286,1.0555e-05
0.50002,-1.57,1.57,4.71,4.1714,1.0612e-05
0.50002,-1.57,1.57,4.7098,1.1943,-2.1529e-05
0.50002,-1.57,1.57,4.71,4.4219,1.055e-05
0.50002,-1.57,1.57,4.7098,-0.049258,-5.6798e-06
0.50002,-1.57,1.57,4.7101,0.943,-2.1775e-05
0.50002,-1.57,1.57,4.71,2.433,1.0615e-05
0.50002,-1.57,1.57,4.71,-1.2898,1.0678e-05
0.50002,-1.57,1.57,4.7101,-1.5368,1.0489e-05
0.50002,-1.57,1.57,4.7101,1.4416,-1.5216e-05
0.50002,-1.57,1.57,4.71,0.45128,-2.1474e-05
0.50002,-1.57,1.57,4.71,1.9363,-5.3429e-06
0.50002,-1.57,1.57,4.7099,-0.54333,1.0611e-05
0.50002,-1.57,1.57,4.71,2.1862,1.0544e-05
0.50002,-1.57,1.57,4.7101,0.69964,-5.4651e-06
0.50002,-1.57,1.57,4.71,-1.039,1.0607e-05
0.50002,-1.57,1.57,4.7099,2.9328,1.0561e-05
0.50002,-1.57,1.57,4.71,2.6846,2.0995e-06
0.50002,-1.57,1.57,4.71,3.1789,1.0247e-05
0.50002,-1.57,1.57,4.71,-0.79364,1.0665e-05
0.50002,-1.57,1.57,4.71,-0.29787,-5.481e-06
0.50002,-1.57,1.57,4.71,1.6885,-2.1488e-05
0.50002,-1.57,1.57,4.71,3.6752,1.0706e-05
0.50002,-1.57,1.57,4.71,3.9244,5.2072e-06
Binary file added real_robot_comparison/1.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added real_robot_comparison/2.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added real_robot_comparison/3.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added real_robot_comparison/4.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added real_robot_comparison/5.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added real_robot_comparison/6.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added real_robot_comparison/7.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added real_robot_comparison/8.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
73 changes: 1 addition & 72 deletions src/ur5_rtt_gazebo_component.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -637,27 +637,10 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
}





}

















/***************************************************************************************************************************************************/


Expand Down Expand Up @@ -726,24 +709,12 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
//



/***************************************************************************************************************************************************/



/***************************************************************************************************************************************************/
/*
* Compliance of the robot.
*/









// //if ((sim_id % 2000)&&(elm_id >= 1000)&&(sim_id>14000))
// if (elm_id == mean_trq_step)
// {
Expand Down Expand Up @@ -1137,15 +1108,7 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
last_error[j] = error_value[j];
pid_it = 0;

// 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.
//RTT::log(RTT::Error) << "Ki " << Kd[1] << " agl0 " << model->GetJoints()[joints_idx[0]]->GetAngle(0).Radian() <<" trg_agl1 " <<target_value[1] << " agl1 " << model->GetJoints()[joints_idx[1]]->GetAngle(0).Radian() << " trg_agl2 " <<target_value[2] << " agl2 " << model->GetJoints()[joints_idx[2]]->GetAngle(0).Radian() << RTT::endlog();

}

for (unsigned j = 0; j < joints_idx.size(); j++)
Expand All @@ -1156,41 +1119,14 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
/***************************************************************************************************************************************************/


/*
* Test for mass modification at the end-effector.
*/

/*
if (sim_id == 8000)
{
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);
}
*/
//
//
// Sensibility in "3D"

// if ((sim_id > 12000))
// {
// model_links_[links_idx[links_idx.back()]]->AddLinkForce(coef*test_force[id_force],test_offset);
// // model_links_[links_idx[links_idx.back()]]->AddLinkForce(0.5*gazebo::math::Vector3(0,0,-1),test_offset);
// }
//
//
//
//
//
//
// if ((sim_id%6000 == 0)&&(sim_id > 18000))
// {
// bool modif = false;
Expand Down Expand Up @@ -1249,8 +1185,6 @@ class UR5RttGazeboComponent: public RTT::TaskContext {

protected:

//! Synchronization ??

// File where data are written.
std::ofstream data_file;
std::ofstream error_file;
Expand All @@ -1272,7 +1206,6 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
std::vector<int> joints_idx;
std::vector<int> links_idx;


std::vector<gazebo::physics::JointPtr> gazebo_joints_;
gazebo::physics::Link_V model_links_;
std::vector<std::string> joint_names_;
Expand All @@ -1284,7 +1217,6 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
double l1; // length of link1
double l2; // length of link2


// For recording data randomly.
std::vector<double> jnt_it;
std::vector<double> jnt_width; // Shouldn't forget to modify it for joint 2!!
Expand All @@ -1307,7 +1239,6 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
std::vector<double> error_derivative;
std::vector<double> supp_target_value;


int pid_it;


Expand All @@ -1332,7 +1263,6 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
double trq1;
double trq2;


// Variable to save intermediate robot position - to decide if the data will be written in the file.
std::vector< std::vector<double> > inter_torque_elm;

Expand All @@ -1353,7 +1283,6 @@ class UR5RttGazeboComponent: public RTT::TaskContext {
double coef;
int id_force;


// Read position file
ifstream pos_file;

Expand Down

0 comments on commit c9cd1a1

Please sign in to comment.