Merge pull request #78 from ipa-fxm/investigate_damped_gpm_computation

use undamped jacobian in nullspace projection
4am-robotics · Jan 26, 2016 · 1f61b3b · 1f61b3b
2 parents a24bc30 + 1dfbe92
commit 1f61b3b
Show file tree

Hide file tree

Showing 8 changed files with 72 additions and 64 deletions.
diff --git a/...ntroller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h b/...ntroller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h
@@ -47,7 +47,7 @@ class ConstraintSolver
     public:
         /**
          * The interface method to solve the inverse kinematics problem. Has to be implemented in inherited classes.
-         * @param inCartVelocities The input velocities vector (in cartesian space).
+         * @param in_cart_velocities The input velocities vector (in cartesian space).
          * @param joint_states The joint states with history.
          * @return The calculated new joint velocities.
          */

diff --git a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
@@ -37,18 +37,18 @@
  * In addtion to the partial solution q_dot = J^+ * v the homogeneous solution (I - J^+ * J) q_dot_0 is calculated.
  * The q_dot_0 results from the sum of the constraint cost function gradients. The terms of the sum are weighted with a factor k_H separately.
  */
-Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& inCartVelocities,
+Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_velocities,
                                                       const JointStates& joint_states)
 {
-    Eigen::MatrixXd damped_jpi = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
-    Eigen::MatrixXd jpi = pinv_calc_.calculate(this->jacobian_data_);
+    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
+    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
 
-    Eigen::MatrixXd particular_solution = damped_jpi * inCartVelocities;
+    Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;
 
-    Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(damped_jpi.rows(), this->jacobian_data_.cols());
-    Eigen::MatrixXd projector = ident - damped_jpi * this->jacobian_data_;
-    // Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jpi.rows(), this->jacobian_data_.cols());
-    // Eigen::MatrixXd projector = ident - jpi * this->jacobian_data_;
+    // Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(damped_pinv.rows(), this->jacobian_data_.cols());
+    // Eigen::MatrixXd projector = ident - damped_pinv * this->jacobian_data_;
+    Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(pinv.rows(), this->jacobian_data_.cols());
+    Eigen::MatrixXd projector = ident - pinv * this->jacobian_data_;
 
     Eigen::MatrixXd homogeneous_solution = Eigen::MatrixXd::Zero(particular_solution.rows(), particular_solution.cols());
     KDL::JntArrayVel predict_jnts_vel(joint_states.current_q_.rows());
@@ -58,14 +58,15 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& inCartVe
         ROS_DEBUG_STREAM("task id: " << (*it)->getTaskId());
         (*it)->update(joint_states, predict_jnts_vel, this->jacobian_data_);
         Eigen::VectorXd q_dot_0 = (*it)->getPartialValues();
-        Eigen::MatrixXd tmpHomogeneousSolution = projector * q_dot_0;
+        Eigen::MatrixXd tmp_projection = projector * q_dot_0;
         double activation_gain = (*it)->getActivationGain();  // contribution of the homo. solution to the part. solution
-        double constraint_k_H = (*it)->getSelfMotionMagnitude(particular_solution, tmpHomogeneousSolution);  // gain of homogenous solution (if active)
-        homogeneous_solution += (constraint_k_H * activation_gain * tmpHomogeneousSolution);
+        double constraint_k_H = (*it)->getSelfMotionMagnitude(particular_solution, tmp_projection);  // gain of homogenous solution (if active)
+        homogeneous_solution += (constraint_k_H * activation_gain * tmp_projection);
     }
 
     Eigen::MatrixXd qdots_out = particular_solution + this->params_.k_H * homogeneous_solution;  // weighting with k_H is done in loop
 
+    // //DEBUG: for verification of nullspace projection
     // std::stringstream ss_part;
     // ss_part << "particular_solution: ";
     // for(unsigned int i=0; i<particular_solution.rows(); i++)

diff --git a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
@@ -40,10 +40,13 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
     double cycle = (now - this->last_time_).toSec();
     this->last_time_ = now;
 
-    Eigen::MatrixXd jacobianPseudoInverse = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
-    Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jacobianPseudoInverse.rows(), this->jacobian_data_.cols());
-    Eigen::MatrixXd projector = ident - jacobianPseudoInverse * this->jacobian_data_;
-    Eigen::MatrixXd particular_solution = jacobianPseudoInverse * in_cart_velocities;
+    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
+    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
+
+    Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;
+
+    Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(pinv.rows(), this->jacobian_data_.cols());
+    Eigen::MatrixXd projector = ident - pinv * this->jacobian_data_;
 
     Eigen::MatrixXd projector_i = Eigen::MatrixXd::Identity(this->jacobian_data_.cols(), this->jacobian_data_.cols());
     Eigen::VectorXd q_i = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
@@ -102,7 +105,7 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
         Eigen::MatrixXd J_task = it->task_jacobian_;
         Eigen::MatrixXd J_temp = J_task * projector_i;
         Eigen::VectorXd v_task = it->task_;
-        Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(it->tcp_, it->db_, J_temp);
+        Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp);
         q_i = q_i + J_temp_inv * (v_task - J_task * q_i);
         projector_i = projector_i - J_temp_inv * J_temp;
     }
@@ -113,15 +116,15 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
 
 
 void StackOfTasksSolver::processState(std::set<ConstraintBase_t>::iterator& it,
-                                              const Eigen::MatrixXd& projector,
-                                              const Eigen::MatrixXd& particular_solution,
-                                              double inv_sum_of_prios,
-                                              Eigen::VectorXd& sum_of_gradient)
+                                      const Eigen::MatrixXd& projector,
+                                      const Eigen::MatrixXd& particular_solution,
+                                      double inv_sum_of_prios,
+                                      Eigen::VectorXd& sum_of_gradient)
 {
     Eigen::VectorXd q_dot_0 = (*it)->getPartialValues();
     const double activation_gain = (*it)->getActivationGain();
-    Eigen::MatrixXd tmpHomogeneousSolution = projector * q_dot_0;
-    const double magnitude = (*it)->getSelfMotionMagnitude(particular_solution, tmpHomogeneousSolution);
+    Eigen::MatrixXd tmp_projection = projector * q_dot_0;
+    const double magnitude = (*it)->getSelfMotionMagnitude(particular_solution, tmp_projection);
     ConstraintState cstate = (*it)->getState();
 
     const double constr_prio = (*it)->getPriorityAsNum();

diff --git a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
@@ -48,10 +48,13 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
 
     Eigen::MatrixXd qdots_out = Eigen::MatrixXd::Zero(this->jacobian_data_.cols(), 1);
     Eigen::VectorXd partial_cost_func = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
-    Eigen::MatrixXd jacobianPseudoInverse = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
-    Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jacobianPseudoInverse.rows(), this->jacobian_data_.cols());
-    Eigen::MatrixXd projector = ident - jacobianPseudoInverse * this->jacobian_data_;
-    Eigen::MatrixXd particular_solution = jacobianPseudoInverse * in_cart_velocities;
+    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
+    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
+
+    Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;
+
+    Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(pinv.rows(), this->jacobian_data_.cols());
+    Eigen::MatrixXd projector = ident - pinv * this->jacobian_data_;
 
     KDL::JntArrayVel predict_jnts_vel(joint_states.current_q_.rows());
 
@@ -81,7 +84,7 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
         if (activation_gain > 0.0)
         {
             Eigen::MatrixXd tmp_matrix = partial_cost_func.transpose() * projector;
-            jac_inv_2nd_term = pinv_calc_.calculate(this->params_, this->damping_, tmp_matrix);
+            jac_inv_2nd_term = pinv_calc_.calculate(tmp_matrix);
         }
 
         Eigen::MatrixXd m_derivative_cost_func_value = derivative_cost_func_value * Eigen::MatrixXd::Identity(1, 1);

diff --git a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
@@ -35,11 +35,11 @@
  * It calculates the pseudo-inverse of the Jacobian via the base implementation of calculatePinvJacobianBySVD.
  * With the pseudo-inverse the joint velocity vector is calculated.
  */
-Eigen::MatrixXd UnconstraintSolver::solve(const Vector6d_t& inCartVelocities,
+Eigen::MatrixXd UnconstraintSolver::solve(const Vector6d_t& in_cart_velocities,
                                           const JointStates& joint_states)
 {
-    Eigen::MatrixXd jacobianPseudoInverse = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
-    Eigen::MatrixXd qdots_out = jacobianPseudoInverse * inCartVelocities;
+    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
+    Eigen::MatrixXd qdots_out = pinv * in_cart_velocities;
     return qdots_out;
 }
 
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
@@ -43,10 +43,11 @@ Eigen::MatrixXd WeightedLeastNormSolver::solve(const Vector6d_t& in_cart_velocit
     Eigen::MatrixXd inv_root_W_WLN =  root_W_WLN.inverse();     // -> W^(-1/2)
 
     // SVD of JLA weighted Jacobian: Damping will be done later in calculatePinvJacobianBySVD for pseudo-inverse Jacobian with additional truncation etc.
-    Eigen::MatrixXd weighted_jacobian_pseudoinverse = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_ * inv_root_W_WLN);
+    Eigen::MatrixXd weighted_jacobian = this->jacobian_data_ * inv_root_W_WLN;
+    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, weighted_jacobian);
 
     // Take care: W^(1/2) * q_dot = weighted_pinv_J * x_dot -> One must consider the weighting!!!
-    Eigen::MatrixXd qdots_out = inv_root_W_WLN * weighted_jacobian_pseudoinverse * in_cart_velocities;
+    Eigen::MatrixXd qdots_out = inv_root_W_WLN * pinv * in_cart_velocities;
     return qdots_out;
 }
 
@@ -56,6 +57,6 @@ Eigen::MatrixXd WeightedLeastNormSolver::solve(const Vector6d_t& in_cart_velocit
 Eigen::MatrixXd WeightedLeastNormSolver::calculateWeighting(const JointStates& joint_states) const
 {
     uint32_t cols = this->jacobian_data_.cols();
-    Eigen::VectorXd output = Eigen::VectorXd::Ones(cols);
-    return output.asDiagonal();
+    Eigen::VectorXd weighting = Eigen::VectorXd::Ones(cols);
+    return weighting.asDiagonal();
 }
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp
@@ -41,14 +41,14 @@ Eigen::MatrixXd WLN_JointLimitAvoidanceSolver::calculateWeighting(const JointSta
     std::vector<double> limits_min = this->limiter_params_.limits_min;
     std::vector<double> limits_max = this->limiter_params_.limits_max;
     uint32_t cols = this->jacobian_data_.cols();
-    Eigen::VectorXd output = Eigen::VectorXd::Zero(cols);
+    Eigen::VectorXd weighting = Eigen::VectorXd::Zero(cols);
 
     KDL::JntArray q = joint_states.current_q_;
     KDL::JntArray q_dot = joint_states.current_q_dot_;
 
     for (uint32_t i = 0; i < cols ; ++i)
     {
-        output(i) = 1.0;    // in the else cases -> output always 1
+        weighting(i) = 1.0;    // in the else cases -> weighting always 1
         if (i < q.rows())
         {
             // See Chan paper ISSN 1042-296X [Page 288]
@@ -61,11 +61,11 @@ Eigen::MatrixXd WLN_JointLimitAvoidanceSolver::calculateWeighting(const JointSta
                 if (denominator != 0.0)
                 {
                     double partialPerformanceCriterion = fabs(nominator / denominator);
-                    output(i) = 1 + partialPerformanceCriterion;
+                    weighting(i) = 1 + partialPerformanceCriterion;
                 }
             }
         }
     }
 
-    return output.asDiagonal();
+    return weighting.asDiagonal();
 }
diff --git a/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp b/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
@@ -50,9 +50,9 @@ Eigen::MatrixXd PInvBySVD::calculate(const Eigen::MatrixXd& jacobian) const
         singularValuesInv(i) = (singularValues(i) < eps_truncation) ? 0.0 : singularValues(i) / denominator;
     }
 
-    Eigen::MatrixXd pseudoInverseJacobian = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
+    Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
 
-    return pseudoInverseJacobian;
+    return result;
 }
 
 /**
@@ -68,24 +68,24 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
     Eigen::VectorXd singularValues = svd.singularValues();
     Eigen::VectorXd singularValuesInv = Eigen::VectorXd::Zero(singularValues.rows());
     double lambda = db->getDampingFactor(singularValues, jacobian);
-    uint32_t i = 0;
 
     if (params.numerical_filtering)
     {
         // Formula 20 Singularity-robust Task-priority Redundandancy Resolution
         // Sum part
-        for (; i < singularValues.rows()-1; ++i)
+        for (uint32_t i = 0; i < singularValues.rows()-1; ++i)
         {
             // pow(beta, 2) << pow(lambda, 2)
             singularValuesInv(i) = singularValues(i) / (pow(singularValues(i), 2) + pow(params.beta, 2));
         }
-        // Formula 20 - additional part
-        singularValuesInv(i) = singularValues(i) / (pow(singularValues(i), 2) + pow(params.beta, 2) + pow(lambda, 2));
+        // Formula 20 - additional part - numerical filtering for least singular value m
+        uint32_t m = singularValues.rows();
+        singularValuesInv(m) = singularValues(m) / (pow(singularValues(m), 2) + pow(params.beta, 2) + pow(lambda, 2));
     }
     else
     {
         // small change to ref: here quadratic damping due to Control of Redundant Robot Manipulators : R.V. Patel, 2005, Springer [Page 13-14]
-        for (; i < singularValues.rows(); ++i)
+        for (uint32_t i = 0; i < singularValues.rows(); ++i)
         {
             double denominator = (singularValues(i) * singularValues(i) + pow(lambda, 2) );
             // singularValuesInv(i) = (denominator < eps_truncation) ? 0.0 : singularValues(i) / denominator;
@@ -100,9 +100,9 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
         // }
     }
 
-    Eigen::MatrixXd pseudoInverseJacobian = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
+    Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
 
-    return pseudoInverseJacobian;
+    return result;
 }
 
 /**
@@ -111,17 +111,17 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
 Eigen::MatrixXd PInvDirect::calculate(const Eigen::MatrixXd& jacobian) const
 {
     Eigen::MatrixXd result;
-    Eigen::MatrixXd j_t = jacobian.transpose();
-    uint32_t jac_rows = jacobian.rows();
-    uint32_t jac_cols = jacobian.cols();
+    Eigen::MatrixXd jac_t = jacobian.transpose();
+    uint32_t rows = jacobian.rows();
+    uint32_t cols = jacobian.cols();
 
-    if (jac_cols >= jac_rows)
+    if (cols >= rows)
     {
-        result = j_t * (jacobian * j_t).inverse();
+        result = jac_t * (jacobian * jac_t).inverse();
     }
     else
     {
-        result = (j_t * jacobian).inverse() * j_t;
+        result = (jac_t * jacobian).inverse() * jac_t;
     }
 
     return result;
@@ -135,26 +135,26 @@ Eigen::MatrixXd PInvDirect::calculate(const TwistControllerParams& params,
                                       const Eigen::MatrixXd& jacobian) const
 {
     Eigen::MatrixXd result;
-    Eigen::MatrixXd j_t = jacobian.transpose();
-    uint32_t jac_rows = jacobian.rows();
-    uint32_t jac_cols = jacobian.cols();
+    Eigen::MatrixXd jac_t = jacobian.transpose();
+    uint32_t rows = jacobian.rows();
+    uint32_t cols = jacobian.cols();
     if (params.damping_method == LEAST_SINGULAR_VALUE)
     {
         ROS_ERROR("PInvDirect does not support SVD. Use PInvBySVD class instead!");
     }
 
     double lambda = db->getDampingFactor(Eigen::VectorXd::Zero(1, 1), jacobian);
-    if (jac_cols >= jac_rows)
+    if (cols >= rows)
     {
-        Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jac_rows, jac_rows);
-        Eigen::MatrixXd toBeInv = jacobian * j_t + lambda * lambda * ident;
-        result = j_t * toBeInv.inverse();
+        Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(rows, rows);
+        Eigen::MatrixXd temp = jacobian * jac_t + lambda * lambda * ident;
+        result = jac_t * temp.inverse();
     }
     else
     {
-        Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(jac_cols, jac_cols);
-        Eigen::MatrixXd toBeInv = j_t * jacobian + lambda * lambda * ident;
-        result = toBeInv.inverse() * j_t;
+        Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(cols, cols);
+        Eigen::MatrixXd temp = jac_t * jacobian + lambda * lambda * ident;
+        result = temp.inverse() * jac_t;
     }
 
     return result;