diff --git a/src/calibration.cpp b/src/calibration.cpp index d690484..3fbeef5 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -97,9 +97,9 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) 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()); + //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()); + //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); @@ -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 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); } @@ -180,13 +180,13 @@ KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const { 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()); + //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()); + //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); diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index f706e8c..395903c 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -15,14 +15,15 @@ #include #include +namespace +{ TEST(UrRtdeDriver, ur10_ideal) { DHRobot my_robot; - const double pi = std::atan(1)*4; + const double pi = std::atan(1) * 4; // This is an ideal UR10 // 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 , -0.612 , 0 , 0)); my_robot.segments_.push_back(DHSegment(0 , -0.5723, 0 , 0.0)); @@ -50,7 +51,7 @@ TEST(UrRtdeDriver, ur10_ideal) TEST(UrRtdeDriver, calibration) { DHRobot my_robot; - const double pi = std::atan(1)*4; + const double pi = std::atan(1) * 4; // This is an ideal UR10 // clang-format off @@ -89,20 +90,15 @@ TEST(UrRtdeDriver, calibration) fk_solver.JntToCart(jointpositions, result_corrected); // // 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.y(), result_corrected.p.y()); EXPECT_DOUBLE_EQ(result_original.p.z(), result_corrected.p.z()); } +} // namespace int main(int argc, char* argv[]) { - testing::InitGoogleTest(&argc, argv); + ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }