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

reduce output during testing

Browse Source
This commit is contained in:
Felix Mauch
2019-03-20 09:59:00 +01:00
parent eaae523484
commit 8a9192aeba
2 changed files with 11 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/calibration.cpp
View File

@@ -97,9 +97,9 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
KDL::JntArray jointpositions = KDL::JntArray(num_jts); KDL::JntArray jointpositions = KDL::JntArray(num_jts);
fk_solver.JntToCart(jointpositions, passive_frame, 2); fk_solver.JntToCart(jointpositions, passive_frame, 2);
ROS_INFO_STREAM(passive_frame.p.x() << ", " << passive_frame.p.y() << ", " << passive_frame.p.z()); //ROS_INFO_STREAM(passive_frame.p.x() << ", " << passive_frame.p.y() << ", " << passive_frame.p.z());
fk_solver.JntToCart(jointpositions, next_segment_frame, 3); 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()); //ROS_INFO_STREAM(next_segment_frame.p.x() << ", " << next_segment_frame.p.y() << ", " << next_segment_frame.p.z());
Eigen::Vector3d eigen_passive; Eigen::Vector3d eigen_passive;
tf::vectorKDLToEigen(passive_frame.p, eigen_passive); tf::vectorKDLToEigen(passive_frame.p, eigen_passive);
@@ -138,7 +138,7 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0; double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0;
double distance_correction = intersection_param * sign_dir; double distance_correction = intersection_param * sign_dir;
ROS_INFO_STREAM("Corrected chain at " << robot_chain.segments[0].getName()); //ROS_INFO_STREAM("Corrected chain at " << robot_chain.segments[0].getName());
return buildCorrectedChain(robot_chain, new_length, new_theta, distance_correction); return buildCorrectedChain(robot_chain, new_length, new_theta, distance_correction);
} }
@@ -180,13 +180,13 @@ KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const
{ {
KDL::Frame new_frame = robot_chain.getSegment(2).getFrameToTip(); KDL::Frame new_frame = robot_chain.getSegment(2).getFrameToTip();
new_frame.p = robot_chain.getSegment(2).getFrameToTip().p; new_frame.p = robot_chain.getSegment(2).getFrameToTip().p;
ROS_INFO_STREAM("Correcting segment i+2 length from " << new_frame.p.z()); //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 // the d-parameter can be modified by the intersection_parameter which is the distance traveled
// along the rotation axis // along the rotation axis
new_frame.p = KDL::Vector(0, 0, new_frame.p.z() - distance_correction); 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()); //ROS_INFO_STREAM("Corrected segment i+2 length to " << new_frame.p.z());
KDL::Joint new_joint = robot_chain.getSegment(2).getJoint(); KDL::Joint new_joint = robot_chain.getSegment(2).getJoint();
KDL::Segment segment = KDL::Segment(robot_chain.getSegment(2).getName(), new_joint, new_frame); KDL::Segment segment = KDL::Segment(robot_chain.getSegment(2).getName(), new_joint, new_frame);

12
test/calibration_test.cpp
View File

@@ -15,6 +15,8 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <ur_rtde_driver/calibration.h> #include <ur_rtde_driver/calibration.h>
namespace
{
TEST(UrRtdeDriver, ur10_ideal) TEST(UrRtdeDriver, ur10_ideal)
{ {
DHRobot my_robot; DHRobot my_robot;
@@ -22,7 +24,6 @@ TEST(UrRtdeDriver, ur10_ideal)
// This is an ideal UR10 // This is an ideal UR10
// clang-format off // clang-format off
// d, a, theta, alpha
my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , pi / 2));
my_robot.segments_.push_back(DHSegment(0 , -0.612 , 0 , 0)); 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 , -0.5723, 0 , 0.0));
@@ -89,20 +90,15 @@ TEST(UrRtdeDriver, calibration)
fk_solver.JntToCart(jointpositions, result_corrected); fk_solver.JntToCart(jointpositions, result_corrected);
// //
// Check whether our internal KDL representation gives correct values // Check whether our internal KDL representation gives correct values
std::cout << result_original.p.x() << std::endl;
std::cout << result_corrected.p.x() << std::endl;
std::cout << result_original.p.y() << std::endl;
std::cout << result_corrected.p.y() << std::endl;
std::cout << result_original.p.z() << std::endl;
std::cout << result_corrected.p.z() << std::endl;
EXPECT_DOUBLE_EQ(result_original.p.x(), result_corrected.p.x()); 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.y(), result_corrected.p.y());
EXPECT_DOUBLE_EQ(result_original.p.z(), result_corrected.p.z()); EXPECT_DOUBLE_EQ(result_original.p.z(), result_corrected.p.z());
} }
} // namespace
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
testing::InitGoogleTest(&argc, argv); ::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }