We read every piece of feedback, and take your input very seriously.
To see all available qualifiers, see our documentation.
There was an error while loading. Please reload this page.
1 parent 1091a34 commit 66061bcCopy full SHA for 66061bc
cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp
@@ -166,6 +166,16 @@ bool VanillaCartesianImpedanceRule::compute_controls(
166
J_dot_
167
);
168
RCLCPP_DEBUG(logger_, "Computing J_pinv...");
169
+ const Eigen::JacobiSVD<Eigen::MatrixXd> J_svd =
170
+ Eigen::JacobiSVD<Eigen::MatrixXd>(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
171
+ const Eigen::MatrixXd matrix_s = J_svd.singularValues().asDiagonal();
172
+ if (J_svd.singularValues()(0) / J_svd.singularValues()(dims - 1) > 30) {
173
+ success = false;
174
+ RCLCPP_ERROR(
175
+ logger_,
176
+ "Jacobian singularity detected!"
177
+ );
178
+ }
179
J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose();
180
181
if (!model_is_ok) {
0 commit comments