Skip to content

Commit 66061bc

Browse files
author
Thibault Poignonec
committed
check for singularities in impedance control
1 parent 1091a34 commit 66061bc

File tree

1 file changed

+10
-0
lines changed

1 file changed

+10
-0
lines changed

cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,16 @@ bool VanillaCartesianImpedanceRule::compute_controls(
166166
J_dot_
167167
);
168168
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+
}
169179
J_pinv_ = (J_.transpose() * J_ + alpha_pinv_ * I_joint_space_).inverse() * J_.transpose();
170180

171181
if (!model_is_ok) {

0 commit comments

Comments
 (0)