From c96338c9e7d0ad39eac5e5f1094aa5d6d804b796 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 28 Mar 2019 10:31:08 +0100 Subject: [PATCH] create xacro properties again --- include/ur_rtde_driver/calibration.h | 3 ++ src/calibration.cpp | 13 ++++++++ src/calibration_correction.cpp | 50 ++++++++++++++-------------- 3 files changed, 41 insertions(+), 25 deletions(-) diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h index 6bf7d84..31504bf 100644 --- a/include/ur_rtde_driver/calibration.h +++ b/include/ur_rtde_driver/calibration.h @@ -144,6 +144,8 @@ public: return chain_; } + std::string toXacroProperties() {return robot_parameters_corrected_.toXacroProperties();} + Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix& joint_values, const size_t link_nr = 6); private: @@ -152,6 +154,7 @@ private: void buildChain(); DHRobot robot_parameters_; + DHRobot robot_parameters_corrected_; std::vector link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" }; std::vector chain_; diff --git a/src/calibration.cpp b/src/calibration.cpp index 6bc704a..d1e7dc9 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -25,6 +25,7 @@ Calibration::~Calibration() void Calibration::correctChain() { + robot_parameters_corrected_ = robot_parameters_; correctAxis(1); correctAxis(2); } @@ -104,8 +105,10 @@ void Calibration::correctAxis(const size_t link_index) // Correct arm segment length and angle // ROS_INFO_STREAM("Passive old:\n" << chain_[2 * link_index]); chain_[2 * link_index](2, 3) = 0.0; + robot_parameters_corrected_.segments_[link_index].d_ = 0.0; chain_[2 * link_index].topLeftCorner(3, 3) = Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + robot_parameters_corrected_.segments_[link_index].theta_ = new_theta; // ROS_INFO_STREAM("Passive new:\n" << chain_[2 * link_index]); // Correct arm segment length and angle @@ -113,10 +116,20 @@ void Calibration::correctAxis(const size_t link_index) // ROS_INFO_STREAM("Theta correction: " << robot_parameters_.segments_[link_index].theta_ - new_theta); // ROS_INFO_STREAM("Alpha correction: " << robot_parameters_.segments_[link_index].alpha_); chain_[2 * link_index + 1](0, 3) = new_length; + robot_parameters_corrected_.segments_[link_index].a_ = new_length; chain_[2 * link_index + 1].topLeftCorner(3, 3) = Eigen::AngleAxisd(robot_parameters_.segments_[link_index].theta_ - new_theta, Eigen::Vector3d::UnitZ()) .toRotationMatrix() * Eigen::AngleAxisd(robot_parameters_.segments_[link_index].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix(); + if (link_index == 1) + { + robot_parameters_corrected_.delta_theta_correction2_ = robot_parameters_.segments_[link_index].theta_ - new_theta; + } + else if (link_index == 2) + { + robot_parameters_corrected_.delta_theta_correction3_ = robot_parameters_.segments_[link_index].theta_ - new_theta; + } + // ROS_INFO_STREAM("Next new:\n" << chain_[2 * link_index + 1]); // Correct next joint diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp index a4dc374..cee20d5 100644 --- a/src/calibration_correction.cpp +++ b/src/calibration_correction.cpp @@ -43,7 +43,6 @@ void jointStateCallback(sensor_msgs::JointStatePtr msg) Eigen::Matrix4d mat = my_calibration->calcForwardKinematics(jointvalues); - Eigen::Vector3d error; error << mat.topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin()); @@ -61,40 +60,41 @@ int main(int argc, char* argv[]) // 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.0 , -0.612 , 0.0 , 0.0)); + my_robot.segments_.push_back(DHSegment(0.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 )); - //// clang-format on - // Calibration calibration(my_robot + my_robot_calibration); - my_calibration.reset(new Calibration(my_robot)); + //DHRobot my_robot_calibration; + // clang-format off + // 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)); my_calibration->correctChain(); - Eigen::Matrix jointvalues; - jointvalues << 0, 0, 0, 0, 0, 0; - Eigen::Matrix4d fk = my_calibration->calcForwardKinematics(jointvalues); - ROS_INFO_STREAM("Zero transform:\n" << fk); + 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::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); ros::spin();