-
Notifications
You must be signed in to change notification settings - Fork 64
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Gpm null space dimension print #126
Changes from all commits
58be1d3
dbe568e
12c5bba
95994a0
6bf88eb
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -106,12 +106,26 @@ 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); | ||
q_i = q_i + J_temp_inv * (v_task - J_task * q_i); | ||
projector_i = projector_i - J_temp_inv * J_temp; | ||
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(J_temp_inv); | ||
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"); | ||
} | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I think we cannot quit here as Just a warning that the projector of the current task is Zero but continue computation |
||
else{ | ||
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; | ||
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; | ||
} | ||
qdots_out.col(0) = q_i + projector_i * sum_of_gradient; //Why??? | ||
return qdots_out; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Why identation |
||
} | ||
|
||
|
||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why do you evaluate
J_temp_inv
and notprojector_i
?There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.