Gpm null space dimension print #126

bbrito · 2017-01-12T14:27:08Z

Solution for issue #125.

fmessmer

It would be nice if this would have been a new feature on a clean branch instead of being added to #124
Could you cherry-pick ipa320@4cccec9 on ìndigo_dev?
Also, we should have this output for:

…lculations

…ull rank in the order solvers

fmessmer

requesting changes

fmessmer · 2017-01-13T12:47:09Z

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<Eigen::MatrixXd> lu_decomp(projector);


Performance question:
I think it is quite expensive to perform a FullPivLU to determine rank
Could we not just check whether the determinant of a PartialPivLU is 0.0?

Or even easier:
compare projector with a Zero-Matrix using isApprox

fmessmer · 2017-01-13T12:51:24Z

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<Eigen::MatrixXd> lu_decomp(J_temp_inv);


Why do you evaluate J_temp_inv and not projector_i?

Because J_temp_inv is going to project the ith task velocities. If it has rank zero is not adding any velocity for that task.

fmessmer · 2017-01-13T12:53:09Z

cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp

+          qdots_out.col(0) = q_i;
+          ROS_WARN("Null space projection matrix is null. It couldn't satisfy all constraints");
+          return qdots_out;
+        }


I think we cannot quit here as projector_i = projector_i - J_temp_inv * J_temp; so even if some projector_i are Zero, the overall projector_i not necessarily is Zero as it is a sum not a multiplication.

Just a warning that the projector of the current task is Zero but continue computation

fmessmer · 2017-01-13T12:54:09Z

cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp

-
-    qdots_out.col(0) = q_i + projector_i * sum_of_gradient;
-    return qdots_out;
+    Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(projector_i);


same performance comment as above

fmessmer · 2017-01-13T12:55:11Z

cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp

+      return qdots_out;
+    }
+      qdots_out.col(0) = q_i + projector_i * sum_of_gradient;  //WHY???
+      return qdots_out;


Why //WHY???

identation

fmessmer · 2017-01-13T12:55:25Z

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<Eigen::MatrixXd> lu_decomp(projector);


same performance question

fmessmer · 2017-01-13T13:05:11Z

cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp


-    if (this->constraints_.size() > 0)
+    if ((this->constraints_.size() > 0) && (lu_decomp.rank() != 0))


Can you put the check into a new if?
Like this:

if (this->constraints_.size() > 0) { if(lu_decomp.rank() != 0) {

because than it's more clear was has happened, i.e. whether the first or the second condition is false

fmessmer · 2017-01-14T15:40:34Z

I played around with this a bit but couldn't enforce a situation where the projection matrix actually becomes Zero, i.e. rank 0 or determinant 0...maybe we have to discuss this a bit more...:wink:

fmessmer · 2017-01-19T18:23:00Z

See the last two commits on my branch where I tested various methods for testing empty Nullspace...
It seems that isApprox is not suited to determine empty Nullspace

For matrices, the comparison is done using the Hilbert-Schmidt norm (aka Frobenius norm L2 norm).
Because of the multiplicativeness of this comparison, one can't use this function to check whether *this is approximately equal to the zero matrix or vector. Indeed, isApprox(zero) returns false unless *this itself is exactly the zero matrix or vector. If you want to test whether *this is zero, use internal::isMuchSmallerThan(const RealScalar&, RealScalar) instead.

Also, I did quite some indentation fixing

fmessmer · 2017-04-07T13:12:39Z

@ipa-bfb
should we close this unmerged as it does not really help in debugging anyway...
I'll add a pointer to this PR to #152 in order to find it, in case we need it again

fmessmer · 2017-07-18T06:00:05Z

@ipa-bfb FYI

as discussed in https://github.com/ipa320/cob_control/pull/126#issuecomment-292532450 and because a pointer is added in https://github.com/ipa320/cob_control/issues/152#issuecomment-292532872, I'll close it unmerged...has a merge-conflict anyway

fmessmer requested changes Jan 12, 2017

View reviewed changes

bbrito force-pushed the gpm_null_space_dimension_print branch from 4cccec9 to 9f4c64e Compare January 12, 2017 16:31

Null space projection matrix rank is analised to avoid unnecessary ca…

58be1d3

…lculations

bbrito force-pushed the gpm_null_space_dimension_print branch from 9f4c64e to 58be1d3 Compare January 12, 2017 16:34

adding warining messages in projection matrix of the null space all n…

dbe568e

…ull rank in the order solvers

fmessmer requested changes Jan 13, 2017

View reviewed changes

if condition splited as in pull request comment

12c5bba

Bruno Brito added 2 commits January 16, 2017 11:00

code optimisation to compare the projector rank with zero matrix

95994a0

bug fix

6bf88eb

fmessmer mentioned this pull request Apr 7, 2017

[cob_twist_controller] refactor constraints/GPM code #152

Open

6 tasks

fmessmer self-assigned this Apr 7, 2017

fmessmer closed this Jul 18, 2017

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Gpm null space dimension print #126

Gpm null space dimension print #126

bbrito commented Jan 12, 2017

fmessmer left a comment •

edited

Loading

fmessmer left a comment

fmessmer Jan 13, 2017

fmessmer Jan 13, 2017

bbrito Jan 16, 2017

fmessmer Jan 13, 2017

fmessmer Jan 13, 2017

fmessmer Jan 13, 2017

fmessmer Jan 13, 2017

fmessmer Jan 13, 2017

fmessmer commented Jan 14, 2017 •

edited

Loading

fmessmer commented Jan 19, 2017 •

edited

Loading

fmessmer commented Apr 7, 2017

fmessmer commented Jul 18, 2017


		if (this->constraints_.size() > 0)
		if ((this->constraints_.size() > 0) && (lu_decomp.rank() != 0))

Gpm null space dimension print #126

Gpm null space dimension print #126

Conversation

bbrito commented Jan 12, 2017

fmessmer left a comment • edited Loading

Choose a reason for hiding this comment

fmessmer left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmessmer commented Jan 14, 2017 • edited Loading

fmessmer commented Jan 19, 2017 • edited Loading

fmessmer commented Apr 7, 2017

fmessmer commented Jul 18, 2017

fmessmer left a comment •

edited

Loading

fmessmer commented Jan 14, 2017 •

edited

Loading

fmessmer commented Jan 19, 2017 •

edited

Loading