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

added first version of calibration

Browse Source
This commit is contained in:
Felix Mauch
2019-01-10 16:28:45 +01:00
parent 5bbeae1c8f
commit d2eb7a8683
5 changed files with 612 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
CMakeLists.txt
View File

@@ -13,17 +13,21 @@ find_package(catkin REQUIRED
actionlib
control_msgs
controller_manager
eigen_conversions
geometry_msgs
hardware_interface
kdl_parser
industrial_msgs
roscpp
sensor_msgs
std_srvs
tf
tf_conversions
trajectory_msgs
ur_msgs
)
find_package(Boost REQUIRED)
find_package(Eigen3 REQUIRED)
catkin_package(
INCLUDE_DIRS
@@ -34,13 +38,18 @@ catkin_package(
actionlib
control_msgs
controller_manager
eigen_conversions
geometry_msgs
hardware_interface
kdl_parser
industrial_msgs
roscpp
sensor_msgs
trajectory_msgs
ur_msgs
std_srvs
tf
tf_conversions
DEPENDS
Boost
)
@@ -67,6 +76,7 @@ include_directories(
include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_library(ur_rtde_driver
@@ -110,12 +120,19 @@ add_executable(plain_driver
target_link_libraries(plain_driver ${catkin_LIBRARIES} ur_rtde_driver)
add_dependencies(plain_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
install(TARGETS ur_rtde_driver ur_rtde_driver_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
add_executable(calibration_correction
src/calibration.cpp
src/calibration_correction.cpp
)
add_dependencies(calibration_correction ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(calibration_correction
${catkin_LIBRARIES}
yaml-cpp
)
install(DIRECTORY config launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
@@ -136,3 +153,8 @@ install(DIRECTORY include/${PROJECT_NAME}/
#catkin_add_gtest(ur_rtde_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp)
#target_link_libraries(ur_rtde_driver_test ur_rtde_driver ${catkin_LIBRARIES})
#endif()
install(TARGETS calibration_correction RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(TARGETS ur_rtde_driver ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
install(FILES Apache-2.0_ur_modern_driver.txt DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/LICENSES)
install(FILES manifest.yaml rosdoc.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc)

207
include/ur_rtde_driver/calibration.h Normal file
View File

@@ -0,0 +1,207 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-01-10
*
*/
//----------------------------------------------------------------------
#ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED
#define UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED
#include <ros/ros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
/*!
* \brief An internal representation of a DH-parametrized link.
*
* Each segment consists of parameters a, d, alpha and theta.
*/
struct DHSegment
{
double d_;
double a_;
double theta_;
double alpha_;
/*!
* \brief Creates an element with defined elements.
*/
DHSegment(const double d, const double a, const double theta, const double alpha)
: d_(d), a_(a), theta_(theta), alpha_(alpha)
{
}
/*!
* \brief Creates a Segment with all elements equal to zero.
*/
DHSegment() : d_(0), a_(0), theta_(0), alpha_(0)
{
}
/*!
* \brief Adds another segment element-wise (a+a, d+d, alpha+alpha, theta+theta)
*
* \param other Other segment to add
*
* \returns Segment consisting of element-wise addition of \p this and \p other
*/
DHSegment operator+(const DHSegment& other)
{
return DHSegment(this->d_ + other.d_, this->a_ + other.a_, this->theta_ + other.theta_,
this->alpha_ + other.alpha_);
}
};
/*!
* \brief Internal representation of a robot based on DH parameters.
*
* Note that this representation doesn't contain a real DH parameter representation, but is used for
* a corrected model of calibrated UR robots. Shoulder and elbow axes are artificially shortened in
* the final representation, requiring a correction parameter in \p theta_2 and \p theta_3.
*/
struct DHRobot
{
std::vector<DHSegment> segments_;
double delta_theta_correction2_;
double delta_theta_correction3_;
/*!
* \brief Create a new robot representation giving a set of \ref DHSegment objects
*/
DHRobot(const std::vector<DHSegment>& segments)
{
}
DHRobot() = default;
/*!
* \brief Adds another robot representation, by adding their segments element-wise. See \ref
* DHSegment::operator+ for details.
*/
DHRobot operator+(const DHRobot& other)
{
assert(this->segments_.size() == other.segments_.size());
DHRobot ret;
for (size_t i = 0; i < this->segments_.size(); ++i)
{
ret.segments_.push_back(this->segments_[i] + other.segments_[i]);
}
return ret;
}
/*!
* \brief Generates a string representation of this robot representation.
*
* This string can be directly included into the xacro file used for a calibrated robot.
*/
std::string toXacroProperties()
{
std::stringstream ss;
for (size_t i = 0; i < segments_.size(); ++i)
{
ss << "<xacro:property name=\"d" << i + 1 << "\" value=\"" << segments_[i].d_ << "\" />\n";
ss << "<xacro:property name=\"a" << i + 1 << "\" value=\"" << segments_[i].a_ << "\" />\n";
ss << "<xacro:property name=\"theta" << i + 1 << "\" value=\"" << segments_[i].theta_ << "\" />\n";
ss << "<xacro:property name=\"alpha" << i + 1 << "\" value=\"" << segments_[i].alpha_ << "\" />\n";
}
ss << "<xacro:property name=\"delta_theta_correction2\" value=\"" << delta_theta_correction2_ << "\" />\n";
ss << "<xacro:property name=\"delta_theta_correction3\" value=\"" << delta_theta_correction3_ << "\" />\n";
return ss.str();
}
};
/*!
* \brief Class that handles the calibration correction for Universal Robots
*
* Universal robots provide a factory calibration of their DH parameters to exactly estimate their
* TCP pose using forward kinematics. However, those DH parameters construct a kinematic chain that
* can be very long, as upper arm and lower arm segments can be drawn out of their physical position
* by multiple meters (even above 100m can occur).
*
* This class helps creating a kinematic chain, that is as close as possible to the physical model,
* by dragging the upper and lower arm segments back to their zero position.
*/
class Calibration
{
public:
Calibration();
virtual ~Calibration();
/*!
* \brief Corrects a UR kinematic chain in such a way that shoulder and elbow offsets are 0.
*/
KDL::Chain correctChain();
/*!
* \brief Print information about every segment in the chain
*
*/
static void debugChain(const KDL::Chain& chain);
private:
/*!
* \brief Splits the given chain \p in into a subchain before \p split_index and after.
*
* \param[in,out] in Complete chain that will be split. Will be only the part before the \p
* split_index after the function returns.
* \param[out] second Will contain the subchain after \p split_index after the function returns.
* \param[in] split_index Index at which the chain should be split. The segment with this index
* will be the first segment in \p second.
*
* \returns true on success, false if \p split_index is larger than the chain size. In the latter
* case, \p in will not be touched.
*/
bool splitChain(KDL::Chain& in, KDL::Chain& second, const size_t split_index);
KDL::Chain correctAxis(KDL::Chain& chain);
/*!
* \brief Converts a \p KDL::Chain representation into a \ref DHRobot representation
*/
static DHRobot chainToDH(const KDL::Chain& chain);
/*!
* \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_
*/
KDL::Chain getChain();
/*!
* \brief Modifies the robot chain at segment \p correction_index
*
* Correction will set the \p d parameter at the segment to 0 and adapt the rest of the chain
* accordingly
*
* \param[in,out] robot_chain The kinemtatic chain to correct
* \param[in] correction_index Index at which the correction step should be performed.
*
* \returns true on success, false if the chain is smaller than the \p correction_index
*/
bool correctChainAt(KDL::Chain& robot_chain, const size_t correction_index);
/*!
* \brief Given all parameters to correct the chain and a chain, create a corrected chain.
*
* \param[in] robot_chain Uncorrected chain from which the correction parameters were calculated.
* \param[in] new_length New segment length (\p a) of the corrected segment
* \param[in] new_theta New rotation offset (\p theta) of the corrected segment
* \param[in] distance_correction Distance by which the next segment has to be adapted due to the
* correction
*
* \returns The corrected chain, where the first segment's \p d parameter is 0
*/
static KDL::Chain buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, const double new_theta, const double distance_correction);
DHRobot robot_parameters_;
std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" };
};
#endif // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED

3
package.xml
View File

@@ -24,13 +24,16 @@
<depend>controller_manager</depend>
<depend>actionlib</depend>
<depend>control_msgs</depend>
<depend>eigen_conversions</depend>
<depend>geometry_msgs</depend>
<depend>kdl_parser</depend>
<depend>industrial_msgs</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>trajectory_msgs</depend>
<depend>ur_msgs</depend>
<depend>tf</depend>
<depend>tf_conversions</depend>
<depend>std_srvs</depend>
<exec_depend>force_torque_sensor_controller</exec_depend>

351
src/calibration.cpp Normal file
View File

@@ -0,0 +1,351 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-01-10
*
*/
//----------------------------------------------------------------------
#include <ur_rtde_driver/calibration.h>
#include <eigen_conversions/eigen_kdl.h>
Calibration::Calibration()
{
// This is the data of our ids-ur10-2 Later, dh parameters will be given using another
// constructor.
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));
// my_robot.segments_.push_back(DHSegment(0.1, 0.2, 0.3, 0.4));
// DHRobot my_robot_calibration;
//// clang-format off
//// d, a, theta, alpha
// my_robot_calibration.segments_.push_back(DHSegment(0 ,0 ,0 ,0 ));
// my_robot_calibration.segments_.push_back(DHSegment(1.5 ,0 ,0.0 ,0.0 ));
// my_robot_calibration.segments_.push_back(DHSegment(0.5 ,0.0 ,0.2 ,0.0 ));
// my_robot_calibration.segments_.push_back(DHSegment(-2 ,0 ,0 ,0 ));
// my_robot_calibration.segments_.push_back(DHSegment(0 ,0 ,0 ,0 ));
// my_robot_calibration.segments_.push_back(DHSegment(0 ,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
robot_parameters_ = my_robot + my_robot_calibration;
}
Calibration::~Calibration()
{
}
KDL::Chain Calibration::correctChain()
{
KDL::Chain robot_chain = getChain();
correctChainAt(robot_chain, 2);
correctChainAt(robot_chain, 4);
return robot_chain;
}
bool Calibration::correctChainAt(KDL::Chain& robot_chain, const size_t correction_index)
{
bool success = true;
KDL::Chain correction_chain, corrected_subchain;
// Split chain into part before the correction joint and after. We then change the first link on
// the second chain. This splitting is necessary as KDL doesn't support forward kinematics
// relative to another link than the first.
success = splitChain(robot_chain, correction_chain, correction_index);
// Correct the chain after the correction joint
corrected_subchain = correctAxis(correction_chain);
// Add the corrected second part to the robot chain to get a full robot model again
robot_chain.addChain(corrected_subchain);
return success;
}
bool Calibration::splitChain(KDL::Chain& robot_chain, KDL::Chain& second_chain, const size_t split_index)
{
if (split_index >= robot_chain.segments.size())
{
return false;
}
second_chain = KDL::Chain();
for (auto it = robot_chain.segments.begin() + split_index; it != robot_chain.segments.end();)
{
KDL::Segment new_segment = *it;
robot_chain.segments.erase(it);
second_chain.addSegment(new_segment);
}
return true;
}
KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
{
// Each DH-Segment is split into two KDL segments. One representing the d and theta parameters and
// one with the a and alpha parameters. If we start from the first segment (which represents d and
// theta), there follows a passive segment (with a and alpha) and the next d/theta-segment after
// that.
//
// In principle, the d parameter of the first segment gets set to zero, first. With this change,
// the kinematic structure gets destroyed, which has to be corrected:
// - With setting d to 0, both the start and end points of the passive segment move along the
// rotational axis of the start segment. Instead, the end point of the passive segment has to
// move along the rotational axis of the next segment. This creates a change in a and and theta, if
// the two rotational axes are not parallel.
//
// - The length of moving along the next segment's rotational axis is calculated by intersecting
// the rotational axis with the XY-plane of the first segment.
//
// -
KDL::Frame next_segment_frame;
KDL::Frame passive_frame;
KDL::ChainFkSolverPos_recursive fk_solver(robot_chain);
uint32_t num_jts = robot_chain.getNrOfJoints();
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
fk_solver.JntToCart(jointpositions, passive_frame, 2);
ROS_INFO_STREAM(passive_frame.p.x() << ", " << passive_frame.p.y() << ", " << passive_frame.p.z());
fk_solver.JntToCart(jointpositions, next_segment_frame, 3);
ROS_INFO_STREAM(next_segment_frame.p.x() << ", " << next_segment_frame.p.y() << ", " << next_segment_frame.p.z());
Eigen::Vector3d eigen_passive;
tf::vectorKDLToEigen(passive_frame.p, eigen_passive);
Eigen::Vector3d eigen_next;
tf::vectorKDLToEigen(next_segment_frame.p, eigen_next);
// Construct a representation of the next segment's rotational axis
Eigen::ParametrizedLine<double, 3> next_line;
next_line = Eigen::ParametrizedLine<double, 3>::Through(eigen_passive, eigen_next);
// ROS_INFO_STREAM("next_line:" << std::endl
//<< "Base:" << std::endl
//<< next_line.origin() << std::endl
//<< "Direction:" << std::endl
//<< next_line.direction());
// XY-Plane of first segment's start
Eigen::Hyperplane<double, 3> plane(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0));
// Intersect the rotation axis with the XY-Plane. We use both notations, the length and
// intersection point, as we will need both. The intersection_param is the length moving along the
// plane (change in the next segment's d param), while the intersection point will be used for
// calculating the new angle theta.
double intersection_param = next_line.intersectionParameter(plane);
Eigen::Vector3d intersection = next_line.intersectionPoint(plane);
double new_theta = std::atan(intersection.y() / intersection.x());
// Upper and lower arm segments on URs all have negative length due to dh params
double new_length = -1 * intersection.norm();
// ROS_INFO_STREAM("Wrist line intersecting at " << std::endl << intersection);
// ROS_INFO_STREAM("Angle is " << new_theta);
// ROS_INFO_STREAM("Length is " << new_length);
// ROS_INFO_STREAM("Intersection param is " << intersection_param);
// as we move the passive segment towards the first segment, we have to move away the next segment
// again, to keep the same kinematic structure.
double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0;
double distance_correction = intersection_param * sign_dir;
ROS_INFO_STREAM("Corrected chain at " << robot_chain.segments[0].getName());
return buildCorrectedChain(robot_chain, new_length, new_theta, distance_correction);
}
KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, const double new_theta,
const double distance_correction)
{
KDL::Chain corrected_chain;
// Set d parameter of the first segment to 0 and theta to the calculated new angle
{
KDL::Frame frame = KDL::Frame::DH(0, 0, 0, new_theta);
KDL::Joint joint = KDL::Joint(KDL::Vector(0, 0, 0), KDL::Vector(0, 0, 1), KDL::Joint::RotAxis);
KDL::Segment segment = KDL::Segment(robot_chain.segments[0].getName(), joint, frame);
corrected_chain.addSegment(segment);
}
// Correct arm segment length and angle
{
// Get the original theta from the first link
double roll_a, pitch_a, yaw_a;
robot_chain.segments[0].getFrameToTip().M.GetRPY(roll_a, pitch_a, yaw_a);
double roll, pitch, yaw;
robot_chain.segments[1].getFrameToTip().M.GetRPY(roll, pitch, yaw);
// Since we changed the kinematic structure, we have to make sure to rotate around the correct
// axis, so we revert the theta change in this joint, again.
KDL::Rotation rotation = KDL::Rotation::EulerZYX(yaw_a - new_theta, 0, roll);
KDL::Vector position(new_length, 0, 0);
KDL::Frame frame(rotation, position);
// KDL::Frame frame = KDL::Frame::DH(new_length, roll, 0, yaw_a - new_theta);
KDL::Segment segment = KDL::Segment(robot_chain.segments[1].getName(), KDL::Joint(KDL::Joint::None), frame);
corrected_chain.addSegment(segment);
}
// Correct next joint
{
KDL::Frame new_frame = robot_chain.getSegment(2).getFrameToTip();
new_frame.p = robot_chain.getSegment(2).getFrameToTip().p;
ROS_INFO_STREAM("Correcting segment i+2 length from " << new_frame.p.z());
// the d-parameter can be modified by the intersection_parameter which is the distance traveled
// along the rotation axis
new_frame.p = KDL::Vector(0, 0, new_frame.p.z() - distance_correction);
ROS_INFO_STREAM("Corrected segment i+2 length to " << new_frame.p.z());
KDL::Joint new_joint = robot_chain.getSegment(2).getJoint();
KDL::Segment segment = KDL::Segment(robot_chain.getSegment(2).getName(), new_joint, new_frame);
corrected_chain.addSegment(segment);
}
for (size_t i = 3; i < robot_chain.segments.size(); ++i)
{
corrected_chain.addSegment(robot_chain.segments[i]);
}
return corrected_chain;
}
DHRobot Calibration::chainToDH(const KDL::Chain& chain)
{
KDL::ChainFkSolverPos_recursive fk_solver(chain);
uint32_t num_jts = chain.getNrOfJoints();
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
// Assign some values to the joint positions
for (unsigned int i = 0; i < num_jts; i++)
{
jointpositions(i) = 0;
}
DHRobot robot;
for (size_t i = 0; i < chain.segments.size(); i += 2)
{
ROS_INFO_STREAM("Extracting DH parameters for joint " << i);
for (size_t j = i; j < i + 2; ++j)
{
// ROS_INFO_STREAM(j << " < " << i << "+2");
KDL::Frame result;
result = chain.segments[j].getFrameToTip();
ROS_INFO_STREAM("Relative position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z());
double roll, pitch, yaw;
result.M.GetRPY(roll, pitch, yaw);
ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw);
fk_solver.JntToCart(jointpositions, result, j);
KDL::Joint joint = chain.segments[j].getJoint();
}
DHSegment seg;
{
KDL::Frame result = chain.segments[i].getFrameToTip();
double roll, pitch, yaw;
result.M.GetRPY(roll, pitch, yaw);
seg.theta_ = yaw;
seg.d_ = result.p.z();
}
{
KDL::Frame result = chain.segments[i + 1].getFrameToTip();
double roll, pitch, yaw;
result.M.GetRPY(roll, pitch, yaw);
seg.alpha_ = roll;
seg.a_ = result.p.x();
}
robot.segments_.push_back(seg);
}
{
double roll, pitch, yaw;
chain.segments[3].getFrameToTip().M.GetRPY(roll, pitch, yaw);
robot.delta_theta_correction2_ = yaw;
chain.segments[5].getFrameToTip().M.GetRPY(roll, pitch, yaw);
robot.delta_theta_correction3_ = yaw;
}
return robot;
}
KDL::Chain Calibration::getChain()
{
KDL::Chain robot_chain;
for (size_t i = 0; i < robot_parameters_.segments_.size(); ++i)
{
KDL::Frame frame = KDL::Frame::DH(0, 0, robot_parameters_.segments_[i].d_, robot_parameters_.segments_[i].theta_);
KDL::Joint joint = KDL::Joint(link_names_[i], KDL::Vector(0, 0, robot_parameters_.segments_[i].d_),
KDL::Vector(0, 0, 1), KDL::Joint::RotAxis);
KDL::Segment segment = KDL::Segment(link_names_[i], joint, frame);
robot_chain.addSegment(segment);
KDL::Frame frame2 = KDL::Frame::DH(robot_parameters_.segments_[i].a_, robot_parameters_.segments_[i].alpha_, 0, 0);
KDL::Segment segment2 =
KDL::Segment(link_names_[i] + "_passive", KDL::Joint(link_names_[i] + "_passive", KDL::Joint::None), frame2);
robot_chain.addSegment(segment2);
}
return robot_chain;
}
void Calibration::debugChain(const KDL::Chain& robot_chain)
{
uint32_t num_jts = robot_chain.getNrOfJoints();
KDL::ChainFkSolverPos_recursive fk_solver(robot_chain);
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
// Assign some values to the joint positions
for (unsigned int i = 0; i < num_jts; i++)
{
jointpositions(i) = 0;
}
for (size_t i = 0; i < robot_chain.segments.size(); ++i)
{
ROS_INFO_STREAM("Segment " << i << ": " << robot_chain.segments[i].getName());
KDL::Frame result;
result = robot_chain.segments[i].getFrameToTip();
ROS_INFO_STREAM("Relative position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z());
double roll, pitch, yaw;
result.M.GetRPY(roll, pitch, yaw);
ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw);
fk_solver.JntToCart(jointpositions, result, i);
ROS_INFO_STREAM(std::setprecision(15) << "Absolute position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z());
KDL::Joint joint = robot_chain.segments[i].getJoint();
ROS_INFO_STREAM("Joint type: " << joint.getTypeName());
ROS_INFO_STREAM("Joint position: " << joint.pose(0).p.x() << ", " << joint.pose(0).p.y() << ", "
<< joint.pose(0).p.z());
ROS_INFO_STREAM("Joint Axis: " << joint.JointAxis().x() << "," << joint.JointAxis().y() << ", "
<< joint.JointAxis().z());
}
DHRobot robot_out = Calibration::chainToDH(robot_chain);
ROS_INFO_STREAM("Robot data:\n" << robot_out.toXacroProperties());
}

25
src/calibration_correction.cpp Normal file
View File

@@ -0,0 +1,25 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-01-10
*
*/
//----------------------------------------------------------------------
#include <ur_rtde_driver/calibration.h>
int main(int argc, char* argv[])
{
Calibration calibration;
KDL::Chain corrected_chain = calibration.correctChain();
calibration.debugChain(corrected_chain);
return 0;
}