Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +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

13
src/calibration.cpp
View File

@@ -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