diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h index 81ed368c..5748bcd3 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h +++ b/cob_twist_controller/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 index e7fc2f17..6f3e8f69 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 @@ -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; ilast_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::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 index e4bcf1f8..c0e24509 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 @@ -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 index d4be75c8..4c23edde 100644 --- 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 index 7e4ee33f..120b961d 100644 --- 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 index b8ca1fad..f997f063 100644 --- 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 limits_min = this->limiter_params_.limits_min; std::vector 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 index 5524806d..78de5894 100644 --- 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;