ConstrainedOptimizationIK: Made global tolerance optional

VirtualRobot/IK/ConstrainedOptimizationIK.cpp

@@ -28,11 +28,11 @@
 
 using namespace VirtualRobot;
 
-ConstrainedOptimizationIK::ConstrainedOptimizationIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float timeout, float tolerance) :
+ConstrainedOptimizationIK::ConstrainedOptimizationIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float timeout, float globalTolerance) :
     ConstrainedIK(robot, nodeSet, 30),
     nodeSet(nodeSet),
     timeout(timeout),
-    tolerance(tolerance)
+    globalTolerance(globalTolerance)
 {
     clearSeeds();
     addSeed(eSeedInitial);
@@ -58,8 +58,12 @@ bool ConstrainedOptimizationIK::initialize()
     optimizer->set_lower_bounds(low);
     optimizer->set_upper_bounds(high);
 
+    if(!std::isnan(globalTolerance))
+    {
+        optimizer->set_stopval(globalTolerance * globalTolerance);
+    }
+
     optimizer->set_maxtime(timeout);
-    optimizer->set_stopval(tolerance * tolerance);
     optimizer->set_ftol_abs(1e-6);
     optimizer->set_xtol_abs(1e-4);
 

VirtualRobot/IK/ConstrainedOptimizationIK.h

@@ -42,7 +42,7 @@ namespace VirtualRobot
     class VIRTUAL_ROBOT_IMPORT_EXPORT ConstrainedOptimizationIK : public ConstrainedIK, public boost::enable_shared_from_this<ConstrainedOptimizationIK>
     {
     public:
-        ConstrainedOptimizationIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float timeout = 0.5, float tolerance = 0.01);
+        ConstrainedOptimizationIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float timeout = 0.5, float globalTolerance = std::numeric_limits<float>::quiet_NaN());
 
         bool initialize();
         bool solve(bool stepwise = false);
@@ -63,7 +63,7 @@ namespace VirtualRobot
         OptimizerPtr optimizer;
 
         float timeout;
-        float tolerance;
+        float globalTolerance;
 
         std::vector<double> currentX;