From 4cccec9aab258e09cd136d81224603963221e011 Mon Sep 17 00:00:00 2001 From: Bruno Brito Date: Thu, 12 Jan 2017 15:25:49 +0100 Subject: [PATCH] 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;