Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-09 17:40:47 +02:00
Code Releases Activity

check multiple calibrations in one run

Browse Source
This commit is contained in:
Felix Mauch
2019-03-28 16:03:47 +01:00
parent 281d4c3955
commit 579a39a7df
1 changed files with 67 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
src/calibration_correction.cpp
View File

@@ -66,37 +66,79 @@ int main(int argc, char* argv[])
my_robot.segments_.push_back(DHSegment(0.1157 , 0 , 0 , -M_PI_2));
my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0));
// clang-format on
//DHRobot my_robot_calibration;
// clang-format off
std::map<std::string, DHRobot> calibrations;
// d, a, theta, alpha
// ids-ur10-2
//my_robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , -7.290070070824746e-05 , 0.000211987863869334 ));
//my_robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , -0.01713897289704999 , -0.0072553625957652995));
//my_robot_calibration.segments_.push_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , -0.03707159413492756 , -0.013483226769541364 ));
//my_robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , 0.054279462160583214 , 0.0013495820227329425 ));
//my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , 1.488984257025741e-07 , -0.001263136163679901 ));
//my_robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , 1.551499479707493e-05 , 0 ));
//
// ids-ur10-3
//my_robot_calibration.segments_.push_back(DHSegment(0.000489427369583322891 , -0.000157659251888300598 , 7.12144160891142552e-06 , -0.000181484221894123721));
//my_robot_calibration.segments_.push_back(DHSegment(2.15820897072427709 , 0.000797598154419598693 , -0.0597679909195674153 , -0.0169424433153391799 ));
//my_robot_calibration.segments_.push_back(DHSegment(-0.200070560336579328 , 0.00141079051860837357 , 0.0276740817239430857 , -0.00936706021457411366 ));
//my_robot_calibration.segments_.push_back(DHSegment(-1.95850825255269623 , 8.02528233902343753e-05 , 0.0321549453426039911 , 0.000705308984090935454 ));
//my_robot_calibration.segments_.push_back(DHSegment(-2.27384815544295904e-05, 2.49305433182637798e-06 , -7.91131358362739777e-05, -0.000129247903554619015));
//my_robot_calibration.segments_.push_back(DHSegment(1.40058148229704749e-05 , 0 , 2.43093497590859384e-05 , 0 ));
// clang-format on
//my_calibration.reset(new Calibration(my_robot + my_robot_calibration));
my_calibration.reset(new Calibration(my_robot));
{
DHRobot robot_calibration;
// clang-format off
robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , -7.290070070824746e-05 , 0.000211987863869334 ));
robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , -0.01713897289704999 , -0.0072553625957652995));
robot_calibration.segments_.push_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , -0.03707159413492756 , -0.013483226769541364 ));
robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , 0.054279462160583214 , 0.0013495820227329425 ));
robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , 1.488984257025741e-07 , -0.001263136163679901 ));
robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , 1.551499479707493e-05 , 0 ));
// clang-format on
calibrations.insert(std::make_pair("ids-ur10-2", robot_calibration));
}
{
DHRobot robot_calibration;
// clang-format off
robot_calibration.segments_.push_back(DHSegment(0.000489427369583322891 , -0.000157659251888300598 , 7.12144160891142552e-06 , -0.000181484221894123721));
robot_calibration.segments_.push_back(DHSegment(2.15820897072427709 , 0.000797598154419598693 , -0.0597679909195674153 , -0.0169424433153391799 ));
robot_calibration.segments_.push_back(DHSegment(-0.200070560336579328 , 0.00141079051860837357 , 0.0276740817239430857 , -0.00936706021457411366 ));
robot_calibration.segments_.push_back(DHSegment(-1.95850825255269623 , 8.02528233902343753e-05 , 0.0321549453426039911 , 0.000705308984090935454 ));
robot_calibration.segments_.push_back(DHSegment(-2.27384815544295904e-05, 2.49305433182637798e-06 , -7.91131358362739777e-05, -0.000129247903554619015));
robot_calibration.segments_.push_back(DHSegment(1.40058148229704749e-05 , 0 , 2.43093497590859384e-05 , 0 ));
// clang-format on
calibrations.insert(std::make_pair("ids-ur10-3", robot_calibration));
}
// my_calibration.reset(new Calibration(my_robot));
my_calibration->correctChain();
for (auto& calib : calibrations)
{
Eigen::Matrix<double, 6, 1> jointvalues;
ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties());
double min_error = 9999;
double max_error = 0;
double mean_error = 0;
const size_t num_runs = 100000;
for (size_t i = 0; i < num_runs; ++i)
{
Calibration calibration(my_robot + calib.second);
jointvalues = 2 * M_PI * Eigen::Matrix<double, 6, 1>::Random();
Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues);
calibration.correctChain();
Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues);
tf_listener.reset(new tf::TransformListener);
Eigen::Vector3d error;
error << fk_orig.topRightCorner(3, 1) - fk_corrected.topRightCorner(3, 1);
ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback);
if (error.norm() < min_error)
{
min_error = error.norm();
}
if (error.norm() > max_error)
{
max_error = error.norm();
}
mean_error += error.norm() / static_cast<double>(num_runs);
}
ros::spin();
ROS_INFO_STREAM(calib.first << ": Error over " << num_runs << " joint angles (mean, min, max): (" << mean_error
<< ", " << min_error << ", " << max_error << ")");
// Calibration calibration(my_robot + calib.second);
// calibration.correctChain();
// ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration.toXacroProperties());
}
// my_calibration->correctChain();
// ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties());
// tf_listener.reset(new tf::TransformListener);
// ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback);
// ros::spin();
return 0;
}