Solver files...

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

VirtualRobot/IK/HierarchicalIKSolver.cpp

+#include "HierarchicalIKSolver.h"
+//#define DEBUG
+
+HierarchicalIKSolver::HierarchicalIKSolver(RobotNodeSetPtr allRobotNodes) : HierarchicalIK(allRobotNodes)
+{
+    //rns = allRobotNodes;
+}
+
+bool HierarchicalIKSolver::solveIK( float stepSize, float minChange, int maxSteps)
+{
+    return computeSteps(stepSize,minChange,maxSteps);
+}
+
+bool HierarchicalIKSolver::computeSteps(float stepSize, float minChange, int maxSteps) {
+    //std::vector<RobotNodePtr> rn = rns->getAllRobotNodes();
+    //RobotPtr robot = rns->getRobot();
+    //std::vector<float> jv(rns->getSize(),0.0f);
+    int step = 0;
+    checkTolerances();
+    //float lastDist = FLT_MAX;
+
+    while (step < maxSteps) {
+        Eigen::VectorXf delta = computeStep(jacobies, stepSize);
+
+        if(!MathTools::isValid(delta)) {
+#ifdef DEBUG
+            VR_INFO << "Singular Jacobian" << endl;
+#endif
+            return false;
+        }
+
+        Eigen::VectorXf jv(delta.rows());
+        rns->getJointValues(jv);
+        jv += delta;
+        rns->setJointValues(jv);
+
+        if(checkTolerances()) {
+#ifdef DEBUG
+            VR_INFO << "Tolerances ok, loop:" << step << endl;
+#endif
+            return true;
+        }
+
+        if (delta.norm()<minChange)
+        {
+#ifdef DEBUG
+            VR_INFO << "Could not improve result any more (dTheta.norm()=" << delta.norm() << "), loop:" << step << endl;
+#endif
+            return false;
+        }
+
+        step++;
+    }
+    return false;
+}
+
+bool HierarchicalIKSolver::checkTolerances() {
+    for(int i = 0; i < jacobies.size(); i++) {
+        if (!jacobies[i]->checkTolerances())
+            return false;
+    }
+    return true;
+}
+
+void HierarchicalIKSolver::addIK(JacobiProviderPtr jacProvider)
+{
+    jacobies.push_back(jacProvider);
+}
+
+void HierarchicalIKSolver::clearIKs() {
+    jacobies.clear();
+}

VirtualRobot/IK/HierarchicalIKSolver.h

+#ifndef HIERARCHICALIKSOLVER_H
+#define HIERARCHICALIKSOLVER_H
+
+#include "HierarchicalIK.h"
+
+using namespace VirtualRobot;
+
+class HierarchicalIKSolver : public HierarchicalIK, public boost::enable_shared_from_this<HierarchicalIKSolver> {
+
+public:
+    HierarchicalIKSolver(RobotNodeSetPtr allRobotNodes);
+    bool solveIK(float stepSize = 0.2, float minChange = 0.0, int maxSteps = 50);
+    bool computeSteps(float stepSize, float minChange, int maxSteps);
+    void addIK(JacobiProviderPtr jacProvider);
+
+    bool checkTolerances();
+    void clearIKs();
+protected:
+
+    std::vector<JacobiProviderPtr> jacobies;
+};
+
+typedef boost::shared_ptr<HierarchicalIKSolver> HierarchicalIKSolverPtr;
+#endif // HIERARCHICALIKSOLVER_H