VirtualRobot: Removed JacobiDefinition structure

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

VirtualRobot/IK/ConstrainedHierarchicalIK.cpp

@@ -15,9 +15,7 @@ bool ConstrainedHierarchicalIK::initialize()
 
     for(auto &constraint : constraints)
     {
-        HierarchicalIK::JacobiDefinition jac;
-        jac.jacProvider = constraint;
-        jacobians.push_back(jac);
+        jacobians.push_back(constraint);
     }
 
     ConstrainedIK::initialize();

VirtualRobot/IK/ConstrainedHierarchicalIK.h

@@ -34,16 +34,16 @@ namespace VirtualRobot
     class VIRTUAL_ROBOT_IMPORT_EXPORT ConstrainedHierarchicalIK : public ConstrainedIK, public boost::enable_shared_from_this<ConstrainedHierarchicalIK>
     {
         public:
-            ConstrainedHierarchicalIK(VirtualRobot::RobotPtr &robot, const VirtualRobot::RobotNodeSetPtr &nodeSet);
+            ConstrainedHierarchicalIK(RobotPtr &robot, const RobotNodeSetPtr &nodeSet);
 
             bool initialize();
             bool solveStep();
 
         protected:
-            VirtualRobot::RobotNodeSetPtr nodeSet;
-            VirtualRobot::HierarchicalIKPtr ik;
+            RobotNodeSetPtr nodeSet;
+            HierarchicalIKPtr ik;
 
-            std::vector<VirtualRobot::HierarchicalIK::JacobiDefinition> jacobians;
+            std::vector<JacobiProviderPtr> jacobians;
     };
 
     typedef boost::shared_ptr<ConstrainedHierarchicalIK> ConstrainedHierarchicalIKPtr;

VirtualRobot/IK/ConstrainedStackedIK.cpp

@@ -16,9 +16,7 @@ bool ConstrainedStackedIK::initialize()
 
     for(auto &constraint : constraints)
     {
-        HierarchicalIK::JacobiDefinition jac;
-        jac.jacProvider = constraint;
-        jacobians.push_back(jac);
+        jacobians.push_back(constraint);
     }
 
     ConstrainedIK::initialize();

VirtualRobot/IK/ConstrainedStackedIK.h

@@ -45,7 +45,7 @@ namespace VirtualRobot
             RobotNodeSetPtr nodeSet;
             StackedIKPtr ik;
 
-            std::vector<HierarchicalIK::JacobiDefinition> jacobians;
+            std::vector<JacobiProviderPtr> jacobians;
 
             JacobiProvider::InverseJacobiMethod method;
     };

VirtualRobot/IK/GazeIK.cpp

@@ -86,7 +86,7 @@ namespace VirtualRobot
     {
         VR_ASSERT(ikSolver && ikGaze && virtualTranslationJoint);
 
-        std::vector<HierarchicalIK::JacobiDefinition> jacobies;
+        std::vector<JacobiProviderPtr> jacobies;
 
         Eigen::Matrix4f currentPose = rns->getTCP()->getGlobalPose(); // the "tcp" of the gaze RNS -> the gaze point which is moved via a virtual translational joint
         Eigen::Matrix4f g;
@@ -101,18 +101,12 @@ namespace VirtualRobot
             VR_INFO << "ikGaze delta:\n" << deltaGaze.head(3) << endl;
         }
 
-        HierarchicalIK::JacobiDefinition jd;
-        jd.jacProvider = ikGaze;
-        //jd.delta = deltaGaze.head(3);
-        jacobies.push_back(jd);
+        jacobies.push_back(ikGaze);
 
         // 2. jl avoidance
         if (ikJointLimits)
         {
-            HierarchicalIK::JacobiDefinition jd2;
-            jd2.jacProvider = ikJointLimits;
-            //jd2.delta = ikJointLimits->getError();
-            jacobies.push_back(jd2);
+            jacobies.push_back(ikJointLimits);
         }
 
         //compute step

VirtualRobot/IK/HierarchicalIK.cpp

@@ -24,7 +24,7 @@ namespace VirtualRobot
         verbose = v;
     }
 
-    Eigen::VectorXf HierarchicalIK::computeStep(std::vector<JacobiDefinition> jacDefs, float stepSize)
+    Eigen::VectorXf HierarchicalIK::computeStep(std::vector<JacobiProviderPtr> jacDefs, float stepSize)
     {
         VR_ASSERT(jacDefs.size() > 0 && jacDefs[0].jacProvider && jacDefs[0].jacProvider->getRobotNodeSet());
 
@@ -33,7 +33,7 @@ namespace VirtualRobot
             VR_INFO << "Compute Step" << endl;
         }
 
-        int ndof = jacDefs[0].jacProvider->getRobotNodeSet()->getSize();
+        int ndof = jacDefs[0]->getRobotNodeSet()->getSize();
         Eigen::VectorXf result(ndof);
         result.setZero();
         std::vector<Eigen::MatrixXf> jacobies;
@@ -41,8 +41,8 @@ namespace VirtualRobot
 
         for (size_t i = 0; i < jacDefs.size(); i++)
         {
-            THROW_VR_EXCEPTION_IF(!jacDefs[i].jacProvider->isInitialized(), "JacobiProvider is not initialized...");
-            Eigen::MatrixXf j = jacDefs[i].jacProvider->getJacobianMatrix();// jacDefs[i].tcp);
+            THROW_VR_EXCEPTION_IF(!jacDefs[i]->isInitialized(), "JacobiProvider is not initialized...");
+            Eigen::MatrixXf j = jacDefs[i]->getJacobianMatrix();// jacDefs[i].tcp);
             jacobies.push_back(j);
 
             if (verbose)
@@ -50,7 +50,7 @@ namespace VirtualRobot
                 VR_INFO << "Jacoby " << i << ":\n" << j << endl;
             }
 
-            j = jacDefs[i].jacProvider->computePseudoInverseJacobianMatrix(j);// jacDefs[i].tcp);
+            j = jacDefs[i]->computePseudoInverseJacobianMatrix(j);// jacDefs[i].tcp);
             invJacobies.push_back(j);
 
             if (verbose)
@@ -63,9 +63,9 @@ namespace VirtualRobot
                 THROW_VR_EXCEPTION("Expecting " << ndof << " DOFs, but Jacobi " << i << " has " << jacobies[i].cols() << " columns ");
             }
 
-            if (jacobies[i].rows() != jacDefs[i].jacProvider->getError().rows())
+            if (jacobies[i].rows() != jacDefs[i]->getError().rows())
             {
-                THROW_VR_EXCEPTION("Jacobi " << i << " has " << jacobies[i].rows() << " rows, but delta has " << jacDefs[i].jacProvider->getError().rows() << " rows ");
+                THROW_VR_EXCEPTION("Jacobi " << i << " has " << jacobies[i].rows() << " rows, but delta has " << jacDefs[i]->getError().rows() << " rows ");
             }
         }
 
@@ -77,7 +77,7 @@ namespace VirtualRobot
 
 
         Eigen::MatrixXf Jinv_i_min1;
-        Eigen::VectorXf result_i = Jinv_i * jacDefs[0].jacProvider->getError() * stepSize;
+        Eigen::VectorXf result_i = Jinv_i * jacDefs[0]->getError() * stepSize;
 
         if (verbose)
         {
@@ -137,10 +137,10 @@ namespace VirtualRobot
             }
             if (verbose)
             {
-                VR_INFO << "jacDefs[i].jacProvider->getError() " << i << ":\n" << endl << jacDefs[i].jacProvider->getError().transpose() << endl;
+                VR_INFO << "jacDefs[i].jacProvider->getError() " << i << ":\n" << endl << jacDefs[i]->getError().transpose() << endl;
             }
 
-            result_i = result_i_min1 + Jinv_tilde_i * (jacDefs[i].jacProvider->getError() * stepSize - J_i * result_i_min1);
+            result_i = result_i_min1 + Jinv_tilde_i * (jacDefs[i]->getError() * stepSize - J_i * result_i_min1);
 
             if (verbose)
             {

VirtualRobot/IK/HierarchicalIK.h

@@ -52,13 +52,6 @@ namespace VirtualRobot
 
         virtual ~HierarchicalIK();
 
-        struct JacobiDefinition
-        {
-            JacobiProviderPtr jacProvider; // generates the Jacobi and the PseudoInverse
-            // not needed any more, we use the getError method
-            //      Eigen::VectorXf delta;         // Specifies the delta for the Jacobi (e.g. in workspace). delta.rows() must be equal to jacobi.rows().
-        };
-
         /*!
             computes hierarchical Jacobi step
             \param jacDefs All Jacobians an the corresponding error vectors that should be considered for computing a gradient step.
@@ -66,7 +59,7 @@ namespace VirtualRobot
                            The deltas specify the error for each Jacobian (e.g. in workspace). deltas[i].rows() must be equal to jacobies[i].rows().
             \param stepSize The deltas can be reduced in order to avoid oscillating behavior.
         */
-        Eigen::VectorXf computeStep(std::vector<JacobiDefinition> jacDefs, float stepSize = 0.2f);
+        Eigen::VectorXf computeStep(std::vector<JacobiProviderPtr> jacDefs, float stepSize = 0.2f);
 
         void setVerbose(bool v);
     protected:

VirtualRobot/IK/StackedIK.cpp

@@ -44,19 +44,19 @@ namespace VirtualRobot
         verbose = v;
     }
 
-    Eigen::VectorXf StackedIK::computeStep(std::vector<HierarchicalIK::JacobiDefinition> jacDefs, float stepSize)
+    Eigen::VectorXf StackedIK::computeStep(std::vector<JacobiProviderPtr> jacDefs, float stepSize)
     {
         VR_ASSERT(jacDefs.size() > 0 && jacDefs[0].jacProvider && jacDefs[0].jacProvider->getRobotNodeSet());
 
         // Get dimensions of stacked jacobian
         int total_rows = 0;
-        int total_cols = jacDefs[0].jacProvider->getJacobianMatrix().cols();
+        int total_cols = jacDefs[0]->getJacobianMatrix().cols();
         for(int i = 0; i < jacDefs.size(); i++)
         {
-            THROW_VR_EXCEPTION_IF(!jacDefs[0].jacProvider->isInitialized(), "JacobiProvider is not initialized");
+            THROW_VR_EXCEPTION_IF(!jacDefs[0]->isInitialized(), "JacobiProvider is not initialized");
 
-            Eigen::MatrixXf J = jacDefs[i].jacProvider->getJacobianMatrix();
-            Eigen::VectorXf e = jacDefs[i].jacProvider->getError();
+            Eigen::MatrixXf J = jacDefs[i]->getJacobianMatrix();
+            Eigen::VectorXf e = jacDefs[i]->getError();
 
             THROW_VR_EXCEPTION_IF(J.cols() != total_cols, "Jacobian size mismatch");
             THROW_VR_EXCEPTION_IF(J.rows() != e.rows(), "Jacobian matrix and error vector don't match");
@@ -71,9 +71,9 @@ namespace VirtualRobot
         int current_row = 0;
         for(int i = 0; i < jacDefs.size(); i++)
         {
-            Eigen::MatrixXf J = jacDefs[i].jacProvider->getJacobianMatrix();
+            Eigen::MatrixXf J = jacDefs[i]->getJacobianMatrix();
             jacobian.block(current_row, 0, J.rows(), J.cols()) = J;
-            error.block(current_row, 0, J.rows(), 1) = jacDefs[i].jacProvider->getError();
+            error.block(current_row, 0, J.rows(), 1) = jacDefs[i]->getError();
 
             current_row += J.rows();
         }

VirtualRobot/IK/StackedIK.h

@@ -42,7 +42,7 @@ namespace VirtualRobot
 
         virtual ~StackedIK();
 
-        Eigen::VectorXf computeStep(std::vector<HierarchicalIK::JacobiDefinition> jacDefs, float stepSize = 0.2f);
+        Eigen::VectorXf computeStep(std::vector<JacobiProviderPtr> jacDefs, float stepSize = 0.2f);
 
         void setVerbose(bool v);