Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

implemented a prototype for an integration test for calibration

Browse Source 
We should put this into a proper calibration test.
This commit is contained in:
Felix Mauch
2019-03-12 03:04:25 +01:00
parent 792cdc3a08
commit eaae523484
1 changed files with 85 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
src/calibration_correction.cpp
View File

@@ -14,29 +14,94 @@
#include <ur_rtde_driver/calibration.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_listener.h>
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
sensor_msgs::JointStatePtr joint_state;
std::unique_ptr<tf::TransformListener> tf_listener;
void jointStateCallback(sensor_msgs::JointStatePtr msg)
{
tf::StampedTransform stamped_transform;
try
{
tf_listener->lookupTransform("base", "tool0_controller", ros::Time(0), stamped_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("TF lookup error error: %s", ex.what());
return;
}
geometry_msgs::TransformStamped tf_msg;
tf::transformStampedTFToMsg(stamped_transform, tf_msg);
// ROS_INFO_STREAM(tf_msg);
KDL::Frame result_original;
KDL::JntArray jointpositions = KDL::JntArray(6);
for (size_t i = 0; i < 6; ++i)
{
jointpositions.data[i] = msg->position[i];
}
fk_solver->JntToCart(jointpositions, result_original);
Eigen::Matrix4d mat;
result_original.Make4x4(mat.data());
Eigen::Vector3d error;
error << mat.transpose().topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin());
ROS_INFO_STREAM(error);
}
int main(int argc, char* argv[])
{
DHRobot my_robot;
my_robot.segments_.push_back(DHSegment(0.1273, 0, 0, M_PI / 2));
my_robot.segments_.push_back(DHSegment(0, -0.612, 0, 0));
my_robot.segments_.push_back(DHSegment(0, -0.5723, 0, 0.0));
my_robot.segments_.push_back(DHSegment(0.163841, 0, 0.0, M_PI / 2));
my_robot.segments_.push_back(DHSegment(0.1157, 0, 0, -M_PI / 2));
my_robot.segments_.push_back(DHSegment(0.0922, 0, 0, 0));
DHRobot my_robot_calibration;
// clang-format off
// d, a, theta, alpha
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 ));
// clang-format on
ros::init(argc, argv, "ur_calibration");
ros::NodeHandle nh;
Calibration calibration(my_robot + my_robot_calibration);
KDL::Chain corrected_chain = calibration.correctChain();
calibration.debugChain(corrected_chain);
DHRobot my_robot;
// This is an ideal UR10
// clang-format off
// d, a, theta, alpha
my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , M_PI / 2));
my_robot.segments_.push_back(DHSegment(0 , -0.612 , 0 , 0));
my_robot.segments_.push_back(DHSegment(0 , -0.5723, 0 , 0.0));
my_robot.segments_.push_back(DHSegment(0.163941, 0 , 0 , M_PI / 2));
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
//// d, a, theta, alpha
// 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 ));
// Calibration calibration(my_robot + my_robot_calibration);
Calibration calibration(my_robot);
KDL::Chain corrected_chain = calibration.getChain();
// calibration.debugChain(corrected_chain);
uint32_t num_jts = corrected_chain.getNrOfJoints();
fk_solver.reset(new KDL::ChainFkSolverPos_recursive(corrected_chain));
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
KDL::Frame result_original;
tf_listener.reset(new tf::TransformListener);
ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback);
ros::spin();
return 0;
}