function tests

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

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:

src/frankx/robot.cpp

@@ -60,8 +60,7 @@ std::array<double, 7> Robot::currentJointPositions() {
 }
 
 Affine Robot::forwardKinematics(const std::array<double, 7>& q) {
-    const Eigen::Matrix<double, 7, 1> q_current = Eigen::Map<const Eigen::Matrix<double, 7, 1>>(q.data(), q.size());
-    return Affine(Kinematics::forward(q_current));
+    return kinematics.forward_chain(q);
 }
 
 std::array<double, 7> Robot::inverseKinematics(const Affine& target, const std::array<double, 7>& q0) {

test/function-test.cpp

@@ -0,0 +1,88 @@
+#define CATCH_CONFIG_MAIN
+#include <catch2/catch.hpp>
+
+#include "frankx/frankx.hpp"
+#include <affx/affine.hpp>
+
+// 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<double, 7> currentJointPose = robot.currentJointPositions();
+        affx::Affine calcCurrentAffine = robot.forwardKinematics(currentJointPose);
+        testAffineApprox(currentAffine, calcCurrentAffine);
+    }
+    catch (...)
+    {
+        REQUIRE(false);
+    }
+}
+
+TEST_CASE("Test JointMotion")
+{
+    std::array<double, 7> 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<double, 7> 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);
+    }
+}

test/kinematics-test.cpp

@@ -1,15 +1,116 @@
+#define CATCH_CONFIG_MAIN
 #include <iostream>
 #include <chrono>
 
 #include <Eigen/Core>
 
 #include <frankx/frankx.hpp>
-
+#include <catch2/catch.hpp>
 
 using namespace frankx;
 
+// vector of corresponding pairs Affine (XYZ+Quaternion) and JointPositions
+std::vector<std::pair<std::array<double, 7>, std::array<double,7>>> 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<double, 7, 1> q_current =
+                Eigen::Map<const Eigen::Matrix<double, 7, 1>>(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<double, 7, 1> q_current =
+                Eigen::Map<const Eigen::Matrix<double, 7, 1>>(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<double, 7, 1> q_current =
+                Eigen::Map<const Eigen::Matrix<double, 7, 1>>(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<double, 7> q = {-1.4554923355, 1.1540154275, 1.50061583024, -2.30909621308, -1.3141626213, 1.93919787437, 0.028150367940};
     std::array<double, 6> x = {0.473971, -0.307686, 0.340767, 0.545131, -0.510650, -0.552355};
 

test/unit-test.cpp

@@ -1,7 +1,7 @@
 #define CATCH_CONFIG_MAIN
 #include <random>
 
-#include <catch2/catch.hpp>
+#include <catch.hpp>
 
 #include <frankx/frankx.hpp>