RobotIK: Convert poses to reachability map coordinate frame before query

source/RobotAPI/components/RobotIK/RobotIK.cpp

@@ -93,6 +93,7 @@ namespace armarx
         }
 
         usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
+        offeringTopic("DebugDrawerUpdates");
     }
 
 
@@ -104,6 +105,8 @@ namespace armarx
                 << " This component, however, uses " << _robotFile << " Both models must be identical!";
         }
         _synchronizedRobot = _robotStateComponentPrx->getSynchronizedRobot();
+
+        debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
     }
 
     void RobotIK::onDisconnectComponent()
@@ -326,7 +329,7 @@ namespace armarx
 
     bool RobotIK::isFramedPoseReachable(const std::string& chainName, const FramedPoseBasePtr& tcpPose, const Ice::Current&) const 
     {
-        return isReachable(chainName, toGlobalPose(tcpPose));
+        return isReachable(chainName, toReachabilityMapFrame(tcpPose, chainName));
     }
 
     bool RobotIK::isPoseReachable(const std::string& chainName, const PoseBasePtr& tcpPose, const Ice::Current&) const 
@@ -457,15 +460,43 @@ namespace armarx
         return globalTcpPose->toEigen();
     }
 
+    Eigen::Matrix4f RobotIK::toReachabilityMapFrame(const FramedPoseBasePtr &tcpPose, const std::string &chainName) const
+    {
+        FramedPosePtr p = FramedPosePtr::dynamicCast(tcpPose);
+
+        PosePtr p_global(new Pose(toGlobalPose(tcpPose)));
+        debugDrawer->setPoseDebugLayerVisu("Grasp Pose", p_global);
+
+        std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
+        if (_reachabilities.count(chainName))
+        {
+            ReachabilityCacheType::const_iterator it = _reachabilities.find(chainName);
+
+            p->changeFrame(_synchronizedRobot, it->second->getBaseNode()->getName());
+        }
+        else
+        {
+            ARMARX_WARNING << "Could not convert TCP pose to reachability map frame: Map not found.";
+        }
+
+        return p->toEigen();
+    }
+
     bool RobotIK::isReachable(const std::string& setName, const Eigen::Matrix4f& tcpPose) const 
     {
+        ARMARX_INFO << "Checking reachability for kinematic chain '" << setName << "': " << tcpPose;
+
         std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
         if (_reachabilities.count(setName)) 
         {
             ReachabilityCacheType::const_iterator it = _reachabilities.find(setName);
             return it->second->isCovered(tcpPose);
         }
-        return false;
+        else
+        {
+            ARMARX_WARNING << "Could not find reachability map for kinematic chain '" << setName << "'";
+            return false;
+        }
     }
 
     VirtualRobot::IKSolver::CartesianSelection RobotIK::convertCartesianSelection(armarx::CartesianSelection cs) const

source/RobotAPI/components/RobotIK/RobotIK.h

@@ -30,6 +30,7 @@
 
 #include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
 #include <RobotAPI/interface/core/RobotIK.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
 #include <VirtualRobot/IK/GenericIKSolver.h>
 
@@ -139,6 +140,7 @@ namespace armarx
             armarx::CartesianSelection cs, NameValueMap& iksolution) const;
         
         Eigen::Matrix4f toGlobalPose(const FramedPoseBasePtr& tcpPose) const;
+        Eigen::Matrix4f toReachabilityMapFrame(const FramedPoseBasePtr &tcpPose, const std::string &chainName) const;
         
         bool isReachable(const std::string& setName, const Eigen::Matrix4f& tcpPose) const;
         
@@ -160,6 +162,8 @@ namespace armarx
         std::string _robotFile;
         RobotStateComponentInterfacePrx _robotStateComponentPrx;
         SharedRobotInterfacePrx _synchronizedRobot;
+        DebugDrawerInterfacePrx debugDrawer;
+
         // Reachability cache: not thread safe, always lock!
         typedef std::map<std::string, VirtualRobot::WorkspaceRepresentationPtr> ReachabilityCacheType;
         ReachabilityCacheType _reachabilities;