added SimpleDiffIK::CalculateReachability

source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice

@@ -63,8 +63,11 @@ module armarx
         };
         class GraspCandidateReachabilityInfo
         {
-            bool reachable;
-            float minimumJointLimitMargin;
+            bool reachable = false;
+            float minimumJointLimitMargin = -1;
+            Ice::FloatSeq jointLimitMargins;
+            float maxPosError = 0;
+            float maxOriError = 0;
         };
 
 

source/RobotAPI/libraries/core/SimpleDiffIK.cpp

@@ -83,3 +83,19 @@ SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetP
 
     return result;
 }
+
+SimpleDiffIK::Reachability SimpleDiffIK::CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+{
+    Reachability r;
+    rns->setJointValues(initialJV);
+    for (const Eigen::Matrix4f& target : targets)
+    {
+        Result res = CalculateDiffIK(target, rns, tcp, params);
+        r.aggregate(res);
+    }
+    if (!r.reachable)
+    {
+        r.minimumJointLimitMargin = -1;
+    }
+    return r;
+}

source/RobotAPI/libraries/core/SimpleDiffIK.h

@@ -58,8 +58,35 @@ namespace armarx
             Eigen::VectorXf jointLimitMargins;
             float minimumJointLimitMargin;
         };
+        struct Reachability
+        {
+
+            bool reachable = true;
+            float minimumJointLimitMargin = M_PI;
+            Eigen::VectorXf jointLimitMargins;
+            float maxPosError = 0;
+            float maxOriError = 0;
+
+            void aggregate(const Result& result)
+            {
+                reachable = reachable && result.reached;
+                minimumJointLimitMargin = std::min(minimumJointLimitMargin, result.minimumJointLimitMargin);
+                if (jointLimitMargins.rows() == 0)
+                {
+                    jointLimitMargins = result.jointLimitMargins;
+                }
+                else
+                {
+                    jointLimitMargins = jointLimitMargins.cwiseMin(result.jointLimitMargins);
+                }
+                maxPosError = std::max(maxPosError, result.posError);
+                maxOriError = std::max(maxOriError, result.oriError);
+            }
+        };
 
         static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
 
+        static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
+
     };
 }