Merge branch 'main' of git.h2t.iar.kit.edu:sw/armarx/skills/manipulation into main

source/armarx/manipulation/action_planning/planning_steps/CMakeLists.txt

@@ -17,6 +17,7 @@ armarx_add_library(planning_steps
         filtering/HandednessFilter.cpp
         filtering/ApproachDirectionFilter.cpp
         filtering/ReachabilityFilter.cpp
+        filtering/CollisionOnApproachFilter.cpp
 
         scoring/GraspScorer.cpp
         scoring/ZCoordinateGraspScorer.cpp
@@ -39,8 +40,7 @@ armarx_add_library(planning_steps
         filtering/HandednessFilter.h
         filtering/ApproachDirectionFilter.h
         filtering/ReachabilityFilter.h
-        # filtering/ProximityFilter.h
-        # filtering/CollisionFilter.h
+        filtering/CollisionOnApproachFilter.h
 
         scoring/GraspScorer.h
         scoring/ZCoordinateGraspScorer.h
@@ -55,6 +55,8 @@ armarx_add_library(planning_steps
 
         ArmarXCore
 
+        obstacle_avoidance
+
         Simox::VirtualRobot
 
         armarx_manipulation::core

source/armarx/manipulation/action_planning/planning_steps/filtering/CollisionOnApproachFilter.cpp

+/*
+ * 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/>.
+ *
+ * @package    armarx_manipulation
+ * @author     Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date       17.02.23
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "CollisionOnApproachFilter.h"
+
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h>
+
+#include <armarx/manipulation/core/ExecutableAction.h>
+
+namespace armarx::manipulation::planning::steps
+{
+
+    CollisionOnApproachFilter::CollisionOnApproachFilter(
+        const std::string& name,
+        const objpose::ObjectPoseClient& objPoseClient,
+        std::shared_ptr<VirtualRobot::Robot> robot) :
+        AsyncPlanningStep(name),
+        collisionModelHelper_(
+            std::make_unique<obstacle_avoidance::CollisionModelHelper>(objPoseClient)),
+        localRobot_(std::move(robot))
+    {
+    }
+    
+    void
+    CollisionOnApproachFilter::execute(core::ExecutableAction& action) const
+    {
+        ARMARX_CHECK_EQUAL(action.unimanual.size(), 1)
+            << "Collision checking for multiple hands is not implemented yet";
+        const std::string collisionNodeName = action.unimanual.front().getHandRootNode().value();
+        ARMARX_CHECK(localRobot_->hasRobotNode(collisionNodeName));
+
+        auto sceneObjects = collisionModelHelper_->fetchSceneObjects();
+        VirtualRobot::CollisionModelPtr robotCollisionModel =
+            localRobot_->clone()->getRobotNode(collisionNodeName)->getCollisionModel();
+        robotCollisionModel->setGlobalPose(
+            action.unimanual.front().execPose->toGlobalEigen(localRobot_));
+        bool inCollsion = false;
+        inCollsion |= VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
+            robotCollisionModel, sceneObjects);
+
+        robotCollisionModel->setGlobalPose(
+            action.unimanual.front().prePose->toGlobalEigen(localRobot_));
+        inCollsion |= VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
+            robotCollisionModel, sceneObjects);
+        if (inCollsion)
+        {
+            action.markDiscarded();
+        }
+    }
+} // namespace armarx::manipulation::planning::steps
\ No newline at end of file

source/armarx/manipulation/action_planning/planning_steps/filtering/CollisionOnApproachFilter.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/>.
+ *
+ * @package    armarx_manipulation
+ * @author     Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date       17.02.23
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+
+#include <armarx/manipulation/action_planning/PlanningStep.h>
+#include <armarx/manipulation/core/forward_declarations.h>
+
+namespace armarx::obstacle_avoidance
+{
+    class CollisionModelHelper;
+}
+
+namespace armarx::manipulation::planning::steps
+{
+
+
+    class CollisionOnApproachFilter : public armarx::manipulation::planning::AsyncPlanningStep
+    {
+    public:
+        CollisionOnApproachFilter(const std::string& name,
+                                  const objpose::ObjectPoseClient& objPoseClient,
+                                  std::shared_ptr<VirtualRobot::Robot> robot);
+
+        void execute(core::ExecutableAction& action) const override;
+
+    private:
+        std::unique_ptr<obstacle_avoidance::CollisionModelHelper> collisionModelHelper_;
+        const std::shared_ptr<VirtualRobot::Robot> localRobot_;
+    };
+
+} // namespace armarx::manipulation::planning::steps

source/armarx/manipulation/util/BimanualTransformationHelper.h

@@ -21,8 +21,7 @@
  *             GNU General Public License
  */
 
-#ifndef armar6_skills_BIMANUALTRANSFORMATIONHELPER_H
-#define armar6_skills_BIMANUALTRANSFORMATIONHELPER_H
+#pragma once
 
 
 #include <VirtualRobot/Robot.h>
@@ -112,4 +111,3 @@ namespace armarx::manipulation::util
     };
 }  // namespace armarx::manipulation::util
 
-#endif //armar6_skills_BIMANUALTRANSFORMATIONHELPER_H

source/armarx/manipulation/util/CMakeLists.txt

@@ -11,4 +11,4 @@ armarx_add_library(util
         BimanualTransformationHelper.h
         LieHelpers.h
         Utilities.h
-        )
+)