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);
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);

16
test/calibration_test.cpp
View File

@@ -15,14 +15,15 @@
#include <gtest/gtest.h>
#include <ur_rtde_driver/calibration.h>
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();
}