ConstrainedIK: Added stall condition

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@816 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/IK/ConstrainedHierarchicalIK.cpp

@@ -2,8 +2,8 @@
 
 using namespace VirtualRobot;
 
-ConstrainedHierarchicalIK::ConstrainedHierarchicalIK(RobotPtr &robot, const RobotNodeSetPtr &nodeSet, float stepSize, int maxIterations) :
-    ConstrainedIK(robot, maxIterations),
+ConstrainedHierarchicalIK::ConstrainedHierarchicalIK(RobotPtr &robot, const RobotNodeSetPtr &nodeSet, float stepSize, int maxIterations, float stall_epsilon) :
+    ConstrainedIK(robot, maxIterations, stall_epsilon),
     nodeSet(nodeSet),
     stepSize(stepSize)
 {
@@ -29,6 +29,14 @@ bool ConstrainedHierarchicalIK::solveStep()
     Eigen::VectorXf jointValues;
     Eigen::VectorXf delta = ik->computeStep(jacobians, stepSize);
 
+    // Check the stall condition
+    if(lastDelta.rows() > 0 && (delta - lastDelta).norm() < stallEpsilon)
+    {
+        VR_INFO << "Constrained IK failed due to stall condition" << std::endl;
+        return false;
+    }
+    lastDelta = delta;
+
     nodeSet->getJointValues(jointValues);
     nodeSet->setJointValues(jointValues + delta);
 

VirtualRobot/IK/ConstrainedHierarchicalIK.h

@@ -34,7 +34,7 @@ namespace VirtualRobot
     class VIRTUAL_ROBOT_IMPORT_EXPORT ConstrainedHierarchicalIK : public ConstrainedIK, public boost::enable_shared_from_this<ConstrainedHierarchicalIK>
     {
         public:
-            ConstrainedHierarchicalIK(RobotPtr &robot, const RobotNodeSetPtr &nodeSet, float stepSize=0.2f, int maxIterations=1000);
+            ConstrainedHierarchicalIK(RobotPtr &robot, const RobotNodeSetPtr &nodeSet, float stepSize=0.2f, int maxIterations=1000, float stall_epsilon = 0.0001);
 
             bool initialize();
             bool solveStep();
@@ -46,6 +46,7 @@ namespace VirtualRobot
             std::vector<JacobiProviderPtr> jacobians;
 
             float stepSize;
+            Eigen::VectorXf lastDelta;
     };
 
     typedef boost::shared_ptr<ConstrainedHierarchicalIK> ConstrainedHierarchicalIKPtr;

VirtualRobot/IK/ConstrainedIK.cpp

@@ -2,11 +2,12 @@
 
 using namespace VirtualRobot;
 
-ConstrainedIK::ConstrainedIK(RobotPtr &robot, int maxIterations) :
+ConstrainedIK::ConstrainedIK(RobotPtr &robot, int maxIterations, float stall_epsilon) :
     robot(robot),
     maxIterations(maxIterations),
     currentIteration(0),
-    running(false)
+    running(false),
+    stallEpsilon(stall_epsilon)
 {
 
 }

VirtualRobot/IK/ConstrainedIK.h

@@ -35,7 +35,7 @@ namespace VirtualRobot
     class VIRTUAL_ROBOT_IMPORT_EXPORT ConstrainedIK : public boost::enable_shared_from_this<ConstrainedIK>
     {
         public:
-            ConstrainedIK(RobotPtr &robot, int maxIterations = 1000);
+            ConstrainedIK(RobotPtr &robot, int maxIterations = 1000, float stall_epsilon = 0.0001);
 
             void addConstraint(const ConstraintPtr &constraint, int priority=0, bool hard_constraint=true);
             std::vector<ConstraintPtr> getConstraints();
@@ -60,6 +60,7 @@ namespace VirtualRobot
             int maxIterations;
             int currentIteration;
             bool running;
+            float stallEpsilon;
     };
 
     typedef boost::shared_ptr<ConstrainedIK> ConstrainedIKPtr;