Fixed bug in ConditionCheckMagnitudeChecks

source/RobotAPI/libraries/robotstate/remote/checks/ConditionCheckMagnitudeChecks.cpp

@@ -31,35 +31,20 @@ namespace armarx {
         if(type == VariantType::Vector3)
         {
             Eigen::Vector3f vec = value.getClass<Vector3>()->toEigen();
-            Eigen::Vector3f vec2 = getParameter(0).getClass<Vector3>()->toEigen();
-            return (vec-vec2).norm() > getParameter(1).getFloat();
+            return (vec).norm() > getParameter(0).getFloat();
         }
 
         if(type == VariantType::FramedVector3)
         {
             FramedVector3Ptr fV1 = value.getClass<FramedVector3>();
-            FramedVector3Ptr fV2 = getParameter(0).getClass<FramedVector3>();
-            if(fV1->frame != fV2->frame)
-            {
-                ARMARX_WARNING_S << deactivateSpam(3) << "Cannot compare FramedVector3 with different reference frames, because no robot is here available. Use LinkedVector3 instead or provide in correct frames";
-                return false;
-            }
-
             Eigen::Vector3f vec = value.getClass<FramedVector3>()->toEigen();
-            Eigen::Vector3f vec2 = getParameter(0).getClass<FramedVector3>()->toEigen();
-            return (vec-vec2).norm() > getParameter(1).getFloat();
-        }
-
-        if(type == VariantType::LinkedVector3)
+            return (vec).norm() > getParameter(0).getFloat();
+        }        if(type == VariantType::LinkedVector3)
         {
             LinkedVector3Ptr lV1 = value.getClass<LinkedVector3>();
-            LinkedVector3Ptr lV2 = getParameter(0).getClass<LinkedVector3>();
-            if(lV1->frame != lV2->frame)
-                lV1->changeFrame(lV2->frame);
 
             Eigen::Vector3f vec = value.getClass<LinkedVector3>()->toEigen();
-            Eigen::Vector3f vec2 = getParameter(0).getClass<LinkedVector3>()->toEigen();
-            return (vec-vec2).norm() > getParameter(1).getFloat();
+            return (vec).norm() > getParameter(0).getFloat();
         }
 
         return false;