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

create xacro properties again

Browse Source
This commit is contained in:
Felix Mauch
2019-03-28 10:31:08 +01:00
parent bc15944fa1
commit c96338c9e7
3 changed files with 41 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
include/ur_rtde_driver/calibration.h
View File

@@ -144,6 +144,8 @@ public:
return chain_; return chain_;
} }
std::string toXacroProperties() {return robot_parameters_corrected_.toXacroProperties();}
Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values, const size_t link_nr = 6); Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values, const size_t link_nr = 6);
private: private:
@@ -152,6 +154,7 @@ private:
void buildChain(); void buildChain();
DHRobot robot_parameters_; DHRobot robot_parameters_;
DHRobot robot_parameters_corrected_;
std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" }; std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" };
std::vector<Eigen::Matrix4d> chain_; std::vector<Eigen::Matrix4d> chain_;

13
src/calibration.cpp
View File

@@ -25,6 +25,7 @@ Calibration::~Calibration()
void Calibration::correctChain() void Calibration::correctChain()
{ {
robot_parameters_corrected_ = robot_parameters_;
correctAxis(1); correctAxis(1);
correctAxis(2); correctAxis(2);
} }
@@ -104,8 +105,10 @@ void Calibration::correctAxis(const size_t link_index)
// Correct arm segment length and angle // Correct arm segment length and angle
// ROS_INFO_STREAM("Passive old:\n" << chain_[2 * link_index]); // ROS_INFO_STREAM("Passive old:\n" << chain_[2 * link_index]);
chain_[2 * link_index](2, 3) = 0.0; chain_[2 * link_index](2, 3) = 0.0;
robot_parameters_corrected_.segments_[link_index].d_ = 0.0;
chain_[2 * link_index].topLeftCorner(3, 3) = chain_[2 * link_index].topLeftCorner(3, 3) =
Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); 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]); // ROS_INFO_STREAM("Passive new:\n" << chain_[2 * link_index]);
// Correct arm segment length and angle // 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("Theta correction: " << robot_parameters_.segments_[link_index].theta_ - new_theta);
// ROS_INFO_STREAM("Alpha correction: " << robot_parameters_.segments_[link_index].alpha_); // ROS_INFO_STREAM("Alpha correction: " << robot_parameters_.segments_[link_index].alpha_);
chain_[2 * link_index + 1](0, 3) = new_length; 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) = chain_[2 * link_index + 1].topLeftCorner(3, 3) =
Eigen::AngleAxisd(robot_parameters_.segments_[link_index].theta_ - new_theta, Eigen::Vector3d::UnitZ()) Eigen::AngleAxisd(robot_parameters_.segments_[link_index].theta_ - new_theta, Eigen::Vector3d::UnitZ())
.toRotationMatrix() * .toRotationMatrix() *
Eigen::AngleAxisd(robot_parameters_.segments_[link_index].alpha_, Eigen::Vector3d::UnitX()).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]); // ROS_INFO_STREAM("Next new:\n" << chain_[2 * link_index + 1]);
// Correct next joint // Correct next joint

46
src/calibration_correction.cpp
View File

@@ -43,7 +43,6 @@ void jointStateCallback(sensor_msgs::JointStatePtr msg)
Eigen::Matrix4d mat = my_calibration->calcForwardKinematics(jointvalues); Eigen::Matrix4d mat = my_calibration->calcForwardKinematics(jointvalues);
Eigen::Vector3d error; Eigen::Vector3d error;
error << mat.topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin()); error << mat.topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin());
@@ -61,36 +60,37 @@ int main(int argc, char* argv[])
// clang-format off // clang-format off
// d, a, theta, alpha // d, a, theta, alpha
my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , M_PI_2)); 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 , -0.612 , 0.0 , 0.0));
my_robot.segments_.push_back(DHSegment(0 , -0.5723, 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.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.1157 , 0 , 0 , -M_PI_2));
my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0)); my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0));
// clang-format on // clang-format on
// DHRobot my_robot_calibration; //DHRobot my_robot_calibration;
//// clang-format off // clang-format off
//// d, a, theta, alpha // d, a, theta, alpha
// my_robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , // ids-ur10-2
// -7.290070070824746e-05 , 0.000211987863869334 )); //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 , //my_robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , -0.01713897289704999 , -0.0072553625957652995));
// -0.01713897289704999 , -0.0072553625957652995)); my_robot_calibration.segments_.push_back(DHSegment( //my_robot_calibration.segments_.push_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , -0.03707159413492756 , -0.013483226769541364 ));
// 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(-2.2989425877563705 , 9.918593870679266e-05 , //my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , 1.488984257025741e-07 , -0.001263136163679901 ));
// 0.054279462160583214 , 0.0013495820227329425 )); //my_robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , 1.551499479707493e-05 , 0 ));
// my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , //
// 1.488984257025741e-07 , -0.001263136163679901 )); // ids-ur10-3
// my_robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , //my_robot_calibration.segments_.push_back(DHSegment(0.000489427369583322891 , -0.000157659251888300598 , 7.12144160891142552e-06 , -0.000181484221894123721));
// 1.551499479707493e-05 , 0 )); //my_robot_calibration.segments_.push_back(DHSegment(2.15820897072427709 , 0.000797598154419598693 , -0.0597679909195674153 , -0.0169424433153391799 ));
//// clang-format on //my_robot_calibration.segments_.push_back(DHSegment(-0.200070560336579328 , 0.00141079051860837357 , 0.0276740817239430857 , -0.00936706021457411366 ));
// Calibration calibration(my_robot + my_robot_calibration); //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.reset(new Calibration(my_robot));
my_calibration->correctChain(); my_calibration->correctChain();
Eigen::Matrix<double, 6, 1> jointvalues; ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties());
jointvalues << 0, 0, 0, 0, 0, 0;
Eigen::Matrix4d fk = my_calibration->calcForwardKinematics(jointvalues);
ROS_INFO_STREAM("Zero transform:\n" << fk);
tf_listener.reset(new tf::TransformListener); tf_listener.reset(new tf::TransformListener);