mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-12 11:00:47 +02:00
Use plain Eigen for kinematics
This commit is contained in:
Felix Mauch
@@ -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());
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user