Fix wrong poses of collision meshes

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

@@ -22,10 +22,13 @@
 
 #include "CollisionOnApproachFilter.h"
 
+#include <utility>
+
 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
 #include <VirtualRobot/Robot.h>
 
+#include <RobotAPI/components/ArViz/Client/Elements.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
 #include <RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h>
 
@@ -37,35 +40,83 @@ namespace armarx::manipulation::planning::steps
     CollisionOnApproachFilter::CollisionOnApproachFilter(
         const std::string& name,
         const objpose::ObjectPoseClient& objPoseClient,
-        std::shared_ptr<VirtualRobot::Robot> robot) :
-        AsyncPlanningStep(name),
+        std::shared_ptr<VirtualRobot::Robot> robot,
+        std::optional<armarx::viz::Client> client) :
+        PlanningStep(name),
         collisionModelHelper_(
             std::make_unique<obstacle_avoidance::CollisionModelHelper>(objPoseClient)),
-        localRobot_(std::move(robot))
+        localRobot_(std::move(robot)),
+        globalCollisionChecker_(VirtualRobot::CollisionChecker::getGlobalCollisionChecker()),
+        arvizClient_(std::move(client))
     {
     }
-    
+
     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();
+        std::string collisionNodeName;
+        // TODO: fix hardcoding
+        switch (action.unimanual.front().handedness.value().value)
+        {
+            case core::arondto::Handedness::ImplEnum::Left:
+            {
+                collisionNodeName = "Hand L Col";
+                break;
+            }
+            case core::arondto::Handedness::ImplEnum::Right:
+            {
+                collisionNodeName = "Hand R Col";
+                break;
+            }
+            case core::arondto::Handedness::ImplEnum::Unknown:
+            {
+                action.markDiscarded();
+                break;
+            }
+            case core::arondto::Handedness::ImplEnum::Bimanual:
+                return;
+        }
         ARMARX_CHECK(localRobot_->hasRobotNode(collisionNodeName));
-
+        auto localRobot = localRobot_->clone();
         auto sceneObjects = collisionModelHelper_->fetchSceneObjects();
-        VirtualRobot::CollisionModelPtr robotCollisionModel =
-            localRobot_->clone()->getRobotNode(collisionNodeName)->getCollisionModel();
-        robotCollisionModel->setGlobalPose(
+        auto robotNode = localRobot->getRobotNode(collisionNodeName);
+        ARMARX_CHECK_NOT_NULL(robotNode);
+        VirtualRobot::CollisionModelPtr robotCollisionModel = robotNode->getCollisionModel();
+
+        auto getTransformationFromTCPToCollision =
+            [&action, &collisionNodeName, this](const auto& trans) -> Eigen::Matrix4f
+        {
+            Eigen::Affine3f tcp_pose = Eigen::Affine3f(
+                localRobot_->getRobotNode(action.unimanual.front().getTCPName().value())
+                    ->getGlobalPose());
+            Eigen::Affine3f col_pose =
+                Eigen::Affine3f(localRobot_->getRobotNode(collisionNodeName)->getGlobalPose());
+            return (trans * tcp_pose.inverse() * col_pose).matrix();
+        };
+
+        ARMARX_CHECK_NOT_NULL(robotCollisionModel)
+            << "The robot supplied to the filter needs to have a collision model";
+        Eigen::Matrix4f globalColPose = getTransformationFromTCPToCollision(
             action.unimanual.front().execPose->toGlobalEigen(localRobot_));
+        robotCollisionModel->setGlobalPose(globalColPose);
+        if (arvizClient_.has_value())
+        {
+            obstacle_avoidance::CollisionModelHelper::visualizeCollisionModel(
+                robotCollisionModel, const_cast<viz::Client&>(arvizClient_.value()));
+        }
         bool inCollsion = false;
-        inCollsion |= VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
-            robotCollisionModel, sceneObjects);
-
-        robotCollisionModel->setGlobalPose(
+        inCollsion |= globalCollisionChecker_->checkCollision(robotCollisionModel, sceneObjects);
+        globalColPose = getTransformationFromTCPToCollision(
             action.unimanual.front().prePose->toGlobalEigen(localRobot_));
-        inCollsion |= VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
-            robotCollisionModel, sceneObjects);
+        robotCollisionModel->setGlobalPose(globalColPose);
+        if (arvizClient_.has_value())
+        {
+            obstacle_avoidance::CollisionModelHelper::visualizeCollisionModel(
+                robotCollisionModel, const_cast<viz::Client&>(arvizClient_.value()));
+        }
+        inCollsion |= globalCollisionChecker_->checkCollision(robotCollisionModel, sceneObjects);
         if (inCollsion)
         {
             action.markDiscarded();

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

@@ -22,8 +22,11 @@
 
 #pragma once
 
+#include <experimental/memory>
+
 #include <VirtualRobot/VirtualRobot.h>
 
+#include <RobotAPI/components/ArViz/Client/Client.h>
 #include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
 
 #include <armarx/manipulation/action_planning/PlanningStep.h>
@@ -38,18 +41,21 @@ namespace armarx::manipulation::planning::steps
 {
 
 
-    class CollisionOnApproachFilter : public armarx::manipulation::planning::AsyncPlanningStep
+    class CollisionOnApproachFilter : public armarx::manipulation::planning::PlanningStep
     {
     public:
         CollisionOnApproachFilter(const std::string& name,
                                   const objpose::ObjectPoseClient& objPoseClient,
-                                  std::shared_ptr<VirtualRobot::Robot> robot);
+                                  std::shared_ptr<VirtualRobot::Robot> robot,
+                                  std::optional<armarx::viz::Client> client = std::nullopt);
 
         void execute(core::ExecutableAction& action) const override;
 
     private:
         std::unique_ptr<obstacle_avoidance::CollisionModelHelper> collisionModelHelper_;
         const std::shared_ptr<VirtualRobot::Robot> localRobot_;
+        const std::shared_ptr<VirtualRobot::CollisionChecker> globalCollisionChecker_;
+         std::optional<armarx::viz::Client> arvizClient_;
     };
 
 } // namespace armarx::manipulation::planning::steps