new geometric planning library + doctest assertions

GeometricPlanning/ArticulatedObjectDoorHelper.cpp

+#include "ArticulatedObjectDoorHelper.h"
+
+#include <string>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <GeometricPlanning/assert/assert.h>
+
+
+namespace simox::geometric_planning
+{
+    ArticulatedObjectDoorHelper::ArticulatedObjectDoorHelper(const VirtualRobot::RobotPtr& object,
+                                                             const Params& params) :
+        object(object), params(params)
+    {
+    }
+
+    ArticulatedObjectDoorHelper::DoorInteractionContext
+    ArticulatedObjectDoorHelper::planInteraction(const std::string& nodeSetName) const
+    {
+        const auto rns = object->getRobotNodeSet(nodeSetName);
+        REQUIRE_MESSAGE(rns != nullptr, "Robot node set `" << nodeSetName << "` does not exist!");
+
+        const std::string jointNodeName = nodeSetName + constants::JointSuffix;
+        const std::string handleNodeName = nodeSetName + constants::HandleSuffix;
+        const std::string surfaceProjectionNodeName = nodeSetName + constants::SurfaceSuffix;
+
+        const auto checkNodeExists = [&rns, &nodeSetName](const std::string& nodeName)
+        {
+            REQUIRE_MESSAGE(rns->hasRobotNode(nodeName),
+                            "Robot node `" << nodeName + "` does not exist within robot node set `"
+                                           << nodeSetName << "`!");
+        };
+
+        for (const auto& nodeName : {jointNodeName, handleNodeName, surfaceProjectionNodeName})
+        {
+            checkNodeExists(nodeName);
+        }
+
+        REQUIRE_MESSAGE(params.doorContactHandleDistance > 0.,
+                        "Grasping from the other side not implemented yet!");
+
+        return DoorInteractionContext{
+            .rns = {.joint = rns->getNode(jointNodeName),
+                    .handle = rns->getNode(handleNodeName),
+                    .handleSurfaceProjection = rns->getNode(surfaceProjectionNodeName)},
+            .handleSurfaceProjection_T_door_initial_contact =
+                Pose(Eigen::Translation3f{0.F, -params.doorContactHandleDistance, 0.F}),
+            .door_initial_contact_T_pre_contact =
+                Pose(Eigen::Translation3f{0.F, 0, params.preContactDistance})};
+    }
+
+    ArticulatedObjectDoorHelper::DoorInteractionContextExtended
+    ArticulatedObjectDoorHelper::planInteractionExtended(const std::string& nodeSetName,
+                                                         const Pose& global_T_tcp_in_contact) const
+    {
+        const auto interactionInfo = planInteraction(nodeSetName);
+
+        DoorInteractionContextExtended extendedInfo;
+        extendedInfo.door_initial_contact_T_pre_contact =
+            interactionInfo.door_initial_contact_T_pre_contact;
+        extendedInfo.rns = interactionInfo.rns;
+        extendedInfo.handleSurfaceProjection_T_door_initial_contact =
+            interactionInfo.handleSurfaceProjection_T_door_initial_contact;
+
+        extendedInfo.handleSurfaceProjection_T_tcp_at_handle =
+            handleSurfaceProjection_T_tcp_at_handle(interactionInfo, global_T_tcp_in_contact);
+
+        return extendedInfo;
+    }
+
+    Pose
+    ArticulatedObjectDoorHelper::handleSurfaceProjection_T_tcp_at_handle(
+        const DoorInteractionContext& interactionInfo,
+        const Pose& global_T_tcp_in_contact) const
+    {
+        const Pose global_T_handleSurfaceProjection(
+            interactionInfo.rns.handleSurfaceProjection->getGlobalPose());
+
+        // z points away from the door, y upwards, x to the side
+        const Pose door_surface_T_tcp =
+            global_T_handleSurfaceProjection.inverse() * global_T_tcp_in_contact;
+
+        // VR_INFO << VAROUT(door_surface_T_tcp.translation());
+
+        const float initialDoorDistance = door_surface_T_tcp.translation().z();
+        // VR_INFO << "Initial door distance " << initialDoorDistance;
+
+        return Pose(Eigen::Translation3f{0, 0, initialDoorDistance});
+    }
+} // namespace simox::geometric_planning

GeometricPlanning/ArticulatedObjectDoorHelper.h

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <GeometricPlanning/types.h>
+
+namespace simox::geometric_planning
+{
+
+    namespace constants
+    {
+        inline const std::string JointSuffix = "_joint";
+        inline const std::string HandleSuffix = "_handle";
+        inline const std::string SurfaceSuffix = "_handle_surface_projection";
+    } // namespace constants
+
+    class ArticulatedObjectDoorHelper
+    {
+    public:
+        struct Params
+        {
+            float doorContactHandleDistance = 100;
+            float preContactDistance = 300;
+        };
+
+        struct NamedRobotNodeSet
+        {
+            VirtualRobot::RobotNodePtr joint;
+            VirtualRobot::RobotNodePtr handle;
+            VirtualRobot::RobotNodePtr handleSurfaceProjection;
+        };
+
+        struct DoorInteractionContext
+        {
+            NamedRobotNodeSet rns;
+
+            Pose handleSurfaceProjection_T_door_initial_contact;
+
+            Pose door_initial_contact_T_pre_contact;
+        };
+
+        struct DoorInteractionContextExtended : DoorInteractionContext
+        {
+            Pose handleSurfaceProjection_T_tcp_at_handle;
+        };
+
+        ArticulatedObjectDoorHelper(const VirtualRobot::RobotPtr& object, const Params& params);
+
+        DoorInteractionContext planInteraction(const std::string& nodeSetName) const;
+
+        DoorInteractionContextExtended
+        planInteractionExtended(const std::string& nodeSetName,
+                                const Pose& global_T_tcp_in_contact) const;
+
+
+    protected:
+        Pose handleSurfaceProjection_T_tcp_at_handle(const DoorInteractionContext& interactionInfo,
+                                                     const Pose& global_T_tcp_in_contact) const;
+
+    private:
+        VirtualRobot::RobotPtr object;
+
+        const Params params;
+    };
+} // namespace simox::geometric_planning

GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp

+#include "ArticulatedObjectGeometricPlanningHelper.h"
+
+#include <cmath>
+#include <cstddef>
+#include <iterator>
+#include <numeric>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Nodes/RobotNodeFixed.h>
+#include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
+#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+
+#include "VirtualRobot/VirtualRobotException.h"
+#include <GeometricPlanning/ParametricPath.h>
+#include <GeometricPlanning/path_primitives/CircleSegment.h>
+#include <GeometricPlanning/path_primitives/Line.h>
+#include <GeometricPlanning/path_primitives/PathPrimitive.h>
+#include <GeometricPlanning/types.h>
+#include <GeometricPlanning/assert/assert.h>
+
+
+namespace simox::geometric_planning
+{
+
+    ArticulatedObjectGeometricPlanningHelper::ArticulatedObjectGeometricPlanningHelper(
+        const VirtualRobot::RobotPtr& articulatedObject) :
+        articulatedObject(articulatedObject)
+    {
+    }
+
+    ParametricPath
+    ArticulatedObjectGeometricPlanningHelper::getPathForNode(const std::string& nodeName) const
+    {
+        const auto node = articulatedObject->getRobotNode(nodeName);
+
+        simox::geometric_planning::ArticulatedObjectGeometricPlanningHelper helper(
+            articulatedObject);
+
+        const auto parents = node->getAllParents();
+
+        // VR_INFO << parents.size() << " parents for " << nodeName;
+
+        /// assumption: only one parent is a movable joint
+
+        const auto isJoint = [](const VirtualRobot::RobotNodePtr& node) -> bool
+        {
+            // // VR_INFO << parent->getName() << " " << parent->isJoint();
+            // const std::size_t x = (parent->isJoint()) ? 1 : 0;
+
+            // hack: the above is not working reliably of URDF models.
+            // therefore, we only accept joints with name matching "***_joint"
+
+            return simox::alg::ends_with(node->getName(), "joint");
+        };
+
+        // validate assumption
+        const auto numberMovableParentJoints =
+            std::accumulate(parents.begin(),
+                            parents.end(),
+                            0,
+                            [&isJoint](const std::size_t& init, const auto& parent)
+                            {
+                                const std::size_t x = isJoint(parent) ? 1 : 0;
+
+                                return init + x;
+                            });
+
+        REQUIRE(numberMovableParentJoints == 1);
+
+        const auto parentJoint = std::find_if(parents.begin(), parents.end(), isJoint);
+        const auto& joint = *parentJoint;
+
+        const auto parametricPath = helper.getPathForNode(node->getName(), joint->getName());
+
+        return parametricPath;
+    }
+
+    ParametricPath
+    ArticulatedObjectGeometricPlanningHelper::createCircularPath(
+        const VirtualRobot::RobotNodePtr& node,
+        const VirtualRobot::RobotNodePtr& joint) const
+    {
+
+        const auto* revoluteJoint =
+            dynamic_cast<const VirtualRobot::RobotNodeRevolute*>(joint.get());
+        REQUIRE_MESSAGE(revoluteJoint != nullptr, "`joint` must be a revolute joint!");
+
+        const float initialJointValue = joint->getJointValue();
+
+        joint->setJointValue(joint->getJointLimitLow());
+
+        // We define a reference frame that represents the joint with the axis into +z direction
+        // const Pose global_T_joint_ref(joint->getGlobalPose());
+        // ARMARX_DEBUG << VAROUT(revoluteJoint->getJointRotationAxis());
+        const Pose global_T_joint_orig(revoluteJoint->getGlobalPose());
+
+        const Pose joint_orig_R_global(global_T_joint_orig.inverse().linear());
+
+        const Eigen::Vector3f localJointAxis =
+            joint_orig_R_global * revoluteJoint->getJointRotationAxis();
+        const Eigen::Vector3f localDesiredJointAxis = Eigen::Vector3f::UnitZ();
+
+        const Pose joint_T_joint_ref(
+            Eigen::Quaternionf::FromTwoVectors(localDesiredJointAxis, localJointAxis)
+                .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_node(node->getGlobalPose());
+
+        // this is the node within the joint frame
+        const Pose joint_T_node = global_T_joint.inverse() * global_T_node;
+        // ARMARX_DEBUG << "relative position: " << joint_T_node.translation();
+
+        // the plane in which the node moves is given by the following
+        const Pose global_T_joint_plane =
+            global_T_joint * Eigen::Translation3f{0, 0, joint_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();
+        // ARMARX_DEBUG << "Radius: " << radius;
+
+        REQUIRE_MESSAGE(joint->getJointLimitHigh() > joint->getJointLimitLow(),
+                          "Not implemented yet. Do so by flipping the z axis of the joint.");
+
+        const ParameterRange parameterRange{
+            .min = 0, .max = joint->getJointLimitHigh() - joint->getJointLimitLow()};
+
+        // ARMARX_DEBUG << "radius is " << radius;
+
+        Pose postTransform; //(joint_R_node);
+        postTransform.setIdentity(); // FIXME ???
+
+        //
+        auto subpart = articulatedObject;
+
+        const std::string nodeJointReference = node->getName() + "_joint_reference";
+
+        const Pose joint_T_joint_plane = global_T_joint.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 =
+            global_T_node.translation() - global_T_joint_plane.translation();
+
+        // this is the vector in the joint plane. The z coordinate should be 0
+        const Eigen::Vector3f joint_plane__joint_plane_P_node_initial =
+            global_T_joint_plane.rotation().inverse() * global__joint_plane_P_node_initial;
+
+        const float yaw = std::atan2(joint_plane__joint_plane_P_node_initial.y(),
+                                     joint_plane__joint_plane_P_node_initial.x());
+        // ARMARX_DEBUG << VAROUT(yaw);
+
+        const Pose joint_plane_T_joint_reference(
+            Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()));
+
+        subpart->setJointValue(joint->getName(), joint->getJointLimitLow());
+
+        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_reference(root_T_joint * joint_T_joint_plane *
+                                                joint_plane_T_joint_reference);
+
+        const auto jointReferenceNode = std::make_shared<VirtualRobot::RobotNodeFixed>(
+            subpart, nodeJointReference, root_T_joint_reference.matrix());
+
+        // TODO check if node already exists and unregister if needed
+
+        subpart->registerRobotNode(jointReferenceNode);
+        CHECK(!jointReferenceNode->initialize(subpart->getRootNode()));
+
+        // reset joint state
+        joint->setJointValue(initialJointValue);
+
+
+        return {jointReferenceNode,
+                std::make_unique<CircleSegment>(radius, parameterRange),
+                postTransform};
+    }
+
+    ParametricPath
+    ArticulatedObjectGeometricPlanningHelper::createLinearPath(
+        const VirtualRobot::RobotNodePtr& node,
+        const VirtualRobot::RobotNodePtr& joint) const
+    {
+
+        const Pose relativePose(node->getPoseInFrame(joint));
+
+        // ARMARX_DEBUG << "Relative pose" << relativePose.translation();
+
+        const ParameterRange parameterRange{.min = joint->getJointLimitLow(),
+                                            .max = joint->getJointLimitHigh()};
+
+        // ARMARX_DEBUG << "linear param range " << parameterRange.min << ", " << parameterRange.max;
+        // ARMARX_DEBUG << "Joint in root frame " << joint->getPoseInRootFrame();
+
+        // Info: extractSubPart(joint, "", ""); instead of cloning the full robot is not working properly ...
+        // Compared to a full clone, node->getName())->getPoseInFrame(joint) will be constant for all possible
+        // joint values.
+        // auto subpart = articulatedObject->clone();
+        auto subpart = articulatedObject;
+
+        subpart->setJointValue(joint->getName(), joint->getJointLimitLow());
+        const Pose poseMin(subpart->getRobotNode(node->getName())->getPoseInFrame(joint));
+
+        subpart->setJointValue(joint->getName(), joint->getJointLimitHigh());
+        const Pose poseMax(subpart->getRobotNode(node->getName())->getPoseInFrame(joint));
+
+        // ARMARX_DEBUG << poseMin.translation() << "to" << poseMax.translation();
+
+        // The line parameter should directly relate to the joint state
+        // Therefore, a virtual joint is introduced that is at the same position as the movable node.
+        // The origin of the virtual joint is where the orinal joint state would be 0.
+        Pose subframe = Pose::Identity();
+        subframe.translation() =
+            poseMin.translation() -
+            (poseMax.translation() - poseMin.translation()) * joint->getJointLimitLow();
+
+        const Eigen::Vector3f translationDirection =
+            dynamic_cast<VirtualRobot::RobotNodePrismatic*>(joint.get())
+                ->getJointTranslationDirectionJointCoordSystem();
+        subframe.linear() =
+            Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitX(), translationDirection)
+                .toRotationMatrix();
+
+        // ARMARX_DEBUG << "linear part" << subframe.linear();
+        // ARMARX_DEBUG << "translation direction" << translationDirection;
+
+        const std::string nodeJointReference = node->getName() + "_joint_reference";
+
+        // FIXME attach to parent of joint
+
+        subpart->setJointValue(joint->getName(), joint->getJointLimitLow());
+        const Pose root_T_joint_reference(Pose(joint->getPoseInRootFrame()) * subframe);
+
+        const auto jointReferenceNode = std::make_shared<VirtualRobot::RobotNodeFixed>(
+            subpart, nodeJointReference, root_T_joint_reference.matrix());
+
+        // TODO check if node already exists and unregister if needed
+
+        subpart->registerRobotNode(jointReferenceNode);
+        CHECK(jointReferenceNode->initialize(subpart->getRootNode()));
+
+        // ARMARX_DEBUG << "registered robot node `" << jointReferenceNode->getName() << "`";
+
+        return ParametricPath(jointReferenceNode,
+                              std::make_unique<Line>(parameterRange),
+                              //   Pose(articulatedObject->getGlobalPose()),
+                              Pose::Identity());
+    }
+
+    ParametricPath
+    ArticulatedObjectGeometricPlanningHelper::getPathForNode(const std::string& node,
+                                                             const std::string& joint) const
+    {
+        REQUIRE(articulatedObject->hasRobotNode(node));
+        REQUIRE(articulatedObject->hasRobotNode(joint));
+
+        const auto movableNode = articulatedObject->getRobotNode(node);
+        const auto jointNode = articulatedObject->getRobotNode(joint);
+
+        if (jointNode->isRotationalJoint())
+        {
+            // ARMARX_DEBUG << "joint " << joint << " is rotational joint";
+            return createCircularPath(movableNode, jointNode);
+        }
+
+        if (jointNode->isTranslationalJoint())
+        {
+            // ARMARX_DEBUG << "joint " << joint << " is translational joint";
+            return createLinearPath(movableNode, jointNode);
+        }
+
+        throw VirtualRobot::VirtualRobotException("Unknown joint type!");
+    }
+
+    ParametricPath
+    ArticulatedObjectGeometricPlanningHelper::getPathForNode(
+        const VirtualRobot::RobotNodePtr& node,
+        const VirtualRobot::RobotNodePtr& joint) const
+    {
+        REQUIRE(articulatedObject->hasRobotNode(node));
+        REQUIRE(articulatedObject->hasRobotNode(joint));
+
+        if (joint->isRotationalJoint())
+        {
+            // ARMARX_DEBUG << "joint " << joint << " is rotational joint";
+            return createCircularPath(node, joint);
+        }
+
+        if (joint->isTranslationalJoint())
+        {
+            // ARMARX_DEBUG << "joint " << joint << " is translational joint";
+            return createLinearPath(node, joint);
+        }
+
+        throw VirtualRobot::VirtualRobotException("Unknown joint type!");
+    }
+
+} // namespace simox::geometric_planning

GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.h

+#pragma once
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <GeometricPlanning/ParametricPath.h>
+#include <GeometricPlanning/path_primitives/PathPrimitive.h>
+#include <GeometricPlanning/types.h>
+
+
+namespace simox::geometric_planning
+{
+
+    class ArticulatedObjectGeometricPlanningHelper
+    {
+    public:
+        ArticulatedObjectGeometricPlanningHelper(const VirtualRobot::RobotPtr& articulatedObject);
+
+        /**
+         * @brief Compute the parametric path of a node that results when moving a joint from lower to upper limit.
+         *        It is assumed that only one parent joint exists that is movable.
+         *
+         *
+         * @param node
+         * @param joint
+         * @return ParametricPath node pose depending on joint state
+         */
+        ParametricPath getPathForNode(const std::string& node) const;
+
+
+        /**
+         * @brief Compute the parametric path of a node that results when moving a joint from lower to upper limit.
+         *
+         *
+         * @param node
+         * @param joint
+         * @return ParametricPath node pose depending on joint state
+         */
+        ParametricPath getPathForNode(const std::string& node, const std::string& joint) const;
+
+        /**
+         * @brief Compute the parametric path of a node that results when moving a joint from lower to upper limit.
+         *
+         *
+         * @param node
+         * @param joint
+         * @return ParametricPath node pose depending on joint state
+         */
+        ParametricPath getPathForNode(const VirtualRobot::RobotNodePtr& node,
+                                      const VirtualRobot::RobotNodePtr& joint) const;
+
+    private:
+        ParametricPath createCircularPath(const VirtualRobot::RobotNodePtr& node,
+                                          const VirtualRobot::RobotNodePtr& joint) const;
+
+        ParametricPath createLinearPath(const VirtualRobot::RobotNodePtr& node,
+                                        const VirtualRobot::RobotNodePtr& joint) const;
+
+        const VirtualRobot::RobotPtr articulatedObject;
+    };
+} // namespace simox::geometric_planning

GeometricPlanning/CMakeLists.txt

+PROJECT ( GeometricPlanning )
+
+# cmake_policy(SET CMP0076 NEW)
+
+MESSAGE (STATUS "***** CONFIGURING Simox project GeometricPlanning *****")
+
+find_package(Boost ${Simox_BOOST_VERSION} EXACT COMPONENTS unit_test_framework REQUIRED)
+
+find_package(doctest QUIET)
+
+########################### TESTING #####################################
+MACRO(ADD_GEOMETRIC_PLANNING_TEST TEST_NAME)
+    ADD_EXECUTABLE(${TEST_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/${TEST_NAME}.cpp)
+    target_include_directories(${TEST_NAME} PRIVATE ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/..)
+    if (NOT Boost_USE_STATIC_LIBS)
+        target_compile_definitions(${TEST_NAME} PRIVATE -DBOOST_TEST_DYN_LINK)
+    endif ()
+    TARGET_LINK_LIBRARIES(${TEST_NAME} PRIVATE VirtualRobot GeometricPlanning Boost::unit_test_framework)
+    SET_TARGET_PROPERTIES(${TEST_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_TEST_DIR})
+    SET_TARGET_PROPERTIES(${TEST_NAME} PROPERTIES FOLDER "GeometricPlanning Tests")
+    ADD_TEST(NAME GeometricPlanning_${TEST_NAME}
+    	        COMMAND ${Simox_TEST_DIR}/${TEST_NAME} --output_format=XML --log_level=all --report_level=no)
+ENDMACRO(ADD_GEOMETRIC_PLANNING_TEST)
+
+#################################### FILES ##################################
+SET(SOURCES
+    ParametricPath.cpp
+    ArticulatedObjectGeometricPlanningHelper.cpp
+    ArticulatedObjectDoorHelper.cpp
+    path_primitives/PathPrimitive.cpp
+    path_primitives/Circle.cpp
+    path_primitives/CircleSegment.cpp
+    path_primitives/Line.cpp
+)
+
+SET(HEADERS
+    types.h
+    ParametricPath.h
+    ArticulatedObjectGeometricPlanningHelper.h
+    ArticulatedObjectDoorHelper.h
+    path_primitives/PathPrimitive.h
+    path_primitives/Circle.h
+    path_primitives/CircleSegment.h
+    path_primitives/Line.h
+    assert/assert.h
+)
+
+ADD_LIBRARY(GeometricPlanning SHARED) # sources will be added later
+
+if(doctest_FOUND)
+    message(STATUS "Using doctest")
+    list(APPEND SOURCES
+        assert/doctest/impl.cpp # required for doctest in general
+        assert/doctest/handler.cpp # required for out-of-test assertions
+    )
+
+    list(APPEND HEADERS
+        assert/doctest/assert.h
+    )
+
+    TARGET_LINK_LIBRARIES (GeometricPlanning PUBLIC doctest::doctest )
+    target_compile_definitions (GeometricPlanning PUBLIC -DUSE_DOCTEST )
+
+else()
+    message(STATUS "doctest not found. Using VirtualRobot assertions.")
+
+    list(APPEND SOURCES
+        assert/virtual_robot/assert.cpp
+    )
+
+    list(APPEND HEADERS
+        assert/virtual_robot/assert.h
+    )
+
+endif()
+
+
+if (Simox_USE_COIN_VISUALIZATION)
+	# SET(SOURCES
+	# ${SOURCES}
+	# Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp
+	# )
+
+	# SET(HEADERS
+	# ${HEADERS}
+	# Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h
+	# )
+endif ()
+
+# target_sources(GeometricPlanning
+#     PUBLIC ${HEADERS}
+#     PRIVATE ${SOURCES}
+# )
+
+set_target_properties(GeometricPlanning PROPERTIES SOURCES "${SOURCES};${HEADERS}")
+
+
+#MESSAGE(STATUS "VirtualRobot_ROBOT_LINK_LIBRARIES:" ${VirtualRobot_EXTERNAL_LIBRARIES})
+TARGET_LINK_LIBRARIES (GeometricPlanning PUBLIC VirtualRobot)
+
+# .DLL path
+SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR})
+# .so path
+SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${Simox_LIB_DIR})
+# .lib path (this is needed for setting the DLL-import library path on windows)
+SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${Simox_LIB_DIR})
+
+if(Simox_BUILD_EXAMPLES)
+    # include examples
+    # ADD_SUBDIRECTORY(examples/)
+endif()
+
+if(BUILD_TESTING)
+    # include unit tests
+    # ADD_SUBDIRECTORY(tests/)
+endif()
+
+#######################################################################################
+############################ Setup for installation ###################################
+#######################################################################################
+
+install(TARGETS ${PROJECT_NAME}
+  # IMPORTANT: Add the library to the "export-set"
+  EXPORT SimoxTargets
+  RUNTIME DESTINATION bin COMPONENT bin
+  LIBRARY DESTINATION lib COMPONENT shlib
+  ARCHIVE DESTINATION lib COMPONENT library
+  COMPONENT dev)
+
+INSTALL(DIRECTORY ${PROJECT_SOURCE_DIR} DESTINATION ${Simox_INSTALL_HEADER_DIR}
+	COMPONENT Headers
+        FILES_MATCHING PATTERN "*.h"
+        PATTERN ".svn" EXCLUDE
+        PATTERN ".git" EXCLUDE
+        PATTERN "CMakeModules" EXCLUDE
+        PATTERN "tests" EXCLUDE
+        PATTERN "build*" EXCLUDE
+        PATTERN "data" EXCLUDE
+        PATTERN "examples" EXCLUDE
+	)
+MESSAGE (STATUS "***** Finished CONFIGURING Simox project GeometricPlanning *****\n")

GeometricPlanning/ParametricPath.cpp

+#include "ParametricPath.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
+namespace simox::geometric_planning
+{
+
+    float
+    ParametricPath::parameter(const Pose& global_T_pose) const
+    {
+        return path->parameter(toLocalPathFrame(global_T_pose));
+    }
+
+    float
+    ParametricPath::progress(const Pose& global_T_pose) const
+    {
+        return path->progress(toLocalPathFrame(global_T_pose));
+    }
+
+    Pose
+    ParametricPath::toLocalPathFrame(const Pose& global_T_pose) const
+    {
+        return (Pose(frame->getGlobalPose())).inverse() * global_T_pose; // * path_T_pose.inverse();
+    }
+
+    Eigen::Vector3f
+    ParametricPath::getPosition(float t) const
+    {
+        // translation only
+
+        const Pose frame_T_path(Eigen::Translation3f(path->getPosition(t)));
+
+        return (Pose(frame->getGlobalPose()) * frame_T_path * path_T_pose).translation();
+    }
+
+    Eigen::Vector3f
+    ParametricPath::getPositionDerivative(float t) const
+    {
+        // rotation only
+        Pose frame_T_path_deriv = Pose::Identity();
+        frame_T_path_deriv.translation() = path->getPositionDerivative(t);
+        // frame_T_path_deriv.linear() = path->getOrientation(t).toRotationMatrix();
+
+        Pose preRotate(Pose(frame->getGlobalPose()).linear() *
+                       path->getOrientation(t).toRotationMatrix());
+        Pose postRotate(path_T_pose.linear());
+
+        // Pose T = Pose::Identity();
+
+        return (preRotate * frame_T_path_deriv * postRotate).translation();
+
+        // return R * path->getPositionDerivative(t) ;
+    }
+
+    Eigen::Quaternionf
+    ParametricPath::getOrientation(float t) const
+    {
+        // rotation only
+        const Pose frame_T_path(path->getOrientation(t));
+
+        const Eigen::Quaternionf ori(
+            (Pose(frame->getGlobalPose()) * frame_T_path * path_T_pose).linear());
+
+        return ori;
+    }
+
+    Eigen::Vector3f
+    ParametricPath::getOrientationDerivative(float t) const
+    {
+        const Eigen::Quaternionf frameOri((Pose(frame->getGlobalPose()) * path_T_pose).linear());
+
+        return frameOri.toRotationMatrix() * path->getOrientationDerivative(t);
+    }
+
+    ParametricPath::ParametricPath(const VirtualRobot::RobotNodePtr& frame,
+                                   const std::shared_ptr<PathPrimitive>& path,
+                                   const Pose& postTransform) :
+        frame(frame), path(path), path_T_pose(postTransform)
+    {
+    }
+    ParameterRange
+    ParametricPath::parameterRange() const
+    {
+        return path->parameterRange();
+    }
+} // namespace simox::geometric_planning

GeometricPlanning/ParametricPath.h

+#pragma once
+
+#include <memory>
+
+#include <Eigen/Geometry>
+
+#include <GeometricPlanning/path_primitives/PathPrimitive.h>
+#include <GeometricPlanning/types.h>
+
+
+namespace simox::geometric_planning
+{
+
+    using ParametricPathPtr = std::shared_ptr<class ParametricPath>;
+
+    class ParametricPath : virtual public PathPrimitive
+    {
+    public:
+        ParametricPath(const VirtualRobot::RobotNodePtr& frame,
+                       const std::shared_ptr<PathPrimitive>& path,
+                       const Pose& postTransform);
+
+        //! reference frame describes the movement
+        VirtualRobot::RobotNodePtr frame;
+
+        //! the movement as a parameterized path
+        std::shared_ptr<PathPrimitive> path;
+
+        Pose path_T_pose;
+
+        // helper function to obtain the PathPrimitive's pose directly in the global frame
+
+        // helper functions to obtain the PathPrimitive's parameter and progress directly from a global pose
+        float progress(const Pose& global_T_pose) const;
+
+        Pose toLocalPathFrame(const Pose& global_T_pose) const;
+
+        float parameter(const Pose& global_T_pose) const override;
+        ParameterRange parameterRange() const override;
+
+        Eigen::Vector3f getPosition(float t) const override;
+        Eigen::Vector3f getPositionDerivative(float t) const override;
+        Eigen::Quaternionf getOrientation(float t) const override;
+        Eigen::Vector3f getOrientationDerivative(float t) const override;
+    };
+
+} // namespace simox::geometric_planning

GeometricPlanning/assert/assert.h

+#pragma once
+
+/**
+ * Depending on the configuration (whether doctest should be used or not),
+ * we will either use doctest natively or mimick it's behavior by existing 
+ * functionality from the VirtualRobot library + extensions.
+ *
+ * You can use the following:
+ *
+ * - REQUIRE()
+ * - REQUIRE_MESSAGE()
+ * - CHECK()
+ * - CHECK_MESSAGE()
+ *
+ */
+
+#ifdef USE_DOCTEST
+  #include <GeometricPlanning/assert/doctest/assert.h>
+#else
+  #include <GeometricPlanning/assert/virtual_robot/assert.h>
+#endif // USE_DOCTEST

GeometricPlanning/assert/doctest/assert.h

+#pragma once
+
+#include <doctest/doctest.h>

GeometricPlanning/assert/doctest/handler.cpp

+#ifndef DOCTEST_CONFIG_DISABLE
+#include <doctest/doctest.h>
+
+DOCTEST_MAKE_STD_HEADERS_CLEAN_FROM_WARNINGS_ON_WALL_BEGIN
+#include <algorithm>
+#include <cstring>
+#include <iostream>
+#include <sstream>
+DOCTEST_MAKE_STD_HEADERS_CLEAN_FROM_WARNINGS_ON_WALL_END
+
+#include <VirtualRobot/VirtualRobotException.h>
+
+// std::mutex g_mut;
+
+namespace simox::control::assert::doctest
+{
+    static void
+    handler(const ::doctest::AssertData& ad)
+    {
+        // uncomment if asserts will be used in a multi-threaded context
+        // std::lock_guard<std::mutex> lock(g_mut);
+
+        // here we can choose what to do:
+        // - log the failed assert
+        // - throw an exception
+        // - call std::abort() or std::terminate()
+
+        std::stringstream stream;
+
+        stream << ::doctest::Color::LightGrey << ::doctest::skipPathFromFilename(ad.m_file) << "("
+               << ad.m_line << "): ";
+        stream << ::doctest::Color::Red << ::doctest::failureString(ad.m_at) << ": ";
+
+        // handling only normal (comparison and unary) asserts - exceptions-related asserts have been skipped
+        if ((ad.m_at & ::doctest::assertType::is_normal) != 0)
+        {
+            stream << ::doctest::Color::Cyan << ::doctest::assertString(ad.m_at) << "( "
+                   << ad.m_expr << " ) ";
+            stream << ::doctest::Color::None
+                   << (ad.m_threw ? "THREW exception: " : "is NOT correct!\n");
+            if (ad.m_threw) // This should not happen. Doctest should be forced to not throw anything. We are doing it on our own.
+            {
+                stream << ad.m_exception;
+            }
+            else
+            {
+                stream << "  values: " << ::doctest::assertString(ad.m_at) << "( " << ad.m_decomp
+                       << " )";
+            }
+        }
+        else
+        {
+            stream << ::doctest::Color::None << "an assert dealing with exceptions has failed!";
+        }
+
+        throw VirtualRobot::VirtualRobotException(stream.str());
+
+        std::cout << std::endl;
+    }
+
+    class HandlerRegistration
+    {
+    public:
+        HandlerRegistration() : context_(0, nullptr)
+        {
+            // sets the context as the default one - so asserts used outside of a testing context do not crash
+            context_.setAsDefaultForAssertsOutOfTestCases();
+
+            // set a handler with a signature: void(const ::doctest::AssertData&)
+            // without setting a handler we would get std::abort() called when an assert fails
+            context_.setAssertHandler(handler);
+        }
+
+    private:
+        // construct a context
+        ::doctest::Context context_;
+    };
+
+    // a global symbol to make the context and the handler persistent
+    const static auto handlerReg = HandlerRegistration();
+
+
+} // namespace simox::control::assert::doctest
+
+
+#endif

GeometricPlanning/assert/doctest/impl.cpp

+#define DOCTEST_CONFIG_IMPLEMENTATION_IN_DLL
+#define DOCTEST_CONFIG_IMPLEMENT
+#include <doctest/doctest.h>

GeometricPlanning/assert/virtual_robot/assert.cpp

+#include "assert.h"
+
+#include <sstream>
+#include <experimental/source_location>
+#include <VirtualRobot/VirtualRobotException.h>
+
+namespace simox::geometric_planning::assert::virtual_robot::detail
+{
+    void
+    check(const bool expressionResult,
+          const std::string& expression,
+          const std::experimental::source_location& sourceLocation)
+    {
+        if (not expressionResult)
+        {
+            std::stringstream stream;
+            stream << "Check `" << expression << "` failed in function `"
+                   << sourceLocation.function_name() << "` line " << sourceLocation.line() << " at "
+                   << sourceLocation.file_name();
+            throw VirtualRobot::VirtualRobotException(stream.str());
+        }
+    }
+
+}  // namespace simox::geometric_planning::assert::virtual_robot::detail

GeometricPlanning/assert/virtual_robot/assert.h

+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <experimental/source_location>
+
+#define REQUIRE(a) VR_ASSERT(a)
+#define REQUIRE_MESSAGE(a, b) VR_ASSERT_MESSAGE(a, b)
+
+namespace simox::geometric_planning::assert::virtual_robot::detail
+{
+    void check(bool expressionResult,
+               const std::string& expression,
+               const std::experimental::source_location& sourceLocation =
+                   std::experimental::source_location::current());
+
+} // namespace simox::geometric_planning::assert
+
+#define CHECK(a) ::simox::geometric_planning::assert::virtual_robot::detail::check(a, #a)

GeometricPlanning/path_primitives/Circle.cpp

+#include "Circle.h"
+
+#include <cmath>
+
+#include <Eigen/Geometry>
+
+#include <GeometricPlanning/assert/assert.h>
+
+
+namespace simox::geometric_planning
+{
+
+    Circle::Circle(const float radius) : radius(radius)
+    {
+    }
+
+    ParameterRange
+    Circle::parameterRange() const
+    {
+        return {.min = 0, .max = 2 * M_PI};
+    }
+
+    Eigen::Vector3f
+    Circle::getPosition(float t) const
+    {
+        REQUIRE(parameterRange().isInRange(t));
+
+        return radius * Eigen::Vector3f(std::cos(t), std::sin(t), 0.0F);
+    }
+
+    Eigen::Vector3f
+    Circle::getPositionDerivative(float t) const
+    {
+        REQUIRE(parameterRange().isInRange(t));
+
+        // return radius * Eigen::Vector3f(-std::sin(t + M_PI_2f32), std::cos(t + M_PI_2f32), 0.0F);
+
+        // the position derivative is always pointing into x direction as orientation changes as well
+        // FIXME update this according to getPosition!
+        return radius * -Eigen::Vector3f::UnitX();
+    }
+
+    Eigen::Quaternionf
+    Circle::getOrientation(float t) const
+    {
+        // x pointing into tangential direction
+        // return Eigen::Quaternionf::Identity(); //(Eigen::AngleAxisf(t, Eigen::Vector3f::UnitZ()));
+        return Eigen::Quaternionf(Eigen::AngleAxisf(t, Eigen::Vector3f::UnitZ()));
+    }
+
+    Eigen::Vector3f
+    Circle::getOrientationDerivative(float /*t*/) const
+    {
+        return Eigen::Vector3f::UnitZ();
+    }
+
+    float
+    Circle::parameter(const Pose& pose) const
+    {
+        // ARMARX_DEBUG << "Local pose for parameter calculation " << pose.matrix();
+
+        const float x = pose.translation().x();
+        const float y = pose.translation().y();
+
+        const float phi = std::atan2(y, x); // (-pi,pi]
+
+        // -> the parameter is defined to be in range [0, 2pi)
+        // if y is positive or zero, phi is in range [0,pi]
+        // if y is negative, phi is in range (-pi, 0)
+        // const float param = [&]() -> float
+        // {
+        //     if (y < 0)
+        //     {
+        //         return phi + M_PIf32;
+        //     }
+
+        //     return phi;
+        // }();
+
+        const float param = phi;
+        // ARMARX_DEBUG << "Param is " << param;
+
+        return std::clamp(param, parameterRange().min, parameterRange().max);
+    }
+
+} // namespace simox::geometric_planning

GeometricPlanning/path_primitives/Circle.h

+
+
+#pragma once
+
+#include <GeometricPlanning/path_primitives/PathPrimitive.h>
+
+namespace simox::geometric_planning
+{
+
+    /**
+     * @brief The circle class.
+     *
+     * The circle is defined in the xy-plane, starting at r*(1,0,0)^T and rotating around the z-axis
+     *
+     */
+    class Circle : virtual public PathPrimitive
+    {
+    public:
+        Circle(float radius);
+
+        Eigen::Vector3f getPosition(float t) const override;
+        Eigen::Vector3f getPositionDerivative(float t) const override;
+        Eigen::Quaternionf getOrientation(float t) const override;
+        Eigen::Vector3f getOrientationDerivative(float t) const override;
+
+        float
+        getRadius() const
+        {
+            return radius;
+        }
+
+        ParameterRange parameterRange() const override;
+        float parameter(const Pose& pose) const override;
+
+    private:
+        float radius;
+    };
+
+} // namespace simox::geometric_planning

GeometricPlanning/path_primitives/CircleSegment.cpp

+
+#include "CircleSegment.h"
+
+#include <GeometricPlanning/assert/assert.h>
+
+namespace simox::geometric_planning
+{
+
+    CircleSegment::CircleSegment(const float radius, const ParameterRange& range) :
+        Circle(radius), range(range)
+    {
+        // VR_INFO << "Circle parameter range " << Circle::parameterRange().min << " "
+        //             << Circle::parameterRange().max;
+
+        REQUIRE(Circle::parameterRange().isInRange(range.min));
+        REQUIRE(Circle::parameterRange().isInRange(range.max));
+    }
+
+    ParameterRange
+    CircleSegment::parameterRange() const
+    {
+        return range;
+    }
+
+} // namespace simox::geometric_planning

GeometricPlanning/path_primitives/CircleSegment.h

+
+
+#pragma once
+
+#include "Circle.h"
+
+namespace simox::geometric_planning
+{
+
+    class CircleSegment : virtual public Circle
+    {
+    public:
+        CircleSegment(float radius, const ParameterRange& range);
+        ParameterRange parameterRange() const override;
+
+    private:
+        ParameterRange range;
+    };
+
+} // namespace simox::geometric_planning

GeometricPlanning/path_primitives/Line.cpp

+#include "Line.h"
+
+#include <algorithm>
+
+#include <GeometricPlanning/assert/assert.h>
+
+namespace simox::geometric_planning
+{
+
+    Line::Line(const ParameterRange& parameterRange) : range(parameterRange)
+    {
+    }
+
+    ParameterRange
+    Line::parameterRange() const
+    {
+        return range;
+    }
+
+    Eigen::Vector3f
+    Line::getPosition(float t) const
+    {
+        REQUIRE(parameterRange().isInRange(t));
+
+        return t * Eigen::Vector3f(1.0F, 0.0F, 0.0F);
+    }
+
+    Eigen::Vector3f
+    Line::getPositionDerivative(float t) const
+    {
+        REQUIRE(parameterRange().isInRange(t));
+
+        return Eigen::Vector3f::UnitX();
+    }
+
+    Eigen::Quaternionf
+    Line::getOrientation(float /*t*/) const
+    {
+        return Eigen::Quaternionf::Identity();
+    }
+
+    Eigen::Vector3f
+    Line::getOrientationDerivative(float /*t*/) const
+    {
+        return Eigen::Vector3f::Zero();
+    }
+
+    float
+    Line::parameter(const Pose& pose) const
+    {
+        // we only need to consider the x-coordinate; see GetPosition()
+        const float param = pose.translation().x();
+
+        return std::clamp(param, parameterRange().min, parameterRange().max);
+    }
+
+} // namespace simox::geometric_planning

GeometricPlanning/path_primitives/Line.h

+#pragma once
+
+#include <Eigen/Geometry>
+
+#include <GeometricPlanning/types.h>
+#include <GeometricPlanning/path_primitives/PathPrimitive.h>
+
+namespace simox::geometric_planning
+{
+
+    class Line : virtual public PathPrimitive
+    {
+    public:
+        Line(const ParameterRange& parameterRange);
+
+        Eigen::Vector3f getPosition(float t) const override;
+        Eigen::Vector3f getPositionDerivative(float t) const override;
+        Eigen::Quaternionf getOrientation(float t) const override;
+        Eigen::Vector3f getOrientationDerivative(float t) const override;
+
+        ParameterRange parameterRange() const override;
+
+        float parameter(const Pose& pose) const override;
+
+    private:
+        ParameterRange range;
+    };
+
+} // namespace simox::geometric_planning

GeometricPlanning/path_primitives/PathPrimitive.cpp

+#include "PathPrimitive.h"
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/math/Helpers.h>
+
+namespace simox::geometric_planning
+{
+    bool
+    ParameterRange::isInRange(const float t) const noexcept
+    {
+        return t >= min && t <= max;
+    }
+
+    float
+    PathPrimitive::progress(const Pose& pose) const
+    {
+        const float param = parameter(pose);
+        const auto range = parameterRange();
+
+        return (param - range.min) / (range.max - range.min);
+    }
+
+    Pose
+    PathPrimitive::getPose(const float t) const
+    {
+        return Pose(::math::Helpers::CreatePose(getPosition(t), getOrientation(t)));
+    }
+
+    Eigen::Vector3f
+    PathPrimitive::GetPosition(float t)
+    {
+        return static_cast<const PathPrimitive*>(this)->getPosition(t);
+    }
+
+    Eigen::Vector3f
+    PathPrimitive::GetPositionDerivative(float t)
+    {
+        return static_cast<const PathPrimitive*>(this)->getPositionDerivative(t);
+    }
+
+    Eigen::Quaternionf
+    PathPrimitive::GetOrientation(float t)
+    {
+        return static_cast<const PathPrimitive*>(this)->getOrientation(t);
+    }
+
+    Eigen::Vector3f
+    PathPrimitive::GetOrientationDerivative(float t)
+    {
+        return static_cast<const PathPrimitive*>(this)->getOrientationDerivative(t);
+    }
+
+} // namespace simox::geometric_planning