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/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/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) { diff --git a/test/function-test.cpp b/test/function-test.cpp new file mode 100644 index 00000000..bab92a43 --- /dev/null +++ b/test/function-test.cpp @@ -0,0 +1,88 @@ +#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; + +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); + 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); + if (robot.hasErrors()) + robot.automaticErrorRecovery(); + 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); + } +} + + +TEST_CASE("Test WaypointMotion") +{ + 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); + if (robot.hasErrors()) + robot.automaticErrorRecovery(); + movex::Waypoint waypoint{testAffine}; + movex::WaypointMotion targetWaypointMotion({waypoint}); + robot.move(targetWaypointMotion); + affx::Affine currentAffine = robot.currentPose(); + std::cout << "testAffine: " << AffineToString(currentAffine) << std::endl; + std::cout << "currentAffine: " << AffineToString(currentAffine) << std::endl; + testAffineApprox(currentAffine, testAffine); + } + catch (...) + { + REQUIRE(false); + } +} diff --git a/test/kinematics-test.cpp b/test/kinematics-test.cpp index 0a0cd36a..72fa1585 100644 --- a/test/kinematics-test.cpp +++ b/test/kinematics-test.cpp @@ -1,15 +1,116 @@ +#define CATCH_CONFIG_MAIN #include #include #include #include - +#include 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} + } + }; + + +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 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; + 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)); + } +} -int main(int argc, char *argv[]) { +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}; 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