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

Use plain Eigen for kinematics

Browse Source
This commit is contained in:
Felix Mauch
2019-03-20 18:33:04 +01:00
parent 8a9192aeba
commit bc15944fa1
4 changed files with 161 additions and 351 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
include/ur_rtde_driver/calibration.h
View File

@@ -16,9 +16,7 @@
#define UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED
#include <ros/ros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <Eigen/Dense>
/*!
* \brief An internal representation of a DH-parametrized link.
@@ -119,7 +117,6 @@ struct DHRobot
}
};
/*!
* \brief Class that handles the calibration correction for Universal Robots
*
@@ -140,68 +137,23 @@ public:
/*!
* \brief Corrects a UR kinematic chain in such a way that shoulder and elbow offsets are 0.
*/
KDL::Chain correctChain();
void correctChain();
/*!
* \brief Print information about every segment in the chain
*
*/
static void debugChain(const KDL::Chain& chain);
std::vector<Eigen::Matrix4d> getChain()
{
return chain_;
}
/*!
* \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_
*/
KDL::Chain getChain();
Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values, const size_t link_nr = 6);
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);
void correctAxis(const size_t correction_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 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);
void buildChain();
DHRobot robot_parameters_;
std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" };
std::vector<Eigen::Matrix4d> chain_;
};
#endif // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED

290
src/calibration.cpp
View File

@@ -14,66 +14,24 @@
#include <ur_rtde_driver/calibration.h>
#include <eigen_conversions/eigen_kdl.h>
Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters)
{
buildChain();
}
Calibration::~Calibration()
{
}
KDL::Chain Calibration::correctChain()
void Calibration::correctChain()
{
KDL::Chain robot_chain = getChain();
correctChainAt(robot_chain, 2);
correctChainAt(robot_chain, 4);
return robot_chain;
correctAxis(1);
correctAxis(2);
}
bool Calibration::correctChainAt(KDL::Chain& robot_chain, const size_t correction_index)
void Calibration::correctAxis(const size_t link_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
// Each DH-Segment is split into two chain 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.
@@ -87,24 +45,28 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
//
// - 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);
if (chain_[2 * link_index](2, 3) == 0.0)
{
// Nothing to do here.
return;
}
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::Matrix<double, 6, 1> jointvalues;
jointvalues << 0, 0, 0, 0, 0, 0;
// Eigen::Matrix4d fk_current = calcForwardKinematics(jointvalues, link_index);
// Eigen::Vector3d current_passive = fk_current.topRightCorner(3, 1);
// ROS_INFO_STREAM("current passive:\n" << current_passive);
Eigen::Matrix4d fk_current = Eigen::Matrix4d::Identity();
Eigen::Vector3d current_passive = Eigen::Vector3d::Zero();
Eigen::Vector3d eigen_passive;
tf::vectorKDLToEigen(passive_frame.p, eigen_passive);
Eigen::Vector3d eigen_next;
tf::vectorKDLToEigen(next_segment_frame.p, eigen_next);
Eigen::Matrix4d fk_next_passive = Eigen::Matrix4d::Identity();
fk_next_passive *= chain_[link_index * 2] * chain_[link_index * 2 + 1];
Eigen::Vector3d eigen_passive = fk_next_passive.topRightCorner(3, 1);
// ROS_INFO_STREAM("Eigen passive:\n" << eigen_passive);
Eigen::Vector3d eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).topRightCorner(3, 1);
// ROS_INFO_STREAM("Eigen next:\n" << eigen_next);
// Construct a representation of the next segment's rotational axis
Eigen::ParametrizedLine<double, 3> next_line;
@@ -117,14 +79,14 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
//<< 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));
Eigen::Hyperplane<double, 3> plane(fk_current.topLeftCorner(3, 3) * Eigen::Vector3d(0, 0, 1), current_passive);
// 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);
Eigen::Vector3d intersection = next_line.intersectionPoint(plane) - current_passive;
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();
@@ -138,180 +100,66 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
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
// ROS_INFO_STREAM("Passive old:\n" << chain_[2 * link_index]);
chain_[2 * link_index](2, 3) = 0.0;
chain_[2 * link_index].topLeftCorner(3, 3) =
Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix();
// ROS_INFO_STREAM("Passive new:\n" << chain_[2 * link_index]);
// 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);
}
// ROS_INFO_STREAM("Next old:\n" << chain_[2 * link_index + 1]);
// 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;
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();
// ROS_INFO_STREAM("Next new:\n" << chain_[2 * link_index + 1]);
// 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;
// ROS_INFO_STREAM("Second Next old:\n" << chain_[2 * link_index + 2]);
chain_[2 * link_index + 2](2, 3) -= distance_correction;
// ROS_INFO_STREAM("Second Next new:\n" << chain_[2 * link_index + 2]);
}
DHRobot Calibration::chainToDH(const KDL::Chain& chain)
Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values,
const size_t link_nr)
{
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++)
// ROS_INFO_STREAM("Calculating forward kinematics at link " << link_nr);
// Currently ignore input and calculate for zero vector input
Eigen::Matrix4d output = Eigen::Matrix4d::Identity();
for (size_t i = 0; i < link_nr; ++i)
{
jointpositions(i) = 0;
Eigen::Matrix4d rotation = Eigen::Matrix4d::Identity();
rotation.topLeftCorner(3, 3) = Eigen::AngleAxisd(joint_values(i), Eigen::Vector3d::UnitZ()).toRotationMatrix();
output *= chain_[2 * i] * rotation * chain_[2 * i + 1];
}
DHRobot robot;
// ROS_INFO_STREAM("forward_kinematics at " << link_nr << std::endl << output);
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;
return output;
}
KDL::Chain Calibration::getChain()
void Calibration::buildChain()
{
KDL::Chain robot_chain;
chain_.clear();
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);
Eigen::Matrix4d seg1_mat = Eigen::Matrix4d::Identity();
seg1_mat.topLeftCorner(3, 3) =
Eigen::AngleAxisd(robot_parameters_.segments_[i].theta_, Eigen::Vector3d::UnitZ()).toRotationMatrix();
seg1_mat(2, 3) = robot_parameters_.segments_[i].d_;
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);
chain_.push_back(seg1_mat);
robot_chain.addSegment(segment2);
Eigen::Matrix4d seg2_mat = Eigen::Matrix4d::Identity();
seg2_mat.topLeftCorner(3, 3) =
Eigen::AngleAxisd(robot_parameters_.segments_[i].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix();
seg2_mat(0, 3) = robot_parameters_.segments_[i].a_;
chain_.push_back(seg2_mat);
}
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());
}

69
src/calibration_correction.cpp
View File

@@ -17,16 +17,18 @@
#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<Calibration> my_calibration;
std::unique_ptr<tf::TransformListener> tf_listener;
void jointStateCallback(sensor_msgs::JointStatePtr msg)
{
ROS_INFO("Callback");
tf::StampedTransform stamped_transform;
try
{
tf_listener->lookupTransform("base", "tool0_controller", ros::Time(0), stamped_transform);
tf_listener->waitForTransform("base", "tool0_controller", msg->header.stamp, ros::Duration(0.5));
tf_listener->lookupTransform("base", "tool0_controller", msg->header.stamp, stamped_transform);
}
catch (tf::TransformException ex)
{
@@ -37,23 +39,16 @@ void jointStateCallback(sensor_msgs::JointStatePtr msg)
geometry_msgs::TransformStamped tf_msg;
tf::transformStampedTFToMsg(stamped_transform, tf_msg);
// ROS_INFO_STREAM(tf_msg);
Eigen::Matrix<double, 6, 1> jointvalues = Eigen::Matrix<double, 6, 1>(msg->position.data());
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 = my_calibration->calcForwardKinematics(jointvalues);
Eigen::Matrix4d mat;
result_original.Make4x4(mat.data());
Eigen::Vector3d error;
error << mat.transpose().topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin());
error << mat.topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin());
ROS_INFO_STREAM(error);
ROS_INFO_STREAM("Error(abs): " << error.norm());
ROS_INFO_STREAM("Error per axis:\n" << error);
}
int main(int argc, char* argv[])
@@ -65,41 +60,41 @@ int main(int argc, char* argv[])
// 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.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.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 ));
//// 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);
Calibration calibration(my_robot);
KDL::Chain corrected_chain = calibration.getChain();
// calibration.debugChain(corrected_chain);
my_calibration.reset(new Calibration(my_robot));
uint32_t num_jts = corrected_chain.getNrOfJoints();
my_calibration->correctChain();
fk_solver.reset(new KDL::ChainFkSolverPos_recursive(corrected_chain));
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
KDL::Frame result_original;
Eigen::Matrix<double, 6, 1> jointvalues;
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);
ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback);
ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback);
ros::spin();

83
test/calibration_test.cpp
View File

@@ -17,6 +17,11 @@
namespace
{
bool isApproximately(const double val1, const double val2, const double precision)
{
return std::abs(val1 - val2) < precision;
}
TEST(UrRtdeDriver, ur10_ideal)
{
DHRobot my_robot;
@@ -34,18 +39,26 @@ TEST(UrRtdeDriver, ur10_ideal)
Calibration calibration(my_robot);
KDL::Chain robot_chain = calibration.getChain();
uint32_t num_jts = robot_chain.getNrOfJoints();
Eigen::Matrix<double, 6, 1> jointvalues;
{
jointvalues << 0, 0, 0, 0, 0, 0;
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
EXPECT_DOUBLE_EQ(fk(0, 3), my_robot.segments_[1].a_ + my_robot.segments_[2].a_);
EXPECT_DOUBLE_EQ(fk(1, 3), -1 * (my_robot.segments_[3].d_ + my_robot.segments_[5].d_));
EXPECT_DOUBLE_EQ(fk(2, 3), my_robot.segments_[0].d_ - my_robot.segments_[4].d_);
}
KDL::ChainFkSolverPos_recursive fk_solver(robot_chain);
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
KDL::Frame result;
fk_solver.JntToCart(jointpositions, result);
// Check whether our internal KDL representation gives correct values
EXPECT_DOUBLE_EQ(result.p.x(), my_robot.segments_[1].a_ + my_robot.segments_[2].a_);
EXPECT_DOUBLE_EQ(result.p.y(), -1 * (my_robot.segments_[3].d_ + my_robot.segments_[5].d_));
EXPECT_DOUBLE_EQ(result.p.z(), my_robot.segments_[0].d_ - my_robot.segments_[4].d_);
{
jointvalues << M_PI_2, -M_PI_4, M_PI_2, -M_PI_4, 0, 0;
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
EXPECT_DOUBLE_EQ(fk(0, 3), my_robot.segments_[3].d_ + my_robot.segments_[5].d_);
EXPECT_DOUBLE_EQ(fk(1, 3), my_robot.segments_[1].a_ / std::sqrt(2) + my_robot.segments_[2].a_ / std::sqrt(2));
// Because of the sqrt calculations a DOUBLE_EQ does not work here.
EXPECT_PRED3(isApproximately, fk(2, 3),
my_robot.segments_[0].d_ - my_robot.segments_[1].a_ / std::sqrt(2) +
my_robot.segments_[2].a_ / std::sqrt(2) - my_robot.segments_[4].d_,
1e-16);
}
}
TEST(UrRtdeDriver, calibration)
@@ -65,34 +78,36 @@ TEST(UrRtdeDriver, calibration)
// 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 ));
// d, a, theta, alpha
my_robot_calibration.segments_.push_back(DHSegment(0 , 0 , 0 , 0.0));
my_robot_calibration.segments_.push_back(DHSegment(0.5 , 0 , 0.1 , 0.2));
my_robot_calibration.segments_.push_back(DHSegment(0.8 , 0 , 0.3 , 0.4));
my_robot_calibration.segments_.push_back(DHSegment(-1.3 , 0 , 0 , 0.0));
my_robot_calibration.segments_.push_back(DHSegment(0 , 0 , 0 , 0.0));
my_robot_calibration.segments_.push_back(DHSegment(0 , 0 , 0 , 0.0));
// clang-format on
Calibration calibration(my_robot + my_robot_calibration);
KDL::Chain robot_chain = calibration.getChain();
uint32_t num_jts = robot_chain.getNrOfJoints();
// First let's see, whether our calibration input does make sense.
{
Eigen::Matrix<double, 6, 1> jointvalues;
jointvalues << 0, 0, 0, 0, 0, 0;
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
EXPECT_PRED3(isApproximately, -1.25672673098643, fk(0, 3), 1e-11);
EXPECT_PRED3(isApproximately, -0.320928557210126, fk(1, 3), 1e-11);
EXPECT_PRED3(isApproximately, 0.158086917698569, fk(2, 3), 1e-11);
}
KDL::ChainFkSolverPos_recursive fk_solver(robot_chain);
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
KDL::Frame result_original;
fk_solver.JntToCart(jointpositions, result_original);
KDL::Chain robot_chain_corrected = calibration.correctChain();
KDL::ChainFkSolverPos_recursive fk_solver_corrected(robot_chain_corrected);
KDL::Frame result_corrected;
fk_solver.JntToCart(jointpositions, result_corrected);
//
// Check whether our internal KDL representation gives correct values
EXPECT_DOUBLE_EQ(result_original.p.x(), result_corrected.p.x());
EXPECT_DOUBLE_EQ(result_original.p.y(), result_corrected.p.y());
EXPECT_DOUBLE_EQ(result_original.p.z(), result_corrected.p.z());
calibration.correctChain();
{
Eigen::Matrix<double, 6, 1> jointvalues;
jointvalues << 0, 0, 0, 0, 0, 0;
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
EXPECT_PRED3(isApproximately, -1.25672673098643, fk(0, 3), 1e-11);
EXPECT_PRED3(isApproximately, -0.320928557210126, fk(1, 3), 1e-11);
EXPECT_PRED3(isApproximately, 0.158086917698569, fk(2, 3), 1e-11);
}
}
} // namespace