From 58be1d3b16347e4d9afaa924b988c72ae04ad8e7 Mon Sep 17 00:00:00 2001 From: Bruno Brito Date: Thu, 12 Jan 2017 15:25:49 +0100 Subject: [PATCH 1/5] Null space projection matrix rank is analised to avoid unnecessary calculations --- .../gradient_projection_method_solver.cpp | 30 ++++++++++++------- 1 file changed, 20 insertions(+), 10 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 index 6f3e8f69..7950555b 100644 --- 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 @@ -52,19 +52,29 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_ Eigen::MatrixXd homogeneous_solution = Eigen::MatrixXd::Zero(particular_solution.rows(), particular_solution.cols()); KDL::JntArrayVel predict_jnts_vel(joint_states.current_q_.rows()); + Eigen::FullPivLU lu_decomp(projector); + Eigen::MatrixXd qdots_out; - for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) + if(lu_decomp.rank() != 0) { - 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 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, 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 + for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) + { + 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 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, tmp_projection); // gain of homogenous solution (if active) + homogeneous_solution += (constraint_k_H * activation_gain * tmp_projection); + } + + qdots_out = particular_solution + this->params_.k_H * homogeneous_solution; // weighting with k_H is done in loop + } + else{ + qdots_out = particular_solution; + ROS_WARN("Null space projection matrix is null. The constraint may not be satisfied"); + } // //DEBUG: for verification of nullspace projection // std::stringstream ss_part; From dbe568e27f66b2599ff04083a07469352beba04e Mon Sep 17 00:00:00 2001 From: Bruno Brito Date: Fri, 13 Jan 2017 13:03:03 +0100 Subject: [PATCH 2/5] adding warining messages in projection matrix of the null space all null rank in the order solvers --- .../solvers/stack_of_tasks_solver.cpp | 17 ++++++++++++++--- .../solvers/task_priority_solver.cpp | 4 +++- 2 files changed, 17 insertions(+), 4 deletions(-) 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 index 69b5fce1..ca4b91c5 100644 --- 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 @@ -106,12 +106,23 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities, Eigen::MatrixXd J_temp = J_task * projector_i; Eigen::VectorXd v_task = it->task_; Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp); + Eigen::FullPivLU lu_decomp(J_temp_inv); + if(lu_decomp.rank()== 0){ + qdots_out.col(0) = q_i; + ROS_WARN("Null space projection matrix is null. It couldn't satisfy all constraints"); + return qdots_out; + } q_i = q_i + J_temp_inv * (v_task - J_task * q_i); projector_i = projector_i - J_temp_inv * J_temp; } - - qdots_out.col(0) = q_i + projector_i * sum_of_gradient; - return qdots_out; + Eigen::FullPivLU lu_decomp(projector_i); + if(lu_decomp.rank()== 0){ + qdots_out.col(0) = q_i; + ROS_WARN("Null space projection matrix is null. It couldn't satisfy the global weighting for all constraints."); + return qdots_out; + } + qdots_out.col(0) = q_i + projector_i * sum_of_gradient; //WHY??? + return qdots_out; } 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 index c0e24509..1c6515d4 100644 --- a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp +++ b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp @@ -64,8 +64,9 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities, predict_jnts_vel.q(i) = particular_solution(i, 0) * cycle + joint_states.current_q_(i); predict_jnts_vel.qdot(i) = particular_solution(i, 0); } + Eigen::FullPivLU lu_decomp(projector); - if (this->constraints_.size() > 0) + if ((this->constraints_.size() > 0) && (lu_decomp.rank() != 0)) { for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) { @@ -94,6 +95,7 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities, { qdots_out = particular_solution; ROS_ERROR_STREAM("Should not occur solution: " << std::endl << qdots_out); + ROS_WARN("Null space projection matrix is null. The constraint may not be satisfied"); } // Eigen::MatrixXd qdots_out = particular_solution + homogeneousSolution; // weighting with k_H is done in loop From 12c5bbac694096e7b10f1ee31dc1a1dde2c958ee Mon Sep 17 00:00:00 2001 From: Bruno Brito Date: Fri, 13 Jan 2017 17:54:49 +0100 Subject: [PATCH 3/5] if condition splited as in pull request comment --- .../solvers/task_priority_solver.cpp | 49 ++++++++++--------- 1 file changed, 26 insertions(+), 23 deletions(-) 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 index 1c6515d4..440154d3 100644 --- a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp +++ b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp @@ -66,36 +66,39 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities, } Eigen::FullPivLU lu_decomp(projector); - if ((this->constraints_.size() > 0) && (lu_decomp.rank() != 0)) + if (this->constraints_.size() > 0) { - for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) - { - (*it)->update(joint_states, predict_jnts_vel, this->jacobian_data_); - current_cost_func_value = (*it)->getValue(); - derivative_cost_func_value = (*it)->getDerivativeValue(); - partial_cost_func = (*it)->getPartialValues(); // Equal to (partial g) / (partial q) = J_g - activation_gain = (*it)->getActivationGain(); - magnitude = (*it)->getSelfMotionMagnitude(Eigen::MatrixXd::Zero(1, 1), Eigen::MatrixXd::Zero(1, 1)); // not necessary to pass valid values here. + if (lu_decomp.rank() != 0) + { + for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) + { + (*it)->update(joint_states, predict_jnts_vel, this->jacobian_data_); + current_cost_func_value = (*it)->getValue(); + derivative_cost_func_value = (*it)->getDerivativeValue(); + partial_cost_func = (*it)->getPartialValues(); // Equal to (partial g) / (partial q) = J_g + activation_gain = (*it)->getActivationGain(); + magnitude = (*it)->getSelfMotionMagnitude(Eigen::MatrixXd::Zero(1, 1), Eigen::MatrixXd::Zero(1, 1)); // not necessary to pass valid values here. - ROS_INFO_STREAM("activation_gain: " << activation_gain); - ROS_INFO_STREAM("smm: " << magnitude); - } + ROS_INFO_STREAM("activation_gain: " << activation_gain); + ROS_INFO_STREAM("smm: " << magnitude); + } - Eigen::MatrixXd jac_inv_2nd_term = Eigen::MatrixXd::Zero(projector.cols(), partial_cost_func.cols()); - if (activation_gain > 0.0) - { - Eigen::MatrixXd tmp_matrix = partial_cost_func.transpose() * projector; - jac_inv_2nd_term = pinv_calc_.calculate(tmp_matrix); - } + Eigen::MatrixXd jac_inv_2nd_term = Eigen::MatrixXd::Zero(projector.cols(), partial_cost_func.cols()); + if (activation_gain > 0.0) + { + Eigen::MatrixXd tmp_matrix = partial_cost_func.transpose() * projector; + 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); - qdots_out = particular_solution + this->params_.k_H * activation_gain * jac_inv_2nd_term * (magnitude * m_derivative_cost_func_value - partial_cost_func.transpose() * particular_solution); - } - else - { + Eigen::MatrixXd m_derivative_cost_func_value = derivative_cost_func_value * Eigen::MatrixXd::Identity(1, 1); + qdots_out = particular_solution + this->params_.k_H * activation_gain * jac_inv_2nd_term * (magnitude * m_derivative_cost_func_value - partial_cost_func.transpose() * particular_solution); + } + } + else + { qdots_out = particular_solution; ROS_ERROR_STREAM("Should not occur solution: " << std::endl << qdots_out); ROS_WARN("Null space projection matrix is null. The constraint may not be satisfied"); + } } // Eigen::MatrixXd qdots_out = particular_solution + homogeneousSolution; // weighting with k_H is done in loop From 95994a035b64cfcfe9a8a13373170e0d1b1c90cd Mon Sep 17 00:00:00 2001 From: Bruno Brito Date: Mon, 16 Jan 2017 11:00:18 +0100 Subject: [PATCH 4/5] code optimisation to compare the projector rank with zero matrix --- .../gradient_projection_method_solver.cpp | 6 +++--- .../solvers/stack_of_tasks_solver.cpp | 16 ++++++++-------- .../solvers/task_priority_solver.cpp | 3 +-- 3 files changed, 12 insertions(+), 13 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 index 7950555b..2e140e4a 100644 --- 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 @@ -52,10 +52,10 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_ Eigen::MatrixXd homogeneous_solution = Eigen::MatrixXd::Zero(particular_solution.rows(), particular_solution.cols()); KDL::JntArrayVel predict_jnts_vel(joint_states.current_q_.rows()); - Eigen::FullPivLU lu_decomp(projector); Eigen::MatrixXd qdots_out; - - if(lu_decomp.rank() != 0) + uint32_t rows = projector.rows(); + uint32_t cols = projector.cols(); + if(projector.isApprox(Eigen::Matrix::Zero)) { for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) 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 index ca4b91c5..3ba7323d 100644 --- 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 @@ -107,21 +107,21 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities, Eigen::VectorXd v_task = it->task_; Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp); Eigen::FullPivLU lu_decomp(J_temp_inv); - if(lu_decomp.rank()== 0){ - qdots_out.col(0) = q_i; + if(J_temp_inv.isApprox(Eigen::Matrix::Zeros)){ ROS_WARN("Null space projection matrix is null. It couldn't satisfy all constraints"); - return qdots_out; } - q_i = q_i + J_temp_inv * (v_task - J_task * q_i); - projector_i = projector_i - J_temp_inv * J_temp; + else{ + q_i = q_i + J_temp_inv * (v_task - J_task * q_i); + projector_i = projector_i - J_temp_inv * J_temp; + } } - Eigen::FullPivLU lu_decomp(projector_i); - if(lu_decomp.rank()== 0){ + + if(projector_i.isApprox(Eigen::Matrix::Zeros)){ qdots_out.col(0) = q_i; ROS_WARN("Null space projection matrix is null. It couldn't satisfy the global weighting for all constraints."); return qdots_out; } - qdots_out.col(0) = q_i + projector_i * sum_of_gradient; //WHY??? + qdots_out.col(0) = q_i + projector_i * sum_of_gradient; //Why??? return qdots_out; } 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 index 440154d3..a23e7974 100644 --- a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp +++ b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp @@ -64,11 +64,10 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities, predict_jnts_vel.q(i) = particular_solution(i, 0) * cycle + joint_states.current_q_(i); predict_jnts_vel.qdot(i) = particular_solution(i, 0); } - Eigen::FullPivLU lu_decomp(projector); if (this->constraints_.size() > 0) { - if (lu_decomp.rank() != 0) + if (projector.isApprox(Eigen::Matrix::Zeros)) { for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) { From 6bf88ebf84b0b4e8ae9b32d1ed89dafe06d51a4b Mon Sep 17 00:00:00 2001 From: Bruno Brito Date: Mon, 16 Jan 2017 11:06:46 +0100 Subject: [PATCH 5/5] bug fix --- .../solvers/gradient_projection_method_solver.cpp | 2 +- .../constraint_solvers/solvers/stack_of_tasks_solver.cpp | 9 ++++++--- .../constraint_solvers/solvers/task_priority_solver.cpp | 5 +++-- 3 files changed, 10 insertions(+), 6 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 index 2e140e4a..0a604733 100644 --- 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 @@ -55,7 +55,7 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_ Eigen::MatrixXd qdots_out; uint32_t rows = projector.rows(); uint32_t cols = projector.cols(); - if(projector.isApprox(Eigen::Matrix::Zero)) + if(projector.isApprox(Eigen::MatrixXd::Zero(rows,cols))) { for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) 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 index 3ba7323d..381f4eca 100644 --- 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 @@ -107,7 +107,9 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities, Eigen::VectorXd v_task = it->task_; Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp); Eigen::FullPivLU lu_decomp(J_temp_inv); - if(J_temp_inv.isApprox(Eigen::Matrix::Zeros)){ + uint32_t rows = J_temp_inv.rows(); + uint32_t cols = J_temp_inv.cols(); + if(J_temp_inv.isApprox(Eigen::MatrixXd::Zero(rows,cols))){ ROS_WARN("Null space projection matrix is null. It couldn't satisfy all constraints"); } else{ @@ -115,8 +117,9 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities, projector_i = projector_i - J_temp_inv * J_temp; } } - - if(projector_i.isApprox(Eigen::Matrix::Zeros)){ + uint32_t rows = projector_i.rows(); + uint32_t cols = projector_i.cols(); + if(projector_i.isApprox(Eigen::MatrixXd::Zero(rows,cols))){ qdots_out.col(0) = q_i; ROS_WARN("Null space projection matrix is null. It couldn't satisfy the global weighting for all constraints."); return qdots_out; 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 index a23e7974..991775a9 100644 --- a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp +++ b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp @@ -64,10 +64,11 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities, predict_jnts_vel.q(i) = particular_solution(i, 0) * cycle + joint_states.current_q_(i); predict_jnts_vel.qdot(i) = particular_solution(i, 0); } - + uint32_t rows = projector.rows(); + uint32_t cols = projector.cols(); if (this->constraints_.size() > 0) { - if (projector.isApprox(Eigen::Matrix::Zeros)) + if (projector.isApprox(Eigen::MatrixXd::Zero(rows,cols))) { for (std::set::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it) {