From ef7fe1b751e384afdee0e844bb727cf3b850e338 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 28 Dec 2022 12:20:22 +0100
Subject: [PATCH] new geometric planning library + doctest assertions
---
.../ArticulatedObjectDoorHelper.cpp | 92 +++++
.../ArticulatedObjectDoorHelper.h | 88 +++++
...ticulatedObjectGeometricPlanningHelper.cpp | 313 ++++++++++++++++++
...ArticulatedObjectGeometricPlanningHelper.h | 62 ++++
GeometricPlanning/CMakeLists.txt | 141 ++++++++
GeometricPlanning/ParametricPath.cpp | 89 +++++
GeometricPlanning/ParametricPath.h | 47 +++
GeometricPlanning/assert/assert.h | 21 ++
GeometricPlanning/assert/doctest/assert.h | 3 +
GeometricPlanning/assert/doctest/handler.cpp | 86 +++++
GeometricPlanning/assert/doctest/impl.cpp | 3 +
.../assert/virtual_robot/assert.cpp | 24 ++
.../assert/virtual_robot/assert.h | 18 +
GeometricPlanning/path_primitives/Circle.cpp | 86 +++++
GeometricPlanning/path_primitives/Circle.h | 39 +++
.../path_primitives/CircleSegment.cpp | 25 ++
.../path_primitives/CircleSegment.h | 20 ++
GeometricPlanning/path_primitives/Line.cpp | 57 ++++
GeometricPlanning/path_primitives/Line.h | 29 ++
.../path_primitives/PathPrimitive.cpp | 54 +++
.../path_primitives/PathPrimitive.h | 57 ++++
GeometricPlanning/types.h | 38 +++
22 files changed, 1392 insertions(+)
create mode 100644 GeometricPlanning/ArticulatedObjectDoorHelper.cpp
create mode 100644 GeometricPlanning/ArticulatedObjectDoorHelper.h
create mode 100644 GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
create mode 100644 GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.h
create mode 100644 GeometricPlanning/CMakeLists.txt
create mode 100644 GeometricPlanning/ParametricPath.cpp
create mode 100644 GeometricPlanning/ParametricPath.h
create mode 100644 GeometricPlanning/assert/assert.h
create mode 100644 GeometricPlanning/assert/doctest/assert.h
create mode 100644 GeometricPlanning/assert/doctest/handler.cpp
create mode 100644 GeometricPlanning/assert/doctest/impl.cpp
create mode 100644 GeometricPlanning/assert/virtual_robot/assert.cpp
create mode 100644 GeometricPlanning/assert/virtual_robot/assert.h
create mode 100644 GeometricPlanning/path_primitives/Circle.cpp
create mode 100644 GeometricPlanning/path_primitives/Circle.h
create mode 100644 GeometricPlanning/path_primitives/CircleSegment.cpp
create mode 100644 GeometricPlanning/path_primitives/CircleSegment.h
create mode 100644 GeometricPlanning/path_primitives/Line.cpp
create mode 100644 GeometricPlanning/path_primitives/Line.h
create mode 100644 GeometricPlanning/path_primitives/PathPrimitive.cpp
create mode 100644 GeometricPlanning/path_primitives/PathPrimitive.h
create mode 100644 GeometricPlanning/types.h
diff --git a/GeometricPlanning/ArticulatedObjectDoorHelper.cpp b/GeometricPlanning/ArticulatedObjectDoorHelper.cpp
new file mode 100644
index 000000000..1fe8bae85
--- /dev/null
+++ b/GeometricPlanning/ArticulatedObjectDoorHelper.cpp
@@ -0,0 +1,92 @@
+#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
diff --git a/GeometricPlanning/ArticulatedObjectDoorHelper.h b/GeometricPlanning/ArticulatedObjectDoorHelper.h
new file mode 100644
index 000000000..99b6efd59
--- /dev/null
+++ b/GeometricPlanning/ArticulatedObjectDoorHelper.h
@@ -0,0 +1,88 @@
+/**
+ * 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
diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
new file mode 100644
index 000000000..6002c8e95
--- /dev/null
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -0,0 +1,313 @@
+#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
diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.h b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.h
new file mode 100644
index 000000000..058d13bf1
--- /dev/null
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.h
@@ -0,0 +1,62 @@
+#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
diff --git a/GeometricPlanning/CMakeLists.txt b/GeometricPlanning/CMakeLists.txt
new file mode 100644
index 000000000..3e784bd4d
--- /dev/null
+++ b/GeometricPlanning/CMakeLists.txt
@@ -0,0 +1,141 @@
+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")
diff --git a/GeometricPlanning/ParametricPath.cpp b/GeometricPlanning/ParametricPath.cpp
new file mode 100644
index 000000000..bd5e81e9e
--- /dev/null
+++ b/GeometricPlanning/ParametricPath.cpp
@@ -0,0 +1,89 @@
+#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
diff --git a/GeometricPlanning/ParametricPath.h b/GeometricPlanning/ParametricPath.h
new file mode 100644
index 000000000..ec58679d9
--- /dev/null
+++ b/GeometricPlanning/ParametricPath.h
@@ -0,0 +1,47 @@
+#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
diff --git a/GeometricPlanning/assert/assert.h b/GeometricPlanning/assert/assert.h
new file mode 100644
index 000000000..eaaef3985
--- /dev/null
+++ b/GeometricPlanning/assert/assert.h
@@ -0,0 +1,21 @@
+#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
diff --git a/GeometricPlanning/assert/doctest/assert.h b/GeometricPlanning/assert/doctest/assert.h
new file mode 100644
index 000000000..5f7e0d204
--- /dev/null
+++ b/GeometricPlanning/assert/doctest/assert.h
@@ -0,0 +1,3 @@
+#pragma once
+
+#include <doctest/doctest.h>
diff --git a/GeometricPlanning/assert/doctest/handler.cpp b/GeometricPlanning/assert/doctest/handler.cpp
new file mode 100644
index 000000000..cc666fd7d
--- /dev/null
+++ b/GeometricPlanning/assert/doctest/handler.cpp
@@ -0,0 +1,86 @@
+#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
diff --git a/GeometricPlanning/assert/doctest/impl.cpp b/GeometricPlanning/assert/doctest/impl.cpp
new file mode 100644
index 000000000..62be2a6a4
--- /dev/null
+++ b/GeometricPlanning/assert/doctest/impl.cpp
@@ -0,0 +1,3 @@
+#define DOCTEST_CONFIG_IMPLEMENTATION_IN_DLL
+#define DOCTEST_CONFIG_IMPLEMENT
+#include <doctest/doctest.h>
diff --git a/GeometricPlanning/assert/virtual_robot/assert.cpp b/GeometricPlanning/assert/virtual_robot/assert.cpp
new file mode 100644
index 000000000..10e39558b
--- /dev/null
+++ b/GeometricPlanning/assert/virtual_robot/assert.cpp
@@ -0,0 +1,24 @@
+#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
diff --git a/GeometricPlanning/assert/virtual_robot/assert.h b/GeometricPlanning/assert/virtual_robot/assert.h
new file mode 100644
index 000000000..a534de74f
--- /dev/null
+++ b/GeometricPlanning/assert/virtual_robot/assert.h
@@ -0,0 +1,18 @@
+#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)
diff --git a/GeometricPlanning/path_primitives/Circle.cpp b/GeometricPlanning/path_primitives/Circle.cpp
new file mode 100644
index 000000000..3d41c174b
--- /dev/null
+++ b/GeometricPlanning/path_primitives/Circle.cpp
@@ -0,0 +1,86 @@
+#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
diff --git a/GeometricPlanning/path_primitives/Circle.h b/GeometricPlanning/path_primitives/Circle.h
new file mode 100644
index 000000000..f0b711567
--- /dev/null
+++ b/GeometricPlanning/path_primitives/Circle.h
@@ -0,0 +1,39 @@
+
+
+#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
diff --git a/GeometricPlanning/path_primitives/CircleSegment.cpp b/GeometricPlanning/path_primitives/CircleSegment.cpp
new file mode 100644
index 000000000..d3c1bc5a3
--- /dev/null
+++ b/GeometricPlanning/path_primitives/CircleSegment.cpp
@@ -0,0 +1,25 @@
+
+#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
diff --git a/GeometricPlanning/path_primitives/CircleSegment.h b/GeometricPlanning/path_primitives/CircleSegment.h
new file mode 100644
index 000000000..f383afbcf
--- /dev/null
+++ b/GeometricPlanning/path_primitives/CircleSegment.h
@@ -0,0 +1,20 @@
+
+
+#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
diff --git a/GeometricPlanning/path_primitives/Line.cpp b/GeometricPlanning/path_primitives/Line.cpp
new file mode 100644
index 000000000..d9f50e157
--- /dev/null
+++ b/GeometricPlanning/path_primitives/Line.cpp
@@ -0,0 +1,57 @@
+#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
diff --git a/GeometricPlanning/path_primitives/Line.h b/GeometricPlanning/path_primitives/Line.h
new file mode 100644
index 000000000..f055d2aeb
--- /dev/null
+++ b/GeometricPlanning/path_primitives/Line.h
@@ -0,0 +1,29 @@
+#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
diff --git a/GeometricPlanning/path_primitives/PathPrimitive.cpp b/GeometricPlanning/path_primitives/PathPrimitive.cpp
new file mode 100644
index 000000000..d78fb0e52
--- /dev/null
+++ b/GeometricPlanning/path_primitives/PathPrimitive.cpp
@@ -0,0 +1,54 @@
+#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
diff --git a/GeometricPlanning/path_primitives/PathPrimitive.h b/GeometricPlanning/path_primitives/PathPrimitive.h
new file mode 100644
index 000000000..e5e81ae11
--- /dev/null
+++ b/GeometricPlanning/path_primitives/PathPrimitive.h
@@ -0,0 +1,57 @@
+#pragma once
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+
+#include <GeometricPlanning/types.h>
+
+namespace simox::geometric_planning
+{
+ struct ParameterRange
+ {
+ float min;
+ float max;
+
+ bool isInRange(float t) const noexcept;
+ };
+
+ class PathPrimitive : virtual public ::math::AbstractFunctionR1R6
+ {
+ public:
+ virtual ParameterRange parameterRange() const = 0;
+
+ /**
+ * @brief Projects a point onto the parametric model's manifold.
+ *
+ * @param pose
+ * @return the parameter closest to the pose
+ */
+ virtual float parameter(const Pose& pose) const = 0;
+
+ /**
+ * @brief Projects a point onto the parametric model's manifold.
+ *
+ * @param pose
+ * @return the progress in range [0,1]
+ */
+ float progress(const Pose& pose) const;
+
+ // math::AbstractFunctionR1R6 interface
+ Eigen::Vector3f GetPosition(float t) override;
+ Eigen::Vector3f GetPositionDerivative(float t) override;
+ Eigen::Quaternionf GetOrientation(float t) override;
+ Eigen::Vector3f GetOrientationDerivative(float t) override;
+
+ // const variants of the base class' methods
+ virtual Eigen::Vector3f getPosition(float t) const = 0;
+ virtual Eigen::Vector3f getPositionDerivative(float t) const = 0;
+ virtual Eigen::Quaternionf getOrientation(float t) const = 0;
+ virtual Eigen::Vector3f getOrientationDerivative(float t) const = 0;
+
+ Pose getPose(float t) const;
+
+ ~PathPrimitive() override = default;
+ };
+
+} // namespace simox::geometric_planning
diff --git a/GeometricPlanning/types.h b/GeometricPlanning/types.h
new file mode 100644
index 000000000..84a9d140b
--- /dev/null
+++ b/GeometricPlanning/types.h
@@ -0,0 +1,38 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox 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 Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Fabian Reister ( fabian dot reister at kit dot edu )
+* @date 2022
+* @copyright GNU Lesser General Public License
+*
+*/
+
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Geometry>
+
+
+namespace simox::geometric_planning
+{
+ using Pose = Eigen::Isometry3f;
+ using Poses = std::vector<Pose>;
+
+ using Position = Eigen::Vector3f;
+
+} // namespace armarx::manipulation::core
--
GitLab