ConstrainedIK: Added support for seed configurations

VirtualRobot/IK/ConstrainedHierarchicalIK.cpp

@@ -3,7 +3,7 @@
 using namespace VirtualRobot;
 
 ConstrainedHierarchicalIK::ConstrainedHierarchicalIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float stepSize, int maxIterations, float stall_epsilon, float raise_epsilon) :
-    ConstrainedIK(robot, maxIterations, stall_epsilon, raise_epsilon),
+    ConstrainedIK(robot, nodeSet, maxIterations, stall_epsilon, raise_epsilon),
     nodeSet(nodeSet),
     stepSize(stepSize)
 {

VirtualRobot/IK/ConstrainedIK.cpp

@@ -2,14 +2,16 @@
 
 using namespace VirtualRobot;
 
-ConstrainedIK::ConstrainedIK(RobotPtr& robot, int maxIterations, float stall_epsilon, float raise_epsilon) :
+ConstrainedIK::ConstrainedIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, int maxIterations, float stall_epsilon, float raise_epsilon) :
     robot(robot),
+    nodeSet(nodeSet),
     maxIterations(maxIterations),
     currentIteration(0),
     running(false),
     stallEpsilon(stall_epsilon),
     raiseEpsilon(raise_epsilon)
 {
+    addSeed(eSeedInitial);
 }
 
 void ConstrainedIK::addConstraint(const ConstraintPtr& constraint, int priority, bool hard_constraint)
@@ -40,11 +42,23 @@ std::vector<ConstraintPtr> ConstrainedIK::getConstraints()
     return constraints;
 }
 
+void ConstrainedIK::addSeed(ConstrainedIK::SeedType type, const Eigen::VectorXf seed)
+{
+    seeds.push_back(std::pair<SeedType, Eigen::VectorXf>(type, seed));
+}
+
+void ConstrainedIK::clearSeeds()
+{
+    seeds.clear();
+}
+
 bool ConstrainedIK::initialize()
 {
     Eigen::Matrix4f P;
     int moves = 0;
 
+    nodeSet->getJointValues(initialConfig);
+
     for (auto & constraint : constraints)
     {
         constraint->initialize();
@@ -69,6 +83,12 @@ bool ConstrainedIK::initialize()
 
 bool ConstrainedIK::solve(bool stepwise)
 {
+    if(seeds.size() != 1)
+    {
+        // TODO: Implement seeds for IKs other that ConstrainedOptimizationIK
+        VR_WARNING << "Seeds not supported by this IK implementation" << std::endl;
+    }
+
     while (currentIteration < maxIterations)
     {
         if (!solveStep())

VirtualRobot/IK/ConstrainedIK.h

@@ -35,12 +35,28 @@ namespace VirtualRobot
     class VIRTUAL_ROBOT_IMPORT_EXPORT ConstrainedIK : public boost::enable_shared_from_this<ConstrainedIK>
     {
     public:
-        ConstrainedIK(RobotPtr& robot, int maxIterations = 1000, float stall_epsilon = 0.0001, float raise_epsilon = 0.8);
+        enum SeedType
+        {
+            // Zero configuration
+            eSeedZero,
+
+            // The configuration that was applied when initialize() was called
+            eSeedInitial,
+
+            // Some other configuration
+            eSeedOther
+        };
+
+    public:
+        ConstrainedIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, int maxIterations = 1000, float stall_epsilon = 0.0001, float raise_epsilon = 0.8);
 
         void addConstraint(const ConstraintPtr& constraint, int priority = 0, bool hard_constraint = true);
         void removeConstraint(const ConstraintPtr& constraint);
         std::vector<ConstraintPtr> getConstraints();
 
+        void addSeed(SeedType type, const Eigen::VectorXf seed = Eigen::VectorXf());
+        void clearSeeds();
+
         virtual bool initialize();
 
         virtual bool solveStep() = 0;
@@ -56,7 +72,12 @@ namespace VirtualRobot
         std::vector<ConstraintPtr> constraints;
         std::map<ConstraintPtr, int> priorities;
         std::map<ConstraintPtr, bool> hardConstraints;
+
         RobotPtr robot;
+        RobotNodeSetPtr nodeSet;
+        Eigen::VectorXf initialConfig;
+
+        std::vector<std::pair<SeedType, Eigen::VectorXf>> seeds;
 
         int maxIterations;
         int currentIteration;

VirtualRobot/IK/ConstrainedOptimizationIK.cpp

@@ -27,17 +27,21 @@
 using namespace VirtualRobot;
 
 ConstrainedOptimizationIK::ConstrainedOptimizationIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float timeout, float tolerance) :
-    ConstrainedIK(robot),
+    ConstrainedIK(robot, nodeSet),
     nodeSet(nodeSet),
     timeout(timeout),
     tolerance(tolerance),
     maxAttempts(30)
 {
+    clearSeeds();
+    addSeed(eSeedInitial);
+    addSeed(eSeedZero);
 }
 
 bool ConstrainedOptimizationIK::initialize()
 {
     int size = nodeSet->getSize();
+    nodeSet->getJointValues(initialConfig);
 
     optimizer.reset(new nlopt::opt(nlopt::LD_SLSQP, size));
 
@@ -91,20 +95,46 @@ bool ConstrainedOptimizationIK::solve(bool stepwise)
 
         int size = nodeSet->getSize();
         std::vector<double> x(size);
-        for(int i = 0; i < size; i++)
+
+        if(attempt >= seeds.size())
         {
-            if(attempt == 0)
-            {
-                // First try zero-position
-                x[i] = 0;
-            }
-            else
+            // Try random configurations
+            for(int i = 0; i < size; i++)
             {
-                // If zero position fails, try random positions
                 float t = (rand()%1000) / 1000.0;
                 x[i] = nodeSet->getNode(i)->getJointLimitLo() + t * (nodeSet->getNode(i)->getJointLimitHi() - nodeSet->getNode(i)->getJointLimitLo());
             }
         }
+        else
+        {
+            switch(seeds[attempt].first)
+            {
+                case eSeedZero:
+                    // Try zero configuration
+                    for(int i = 0; i < size; i++)
+                    {
+                        x[i] = 0;
+                    }
+                    break;
+
+                case eSeedInitial:
+                    // Try initial configuration
+                    for(int i = 0; i < size; i++)
+                    {
+                        x[i] = initialConfig(i);
+                    }
+                    break;
+
+                case eSeedOther:
+                    // Try used specified seed
+                    Eigen::VectorXf s = seeds[attempt].second;
+                    for(int i = 0; i < size; i++)
+                    {
+                        x[i] = s(i);
+                    }
+                    break;
+            }
+        }
 
         double min_f;
 

VirtualRobot/IK/ConstrainedStackedIK.cpp

@@ -3,7 +3,7 @@
 using namespace VirtualRobot;
 
 ConstrainedStackedIK::ConstrainedStackedIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float stepSize, int maxIterations, JacobiProvider::InverseJacobiMethod method) :
-    ConstrainedIK(robot, maxIterations),
+    ConstrainedIK(robot, nodeSet, maxIterations),
     nodeSet(nodeSet),
     method(method),
     stepSize(stepSize)