use undamped jacobian in nullspace projection

4am-robotics · Jan 16, 2016 · 1dfbe92 · 1dfbe92
1 parent 10cf6d0
commit 1dfbe92
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 14 deletions.
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
@@ -45,10 +45,10 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_
 
     Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;
 
-    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 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());
@@ -66,6 +66,7 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_
 
     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 pinv = pinv_calc_.calculate(this->params_, this->damping_, 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_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 particular_solution = pinv * in_cart_velocities;
 
     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,10 +116,10 @@ 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();

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 pinv = pinv_calc_.calculate(this->params_, this->damping_, 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_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 particular_solution = pinv * in_cart_velocities;
 
     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);