From cdd75df90f366b163b1b69fe8fc23c0e57ecedce Mon Sep 17 00:00:00 2001 From: Peter Klausing Date: Wed, 20 Oct 2021 10:41:34 +0200 Subject: [PATCH 1/8] fix include catch2/catch.hpp to catch.hpp --- test/path-test.cpp | 2 +- test/unit-test.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/test/path-test.cpp b/test/path-test.cpp index 99a2c4db..4706e693 100644 --- a/test/path-test.cpp +++ b/test/path-test.cpp @@ -1,7 +1,7 @@ #define CATCH_CONFIG_MAIN #include -#include +#include #include #include diff --git a/test/unit-test.cpp b/test/unit-test.cpp index bdaa81cc..51079486 100644 --- a/test/unit-test.cpp +++ b/test/unit-test.cpp @@ -1,7 +1,7 @@ #define CATCH_CONFIG_MAIN #include -#include +#include #include From 3c7c022f1f6b3ae4cbe0e833da99522a7d13f4b7 Mon Sep 17 00:00:00 2001 From: Peter Klausing Date: Wed, 20 Oct 2021 10:45:17 +0200 Subject: [PATCH 2/8] add function-test to test the whole library Movements --- CMakeLists.txt | 13 +++++-- test/function-test.cpp | 84 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 94 insertions(+), 3 deletions(-) create mode 100644 test/function-test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2eda4988..e01f646f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ option(BUILD_EXAMPLES "Build example programs" ON) option(BUILD_PYTHON_MODULE "Build python module" ON) option(BUILD_TESTS "Build tests" ON) option(USE_PYTHON_EXTENSION "Use python in frankx library" ON) - +set(TEST_ROBOT_HOSTNAME "" CACHE STRING "Hostname of Franka Emika Panda to run tests on") find_package(Eigen3 3.3.7 REQUIRED NO_MODULE) find_package(Franka 0.8 REQUIRED) @@ -77,12 +77,19 @@ if(BUILD_TESTS) enable_testing() find_package(Catch2 REQUIRED) - - foreach(test IN ITEMS kinematics-test unit-test path-test) + set(FRANKX_TESTS "kinematics-test" "unit-test" "path-test") + string(COMPARE EQUAL TEST_ROBOT_HOSTNAME "" TEST_ROBOT_HOSTNAME_CMP) + if(NOT TEST_ROBOT_HOSTNAME_CMP) + set(FRANKX_TESTS ${FRANKX_TESTS} "function-test") + endif() + foreach(test IN ITEMS ${FRANKX_TESTS}) add_executable(${test} "test/${test}.cpp") target_link_libraries(${test} PRIVATE frankx Catch2::Catch2) add_test(NAME ${test} COMMAND ${test}) endforeach() + if(NOT TEST_ROBOT_HOSTNAME_CMP) + target_compile_definitions(function-test PUBLIC TEST_ROBOT_HOSTNAME="${TEST_ROBOT_HOSTNAME}") + endif() endif() diff --git a/test/function-test.cpp b/test/function-test.cpp new file mode 100644 index 00000000..0de7d774 --- /dev/null +++ b/test/function-test.cpp @@ -0,0 +1,84 @@ +#define CATCH_CONFIG_MAIN +#include + +#include "frankx/frankx.hpp" +#include + +// this epsilon should be reflected, as Franka Emika is not perfect. +constexpr double epsilon = 0.001; + +void testAffineApprox(affx::Affine a, affx::Affine b) +{ + REQUIRE(std::abs(a.x() - b.x()) < epsilon); + REQUIRE(std::abs(a.y() - b.y()) < epsilon); + REQUIRE(std::abs(a.z() - b.z()) < epsilon); + REQUIRE(std::abs(a.qW() - b.qW()) < epsilon); + REQUIRE(std::abs(a.qX() - b.qX()) < epsilon); + REQUIRE(std::abs(a.qY() - b.qY()) < epsilon); + REQUIRE(std::abs(a.qZ() - b.qZ()) < epsilon); +} + +TEST_CASE("Test ForwardKinematics") +{ + try { + frankx::Robot robot(TEST_ROBOT_HOSTNAME); + affx::Affine currentAffine = robot.currentPose(); + std::array currentJointPose = robot.currentJointPositions(); + affx::Affine calcCurrentAffine = robot.forwardKinematics(currentJointPose); + testAffineApprox(currentAffine, calcCurrentAffine); + } + catch (...) + { + REQUIRE(false); + } +} + +TEST_CASE("Test JointMotion") +{ + std::array testJointPose {-0.01981415, -1.036409, -0.05556389, -2.023421, 0.01193091, 1.796796, 1.770148}; + try { + frankx::Robot robot(TEST_ROBOT_HOSTNAME, 0.5); + movex::JointMotion targetJointMotion({testJointPose}); + robot.move(targetJointMotion); + std::array current = robot.currentJointPositions(); + std::cout << robot.currentPose().toString() << std::endl; + for(auto i = 0; i < 7; i++) + { + REQUIRE(std::abs(current.at(i) - testJointPose.at(i)) < epsilon); + } + } + catch (...) + { + REQUIRE(false); + } +} + +std::stringstream AffineToString(affx::Affine a) +{ + std::stringstream ss; + ss << " x: " << a.x() << " y: " << a.y() << " z: " << a.z() + << " q_w: " << a.qW() << " q_x: " << a.qX() + << " q_y: " << a.qY() << " q_z: " << a.qZ(); + return ss; +} + +TEST_CASE("Test WaypointMotion") +{ +// x: 0.181861 y: -0.0318449 z: 0.835031 q_w: 0.557629 q_x: 0.316947 q_y: -0.235395 q_z: -0.730194 + + affx::Affine testAffine {0.182218, -0.032166, 0.834696, -1.752703, 0.200432, -2.349596}; + try { + frankx::Robot robot(TEST_ROBOT_HOSTNAME, 0.5); + movex::Waypoint waypoint{testAffine}; + movex::WaypointMotion targetWaypointMotion({waypoint}); + robot.move(targetWaypointMotion); + affx::Affine currentAffine = robot.currentPose(); + std::cout << "testAffine: " << AffineToString(currentAffine).str() << std::endl; + std::cout << "currentAffine: " << AffineToString(currentAffine).str() << std::endl; + testAffineApprox(currentAffine, testAffine); + } + catch (...) + { + REQUIRE(false); + } +} From 0241481bfa06f1acf68392fc6fc64d1b9ef00715 Mon Sep 17 00:00:00 2001 From: Peter Klausing Date: Wed, 20 Oct 2021 16:38:00 +0200 Subject: [PATCH 3/8] revert catch2 include fix --- test/function-test.cpp | 2 +- test/kinematics-test.cpp | 2 +- test/path-test.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/test/function-test.cpp b/test/function-test.cpp index 0de7d774..c7f55f6f 100644 --- a/test/function-test.cpp +++ b/test/function-test.cpp @@ -1,5 +1,5 @@ #define CATCH_CONFIG_MAIN -#include +#include #include "frankx/frankx.hpp" #include diff --git a/test/kinematics-test.cpp b/test/kinematics-test.cpp index 0a0cd36a..a780b1f1 100644 --- a/test/kinematics-test.cpp +++ b/test/kinematics-test.cpp @@ -4,7 +4,7 @@ #include #include - +#include using namespace frankx; diff --git a/test/path-test.cpp b/test/path-test.cpp index 4706e693..99a2c4db 100644 --- a/test/path-test.cpp +++ b/test/path-test.cpp @@ -1,7 +1,7 @@ #define CATCH_CONFIG_MAIN #include -#include +#include #include #include From 1abbc031734e7a2b1afa56a00bcf8f8dcaf0cebe Mon Sep 17 00:00:00 2001 From: Peter Klausing Date: Wed, 20 Oct 2021 16:39:42 +0200 Subject: [PATCH 4/8] add test to evaluate kinematics implementation --- test/kinematics-test.cpp | 71 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 70 insertions(+), 1 deletion(-) diff --git a/test/kinematics-test.cpp b/test/kinematics-test.cpp index a780b1f1..81605cda 100644 --- a/test/kinematics-test.cpp +++ b/test/kinematics-test.cpp @@ -1,3 +1,4 @@ +#define CATCH_CONFIG_MAIN #include #include @@ -8,8 +9,76 @@ using namespace frankx; +// vector of corresponding pairs Affine (XYZ+Quaternion) and JointPositions +std::vector, std::array>> test_poses + { + { + {0.01291572, -0.219187, 0.5566107, 0.01460959, 0.9677397, -0.2340099, -0.09222686}, + {0.4401378, -1.720145, -0.4573823, -2.394063, -0.6454046, 0.5917417, 0.4929689} + }, + { + {0.1170532, -0.06754467, 0.6470401, -0.406576, 0.7517108, -0.3960603, 0.3358022}, + {0.3027361, -1.755932, -0.3099454, -2.759472, -0.1217491, 2.112061, 1.139796} + }, + { + {0.1146934, -0.08584384, 0.5948072, 0.1094964, 0.8487899, -0.2910642, 0.4276072}, + {-0.4999438, -1.700211, -0.06022732, -2.897249, -0.1245556, 2.109992, 0.1506971} + } + }; -int main(int argc, char *argv[]) { + +std::string affineToString(affx::Affine a) +{ + std::stringstream ss; + ss << std::setprecision(5); + ss << " x: " << a.x() << " y: " << a.y() << " z: " << a.z() + << " q_w: " << a.qW() << " q_x: " << a.qX() + << " q_y: " << a.qY() << " q_z: " << a.qZ() + << " a: " << a.a() << " b: " << a.b() << " c: " << a.c(); + return ss.str(); +} + +TEST_CASE("Test Kinematics forward") +{ + std::cout << "forward" << std::endl; + for (auto pose: test_poses) + { + Affine correct = Affine( + pose.first[0], pose.first[1], + pose.first[2], + pose.first[3], pose.first[4], + pose.first[5], pose.first[6]); + const Eigen::Matrix q_current = + Eigen::Map>(pose.second.data(), pose.second.size()); + Affine calc = Affine(frankx::Kinematics::forward(q_current)); + std::cout << "correct: " << affineToString(correct) << std::endl; + std::cout << "calc: " << affineToString(calc) << std::endl; + std::cout << "angularDistance" << correct.quaternion().angularDistance(calc.quaternion()) << std::endl; + REQUIRE(calc.isApprox(correct)); + } +} + +TEST_CASE("Test Kinematics forwardEuler") +{ + std::cout << "forwardEuler" << std::endl; + for (auto pose: test_poses) + { + Affine correct = Affine( + pose.first[0], pose.first[1], + pose.first[2], + pose.first[3], pose.first[4], + pose.first[5], pose.first[6]); + const Eigen::Matrix q_current = + Eigen::Map>(pose.second.data(), pose.second.size()); + Affine calc = Affine(frankx::Kinematics::forwardEuler(q_current)); + std::cout << "correct: " << affineToString(correct) << std::endl; + std::cout << "calc: " << affineToString(calc) << std::endl; + std::cout << "angularDistance" << correct.quaternion().angularDistance(calc.quaternion()) << std::endl; + REQUIRE(calc.isApprox(correct)); + } +} + +TEST_CASE("Test Kinematics inverse duration"){ std::array q = {-1.4554923355, 1.1540154275, 1.50061583024, -2.30909621308, -1.3141626213, 1.93919787437, 0.028150367940}; std::array x = {0.473971, -0.307686, 0.340767, 0.545131, -0.510650, -0.552355}; From 11c82a1caae86911f7cf8308233cb85f540bd388 Mon Sep 17 00:00:00 2001 From: Peter Klausing Date: Wed, 27 Oct 2021 12:40:19 +0200 Subject: [PATCH 5/8] update tested taget Affine in WaypointMotion Test function-tests --- test/function-test.cpp | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/test/function-test.cpp b/test/function-test.cpp index c7f55f6f..43ce4ba3 100644 --- a/test/function-test.cpp +++ b/test/function-test.cpp @@ -7,6 +7,15 @@ // this epsilon should be reflected, as Franka Emika is not perfect. constexpr double epsilon = 0.001; +std::string AffineToString(affx::Affine a) +{ + std::stringstream ss; + ss << " x: " << a.x() << " y: " << a.y() << " z: " << a.z() + << " q_w: " << a.qW() << " q_x: " << a.qX() + << " q_y: " << a.qY() << " q_z: " << a.qZ(); + return ss.str(); +} + void testAffineApprox(affx::Affine a, affx::Affine b) { REQUIRE(std::abs(a.x() - b.x()) < epsilon); @@ -53,28 +62,18 @@ TEST_CASE("Test JointMotion") } } -std::stringstream AffineToString(affx::Affine a) -{ - std::stringstream ss; - ss << " x: " << a.x() << " y: " << a.y() << " z: " << a.z() - << " q_w: " << a.qW() << " q_x: " << a.qX() - << " q_y: " << a.qY() << " q_z: " << a.qZ(); - return ss; -} TEST_CASE("Test WaypointMotion") { -// x: 0.181861 y: -0.0318449 z: 0.835031 q_w: 0.557629 q_x: 0.316947 q_y: -0.235395 q_z: -0.730194 - - affx::Affine testAffine {0.182218, -0.032166, 0.834696, -1.752703, 0.200432, -2.349596}; + affx::Affine testAffine {0.182016, -0.0320245, 0.834567, -0.236185, -0.382481, 0.0959011, 0.888104}; try { frankx::Robot robot(TEST_ROBOT_HOSTNAME, 0.5); movex::Waypoint waypoint{testAffine}; movex::WaypointMotion targetWaypointMotion({waypoint}); robot.move(targetWaypointMotion); affx::Affine currentAffine = robot.currentPose(); - std::cout << "testAffine: " << AffineToString(currentAffine).str() << std::endl; - std::cout << "currentAffine: " << AffineToString(currentAffine).str() << std::endl; + std::cout << "testAffine: " << AffineToString(currentAffine) << std::endl; + std::cout << "currentAffine: " << AffineToString(currentAffine) << std::endl; testAffineApprox(currentAffine, testAffine); } catch (...) From 8a09bb87f125e84815bad49ae4a11e3a16848b7b Mon Sep 17 00:00:00 2001 From: Peter Klausing Date: Wed, 27 Oct 2021 12:41:05 +0200 Subject: [PATCH 6/8] Use movex::forward_chain instead of custom implementation --- src/frankx/robot.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/frankx/robot.cpp b/src/frankx/robot.cpp index 2df17523..66d1a9b0 100644 --- a/src/frankx/robot.cpp +++ b/src/frankx/robot.cpp @@ -60,8 +60,7 @@ std::array Robot::currentJointPositions() { } Affine Robot::forwardKinematics(const std::array& q) { - const Eigen::Matrix q_current = Eigen::Map>(q.data(), q.size()); - return Affine(Kinematics::forward(q_current)); + return kinematics.forward_chain(q); } std::array Robot::inverseKinematics(const Affine& target, const std::array& q0) { From 8844afbec8800344b1fa54c8b6204decb305e272 Mon Sep 17 00:00:00 2001 From: Peter Klausing Date: Wed, 1 Dec 2021 13:47:01 +0100 Subject: [PATCH 7/8] fix kinematics in franka::Robot * adapt Waypoint test function-test.cpp * add test to kinematics-test.cpp --- include/frankx/robot.hpp | 11 +++++++++-- test/function-test.cpp | 3 ++- test/kinematics-test.cpp | 32 ++++++++++++++++++++++++++++++++ 3 files changed, 43 insertions(+), 3 deletions(-) diff --git a/include/frankx/robot.hpp b/include/frankx/robot.hpp index 4cdf1da8..94434387 100644 --- a/include/frankx/robot.hpp +++ b/include/frankx/robot.hpp @@ -31,8 +31,15 @@ namespace frankx { class Robot: public franka::Robot { // Modified DH-parameters: alpha, d, a const KinematicChain<7> kinematics = KinematicChain<7>( - {{{0.0, 0.333, 0.0}, {-M_PI/2, 0.0, 0.0}, {M_PI/2, 0.316, 0.0}, {M_PI/2, 0.0, 0.0825}, {-M_PI/2, 0.384, -0.0825}, {M_PI/2, 0.0, 0.0}, {M_PI/2, 0.0, 0.088}}}, - Affine(0, 0, 0.107, M_PI/4, 0, M_PI) + {{ + {0.0, 0.333, 0.0}, + {-M_PI/2, 0.0, 0.0}, + {M_PI/2, 0.316, 0.0}, + {M_PI/2, 0.0, 0.0825}, + {-M_PI/2, 0.384, -0.0825}, + {M_PI/2, 0.0, 0.0}, + {M_PI/2, 0.0, 0.088}}}, + Affine(0, 0, 0.107, 0, 0, 0) ); public: diff --git a/test/function-test.cpp b/test/function-test.cpp index 43ce4ba3..4c67e0e2 100644 --- a/test/function-test.cpp +++ b/test/function-test.cpp @@ -65,7 +65,8 @@ TEST_CASE("Test JointMotion") TEST_CASE("Test WaypointMotion") { - affx::Affine testAffine {0.182016, -0.0320245, 0.834567, -0.236185, -0.382481, 0.0959011, 0.888104}; + affx::Affine testAffine {0.182078, -0.0319788, 0.8347213, 0.316669, -0.557494, 0.730278, -0.235824}; + try { frankx::Robot robot(TEST_ROBOT_HOSTNAME, 0.5); movex::Waypoint waypoint{testAffine}; diff --git a/test/kinematics-test.cpp b/test/kinematics-test.cpp index 81605cda..72fa1585 100644 --- a/test/kinematics-test.cpp +++ b/test/kinematics-test.cpp @@ -58,6 +58,38 @@ TEST_CASE("Test Kinematics forward") } } +TEST_CASE("Test Kinematics forward1") +{ + std::cout << "forward" << std::endl; + for (auto pose: test_poses) + { + Affine correct = Affine( + pose.first[0], pose.first[1], + pose.first[2], + pose.first[3], pose.first[4], + pose.first[5], pose.first[6]); + const Eigen::Matrix q_current = + Eigen::Map>(pose.second.data(), pose.second.size()); + const KinematicChain<7> kinematics = KinematicChain<7>( + {{ + {0.0, 0.333, 0.0}, + {-M_PI/2, 0.0, 0.0}, + {M_PI/2, 0.316, 0.0}, + {M_PI/2, 0.0, 0.0825}, + {-M_PI/2, 0.384, -0.0825}, + {M_PI/2, 0.0, 0.0}, + {M_PI/2, 0.0, 0.088}}}, + Affine(0, 0, 0.107, 0, 0, 0) + ); + + Affine calc = kinematics.forward_chain(pose.second); + std::cout << "correct: " << affineToString(correct) << std::endl; + std::cout << "calc: " << affineToString(calc) << std::endl; + std::cout << "angularDistance" << correct.quaternion().angularDistance(calc.quaternion()) << std::endl; +// REQUIRE(calc.isApprox(correct)); + } +} + TEST_CASE("Test Kinematics forwardEuler") { std::cout << "forwardEuler" << std::endl; From 2b40a1cd6f0e1a1095b54c2291c5613eddc0502b Mon Sep 17 00:00:00 2001 From: Peter Klausing Date: Wed, 1 Dec 2021 13:47:52 +0100 Subject: [PATCH 8/8] add error recovery from robot --- test/function-test.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/test/function-test.cpp b/test/function-test.cpp index 4c67e0e2..bab92a43 100644 --- a/test/function-test.cpp +++ b/test/function-test.cpp @@ -47,6 +47,8 @@ TEST_CASE("Test JointMotion") std::array testJointPose {-0.01981415, -1.036409, -0.05556389, -2.023421, 0.01193091, 1.796796, 1.770148}; try { frankx::Robot robot(TEST_ROBOT_HOSTNAME, 0.5); + if (robot.hasErrors()) + robot.automaticErrorRecovery(); movex::JointMotion targetJointMotion({testJointPose}); robot.move(targetJointMotion); std::array current = robot.currentJointPositions(); @@ -69,6 +71,8 @@ TEST_CASE("Test WaypointMotion") try { frankx::Robot robot(TEST_ROBOT_HOSTNAME, 0.5); + if (robot.hasErrors()) + robot.automaticErrorRecovery(); movex::Waypoint waypoint{testAffine}; movex::WaypointMotion targetWaypointMotion({waypoint}); robot.move(targetWaypointMotion);