Merge branch 'master' into '68-virtualrobot-robotnodehemisphere-has-no-member-named-second'

# Conflicts:
#   GeometricPlanning/ArticulatedObjectDoorHelper.cpp

GeometricPlanning/ArticulatedObjectDoorHelper.cpp

@@ -14,13 +14,14 @@ namespace simox::geometric_planning
                                                              const Params& params) :
         object(object), params(params)
     {
+        CHECK_MESSAGE(object != nullptr, "Object must not be null");
     }
 
     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!");
+        CHECK_MESSAGE(rns != nullptr, std::string("Robot node set `" + nodeSetName + "` does not exist!"));
 
         const std::string jointNodeName = nodeSetName + constants::JointSuffix;
         const std::string handleNodeName = nodeSetName + constants::HandleSuffix;
@@ -28,12 +29,9 @@ namespace simox::geometric_planning
 
         const auto checkNodeExists = [&rns, &nodeSetName]([[maybe_unused]] const std::string& nodeName)
         {
-            (void) rns, (void) nodeSetName;
-            /*
-            REQUIRE_MESSAGE(rns->hasRobotNode(nodeName),
-                            "Robot node `" << nodeName + "` does not exist within robot node set `"
-                                           << nodeSetName << "`!");
-            */
+            CHECK_MESSAGE(rns->hasRobotNode(nodeName),
+                            std::string("Robot node `" + nodeName + "` does not exist within robot node set `"
+                                           + nodeSetName + "`!"));
         };
 
         for (const auto& nodeName : {jointNodeName, handleNodeName, surfaceProjectionNodeName})
@@ -41,7 +39,7 @@ namespace simox::geometric_planning
             checkNodeExists(nodeName);
         }
 
-        REQUIRE_MESSAGE(params.doorContactHandleDistance > 0.,
+        CHECK_MESSAGE(params.doorContactHandleDistance > 0.,
                         "Grasping from the other side not implemented yet!");
 
         return DoorInteractionContext{

GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp

@@ -143,8 +143,8 @@ namespace simox::geometric_planning
 
         // ARMARX_DEBUG << "radius is " << radius;
 
-        Pose postTransform; //(joint_R_node);
-        postTransform.setIdentity(); // FIXME ???
+        // Pose postTransform; //(joint_R_node);
+        // postTransform.setIdentity(); // FIXME ???
 
         //
         auto subpart = articulatedObject;
@@ -193,7 +193,7 @@ namespace simox::geometric_planning
 
         return {jointReferenceNode,
                 std::make_unique<CircleSegment>(radius, parameterRange),
-                postTransform};
+                Pose::Identity()};
     }
 
     ParametricPath
@@ -263,7 +263,6 @@ namespace simox::geometric_planning
 
         return ParametricPath(jointReferenceNode,
                               std::make_unique<Line>(parameterRange),
-                              //   Pose(articulatedObject->getGlobalPose()),
                               Pose::Identity());
     }
 

GeometricPlanning/path_primitives/Circle.cpp

@@ -37,7 +37,7 @@ namespace simox::geometric_planning
 
         // 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();
+        return radius * Eigen::Vector3f::UnitY();
     }
 
     Eigen::Quaternionf