diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index 92b02612dd327bc9b80a09f5ec166e047139ccf4..d732c36477545bf42585ca586c074e9898f75058 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -124,22 +124,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(),
@@ -158,7 +159,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 =
@@ -178,9 +180,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>(
@@ -197,10 +199,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
@@ -266,11 +276,16 @@ namespace simox::geometric_planning
subpart->registerRobotNode(jointReferenceNode);
CHECK(jointReferenceNode->initialize(subpart->getRootNode()));
+
// ARMARX_DEBUG << "registered robot node `" << jointReferenceNode->getName() << "`";
+ const Eigen::Isometry3f global_T_node(node->getGlobalPose());
+ const Eigen::Isometry3f global_T_jointReference(jointReferenceNode->getGlobalPose());
+
+ const Eigen::Isometry3f jointReference_T_node = global_T_jointReference.inverse() * global_T_node;
+ // jointRef_T_node = jointRef_T_global * global_T_node
- return ParametricPath(jointReferenceNode,
- std::make_unique<Line>(parameterRange),
- Pose::Identity());
+ return ParametricPath(
+ jointReferenceNode, std::make_unique<Line>(parameterRange), jointReference_T_node);
}
ParametricPath
diff --git a/GeometricPlanning/CMakeLists.txt b/GeometricPlanning/CMakeLists.txt
index 77d85645ef611b144f628c8fd8649fcbbb9b27dc..fe29714155848bea33a1112148a7b8c007d2f224 100644
--- a/GeometricPlanning/CMakeLists.txt
+++ b/GeometricPlanning/CMakeLists.txt
@@ -116,9 +116,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..59f9b89478a8eabe52426b20ca55ae9ebe6505a1
--- /dev/null
+++ b/GeometricPlanning/tests/CMakeLists.txt
@@ -0,0 +1,3 @@
+# FIXME the following test must be adapted. It requires a model that is not available in simox
+
+# ADD_GEOMETRIC_PLANNING_TEST(AOGPHelper)
diff --git a/VirtualRobot/Manipulability/AbstractManipulability.cpp b/VirtualRobot/Manipulability/AbstractManipulability.cpp
index 8e6725d0e62a1158ec840da3615a4f3382cdbab6..fa39669db21ca4354e9953e370c6ee8abe82b539 100644
--- a/VirtualRobot/Manipulability/AbstractManipulability.cpp
+++ b/VirtualRobot/Manipulability/AbstractManipulability.cpp
@@ -108,26 +108,50 @@ VisualizationNodePtr AbstractManipulability::getManipulabilityVis(const Eigen::M
return vis;
}
-void AbstractManipulability::getEllipsoidOrientationAndScale(const Eigen::MatrixXd &manipulability, Eigen::Quaternionf &orientation, Eigen::Vector3d &scale) {
- Eigen::MatrixXd reduced_manipulability = (mode != Mode::Orientation || manipulability.rows() == 3) ? manipulability.block(0, 0, 3, 3) : manipulability.block(3, 3, 3, 3);
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(reduced_manipulability);
- Eigen::MatrixXd eigenVectors = eigenSolver.eigenvectors();
- Eigen::VectorXd eigenValues = eigenSolver.eigenvalues();
- Eigen::VectorXd v1 = eigenVectors.col(eigenVectors.cols() - 1);
- Eigen::VectorXd v2 = eigenVectors.col(eigenVectors.cols() - 2);
-
- orientation = Eigen::Quaterniond::FromTwoVectors(v1, v2).cast<float>();
+void AbstractManipulability::getEllipsoidOrientationAndScale(const Eigen::MatrixXd& manipulability, Eigen::Quaternionf& orientation, Eigen::Vector3d& scale) {
+
+ constexpr std::size_t POSITION_DIMS = 3;
+ constexpr std::size_t ORIENTATION_DIMS = 3;
+ constexpr std::size_t POSE_DIMS = POSITION_DIMS + ORIENTATION_DIMS;
+
+ const Eigen::MatrixXd reduced_manipulability =
+ (mode != Mode::Orientation || manipulability.rows() == 3)
+ ? manipulability.block(0, 0, POSITION_DIMS, POSITION_DIMS)
+ : manipulability.block(
+ POSITION_DIMS, POSITION_DIMS, ORIENTATION_DIMS, ORIENTATION_DIMS);
+ const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(reduced_manipulability);
+
+ // Eigenvectors are the columns of the matrix 'eigenVectors'
+ // they are already normalized to have length 1
+ const Eigen::MatrixXd& eigenVectors = eigenSolver.eigenvectors();
+
+ // Eigen values are sorted in increasing order
+ const Eigen::Vector3d& eigenValues = eigenSolver.eigenvalues();
+
+ // We create a ortho-normal basis of the eigen vectors.
+ // Here, we use the eigen vectors with the eigen values in decreasing order.
+ // To ensure a right-handed coordinate system, the third basis vector is computed
+ // by using the cross product.
+ Eigen::Matrix3d rotationMatrix;
+ rotationMatrix.col(0) = eigenVectors.col(2);
+ rotationMatrix.col(1) = eigenVectors.col(1);
+ rotationMatrix.col(2) = rotationMatrix.col(0).cross(rotationMatrix.col(1));
+
+ orientation = rotationMatrix;
+
+ scale = eigenValues.reverse();
// normalize singular values for scaling
- eigenValues /= eigenValues.sum();
- for (int i = 0; i < eigenValues.rows(); i++) {
- if (eigenValues(i) < 0.005) // 5mm
- eigenValues(i) = 0.005;
- }
+ scale /= scale.sum();
+ for (int i = 0; i < eigenValues.rows(); i++)
+ {
+ constexpr double minEigenVal = 0.005; // [mm]
- scale(0) = eigenValues(eigenValues.rows() - 1);
- scale(1) = eigenValues(eigenValues.rows() - 2);
- scale(2) = eigenValues(eigenValues.rows() - 3);
+ if (scale(i) < minEigenVal)
+ {
+ scale(i) = minEigenVal;
+ }
+ }
}
void AbstractManipulability::getEllipsoidOrientationAndScale(Eigen::Quaternionf &orientation, Eigen::Vector3d &scale) {