diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index d149caed3a25294d59c55b61cd6d19294dcfc8b9..69ae26e2827892198b047ea9396901f56d14b866 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -117,22 +117,23 @@ namespace simox::geometric_planning
.toRotationMatrix());
// ARMARX_DEBUG << VAROUT(joint_T_joint_ref.linear());
- const Pose global_T_joint =
- global_T_joint_orig *
- joint_T_joint_ref; // * Pose(Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), revoluteJoint->getJointRotationAxis()).toRotationMatrix());
+ const Pose global_T_joint_ref = global_T_joint_orig * joint_T_joint_ref;
+ // * Pose(Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), revoluteJoint->getJointRotationAxis()).toRotationMatrix());
const Pose global_T_node(node->getGlobalPose());
// this is the node within the joint frame
- const Pose joint_T_node = global_T_joint.inverse() * global_T_node;
+ // joint_ref_T_node = joint_ref_T_global * global_T_node
+ const Pose joint_ref_T_node = global_T_joint_ref.inverse() * global_T_node;
+
// ARMARX_DEBUG << "relative position: " << joint_T_node.translation();
- // the plane in which the node moves is given by the following
+ // the plane in which the node moves is given by the following (x, y coordinates = 0 for movement plane)
const Pose global_T_joint_plane =
- global_T_joint * Eigen::Translation3f{0, 0, joint_T_node.translation().z()};
+ global_T_joint_ref * Eigen::Translation3f{0, 0, joint_ref_T_node.translation().z()};
// and the radius of the movement is given by the xy coordinate
- const float radius = joint_T_node.translation().head<2>().norm();
+ const float radius = joint_ref_T_node.translation().head<2>().norm();
// ARMARX_DEBUG << "Radius: " << radius;
REQUIRE_MESSAGE(joint->getJointLimitHigh() > joint->getJointLimitLow(),
@@ -151,7 +152,8 @@ namespace simox::geometric_planning
const std::string nodeJointReference = node->getName() + "_joint_reference";
- const Pose joint_T_joint_plane = global_T_joint.inverse() * global_T_joint_plane;
+ // joint_T_joint_plane = joint_ref_T_global * global_t_joint_plane
+ const Pose joint_ref_T_joint_plane = global_T_joint_ref.inverse() * global_T_joint_plane;
// now we make sure that the joint coordinate system is oriented such that the x axis points towards the initial node position
const Eigen::Vector3f global__joint_plane_P_node_initial =
@@ -171,9 +173,9 @@ namespace simox::geometric_planning
const Pose global_T_root(subpart->getGlobalPose());
const Pose root_T_global = global_T_root.inverse();
- const Pose root_T_joint = root_T_global * global_T_joint;
+ const Pose root_T_joint_ref = root_T_global * global_T_joint_ref;
- const Pose root_T_joint_reference(root_T_joint * joint_T_joint_plane *
+ const Pose root_T_joint_reference(root_T_joint_ref * joint_ref_T_joint_plane *
joint_plane_T_joint_reference);
const auto jointReferenceNode = std::make_shared<VirtualRobot::RobotNodeFixed>(
@@ -190,10 +192,18 @@ namespace simox::geometric_planning
// reset joint state
joint->setJointValue(initialJointValue);
+ // we need the transformation from root_T_joint_reference to node, so that the orientations match up.
+ // without the orientation adaption, this will output a coordinate system with x pointing in the
+ // direction from global_T_plane to node, and z pointing upward.
+ // therefore:
+ // global_T_root * root_T_joint_reference = global_T_joint_reference
+ // we just need the linear transformation though (i think), because the translation was already covered before
+ Pose joint_reference_T_node(
+ ((global_T_root * root_T_joint_reference).inverse() * global_T_node).linear());
return {jointReferenceNode,
std::make_unique<CircleSegment>(radius, parameterRange),
- Pose::Identity()};
+ joint_reference_T_node};
}
ParametricPath
@@ -261,9 +271,8 @@ namespace simox::geometric_planning
// ARMARX_DEBUG << "registered robot node `" << jointReferenceNode->getName() << "`";
- return ParametricPath(jointReferenceNode,
- std::make_unique<Line>(parameterRange),
- Pose::Identity());
+ return ParametricPath(
+ jointReferenceNode, std::make_unique<Line>(parameterRange), Pose::Identity());
}
ParametricPath
diff --git a/GeometricPlanning/CMakeLists.txt b/GeometricPlanning/CMakeLists.txt
index 371f3f52179e1b97ec1029cbb43d87dfef8f073b..e93497798bc398edb1fe067e0140cf6edd87b467 100644
--- a/GeometricPlanning/CMakeLists.txt
+++ b/GeometricPlanning/CMakeLists.txt
@@ -114,9 +114,10 @@ endif()
if(BUILD_TESTING)
# include unit tests
- # ADD_SUBDIRECTORY(tests/)
+ ADD_SUBDIRECTORY(tests)
endif()
+
#######################################################################################
############################ Setup for installation ###################################
#######################################################################################
diff --git a/GeometricPlanning/tests/AOGPHelper.cpp b/GeometricPlanning/tests/AOGPHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5c92dc75cbd30b585c21db9f4c33634e28c446e1
--- /dev/null
+++ b/GeometricPlanning/tests/AOGPHelper.cpp
@@ -0,0 +1,176 @@
+
+#include <filesystem>
+
+#include "VirtualRobot/XML/RobotIO.h"
+#define BOOST_TEST_MODULE GeometricPlanning_AOGPHelper
+
+#include <boost/test/unit_test.hpp>
+
+#include "../ArticulatedObjectGeometricPlanningHelper.h"
+
+BOOST_AUTO_TEST_SUITE(ArticulatedObjectGeometricPlanningHelper)
+
+static constexpr const char* scDataDir =
+ "/common/homes/students/hoefer/workspace/base/simox-control/data/";
+
+static constexpr float precision = 0.00001;
+static constexpr size_t nSamples = 50;
+
+static std::filesystem::path
+getPath(const std::string& model)
+{
+ return std::filesystem::path(scDataDir) / "Model" / "mobile_kitchen" / model /
+ (model + "_articulated.xml");
+}
+
+VirtualRobot::RobotPtr
+load_robot(const std::string& model)
+{
+ // TODO(Hawo): somehow input a simple robot model
+ const auto path = getPath(model);
+ BOOST_REQUIRE(std::filesystem::exists(path));
+
+ auto robot = VirtualRobot::RobotIO::loadRobot(path);
+ BOOST_REQUIRE(robot != nullptr);
+
+ return robot;
+}
+
+using AOGPHelper = simox::geometric_planning::ArticulatedObjectGeometricPlanningHelper;
+
+static void
+printMat(const Eigen::MatrixXf& mat)
+{
+ for (int row = 0; row < mat.rows(); ++row)
+ {
+ for (int col = 0; col < mat.cols(); ++col)
+ {
+ float val = mat(row, col);
+ val = std::fabs(val) < precision ? 0 : val;
+ std::cout << val << ",";
+ }
+ std::cout << "\n";
+ }
+}
+BOOST_AUTO_TEST_CASE(AOGPHelper_circle_first_path_element)
+{
+ const std::string articulatedObjectName = "mobile-fridge";
+ const std::string handleName = "Fridge_handle";
+ auto robot = load_robot(articulatedObjectName);
+
+ auto helper = AOGPHelper(robot);
+ auto path = helper.getPathForNode(handleName);
+ auto range = path.parameterRange();
+
+ Eigen::Matrix4f pathPose = path.getPose(range.min).matrix();
+ Eigen::Matrix4f objectPose = robot->getRobotNode(handleName)->getGlobalPose();
+
+ Eigen::Matrix4f diff = objectPose.inverse() * pathPose - Eigen::Matrix4f::Identity();
+ float sse = diff.array().square().sum();
+ if (sse > precision)
+ {
+ std::cout << "Object Pose (target):\n";
+ std::cout << objectPose << "\n";
+ std::cout << "Path Pose:\n";
+ std::cout << pathPose << "\n";
+ printMat(diff);
+ }
+ BOOST_REQUIRE(sse < precision);
+}
+
+
+BOOST_AUTO_TEST_CASE(AOGPHelper_circle_path_whole_around_z)
+{
+ const std::string articulatedObjectName = "mobile-fridge";
+ const std::string handleName = "Fridge_handle";
+ const std::string jointName = "Fridge_joint";
+ auto robot = load_robot(articulatedObjectName);
+
+ auto helper = AOGPHelper(robot);
+ auto path = helper.getPathForNode(handleName);
+ auto range = path.parameterRange();
+
+
+ for (size_t i = 0; i < nSamples; ++i)
+ {
+ float t = range.min + (range.max - range.min) / nSamples * i;
+ Eigen::Matrix4f pathPose = path.getPose(t).matrix();
+
+ robot->setJointValue(jointName, t);
+ Eigen::Matrix4f objectPose = robot->getRobotNode(handleName)->getGlobalPose();
+
+ Eigen::Matrix4f diff = objectPose.inverse() * pathPose - Eigen::Matrix4f::Identity();
+ float sse = diff.array().square().sum();
+ if (sse > precision)
+ {
+ std::cout << "SSE at " << t << ": " << sse << "\n";
+ printMat(diff);
+ }
+ BOOST_REQUIRE(sse < precision);
+ }
+}
+
+BOOST_AUTO_TEST_CASE(AOGPHelper_circle_path_whole_around_y)
+{
+ const std::string articulatedObjectName = "mobile-dishwasher";
+ const std::string handleName = "Dishwasher->Dishwasher_handle";
+ const std::string jointName = "Dishwasher_joint";
+ auto robot = load_robot(articulatedObjectName);
+
+ auto helper = AOGPHelper(robot);
+ auto path = helper.getPathForNode(handleName);
+ auto range = path.parameterRange();
+
+
+ for (size_t i = 0; i < nSamples; ++i)
+ {
+ float t = range.min + (range.max - range.min) / nSamples * i;
+ Eigen::Matrix4f pathPose = path.getPose(t).matrix();
+
+ robot->setJointValue(jointName, t);
+ Eigen::Matrix4f objectPose = robot->getRobotNode(handleName)->getGlobalPose();
+
+ Eigen::Matrix4f diff = objectPose.inverse() * pathPose - Eigen::Matrix4f::Identity();
+ float sse = diff.array().square().sum();
+ if (sse > precision)
+ {
+ std::cout << "SSE at " << t << ": " << sse << "\n";
+ printMat(diff);
+ }
+ BOOST_REQUIRE(sse < precision);
+ }
+}
+
+BOOST_AUTO_TEST_CASE(AOGPHelper_linear_path_whole)
+{
+ const std::string articulatedObjectName = "mobile-kitchen-counter";
+ const std::string handleName = "DrawerMiddle_handle";
+ const std::string jointName = "DrawerMiddle_joint";
+ auto robot = load_robot(articulatedObjectName);
+
+ auto helper = AOGPHelper(robot);
+ auto path = helper.getPathForNode(handleName);
+ auto range = path.parameterRange();
+
+
+ for (size_t i = 0; i < nSamples; ++i)
+ {
+ float t = range.min + (range.max - range.min) / nSamples * i;
+ Eigen::Matrix4f pathPose = path.getPose(t).matrix();
+
+ robot->setJointValue(jointName, t);
+ Eigen::Matrix4f objectPose = robot->getRobotNode(handleName)->getGlobalPose();
+
+ Eigen::Matrix4f diff = objectPose.inverse() * pathPose - Eigen::Matrix4f::Identity();
+ float sse = diff.array().square().sum();
+ if (sse > precision)
+ {
+ std::cout << "SSE at " << t << ": " << sse << "\n";
+ printMat(diff);
+ }
+ BOOST_REQUIRE(sse < precision);
+ }
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/GeometricPlanning/tests/CMakeLists.txt b/GeometricPlanning/tests/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8baade388d86e9278fea99592cb2e7dae381f21e
--- /dev/null
+++ b/GeometricPlanning/tests/CMakeLists.txt
@@ -0,0 +1 @@
+ADD_GEOMETRIC_PLANNING_TEST(AOGPHelper)