[fix] Generalize variable stiffness interface#51
[fix] Generalize variable stiffness interface#51domrachev03 wants to merge 4 commits intolearnsyslab:mainfrom
Conversation
| // Full 6x6 path: 36-element row-major matrix. | ||
| if (msg->data.size() == 36) { | ||
| Eigen::Matrix<double, 6, 6> K = | ||
| Eigen::Map<const Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>(msg->data.data()); | ||
|
|
||
| // Enforce symmetry: a physical stiffness matrix must satisfy K == K^T. | ||
| constexpr double kSymmetryTolerance = 1e-6; | ||
| const double max_asym = (K - K.transpose()).cwiseAbs().maxCoeff(); | ||
| if (max_asym > kSymmetryTolerance) { | ||
| RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100, | ||
| "Topic impedance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_rot); | ||
| vals[i] = std::clamp(vals[i], 0.0, max_k_rot); | ||
| "Topic impedance stiffness matrix not symmetric (max |K - K^T|=%.3g > %.1g), ignoring.", | ||
| max_asym, kSymmetryTolerance); | ||
| return; |
There was a problem hiding this comment.
instead of publishing 36 we could publish 21 elements and then we dont need an extra check. We are always sure that the matrix would be fine but this is more of an aesthetic thing
There was a problem hiding this comment.
We could start adding a few common functions to these calls (like previously discussed) e.g. under utils/stiffness_processing.hpp (even if having a utils module is an antipattern but this is a different topic...) and have a function that handles the symmetry check (if we stick to doing symmetry check) and one for clamping stiffness values. As a small step to get common utils between controllers
danielsanjosepro
left a comment
danielsanjosepro
left a comment
There was a problem hiding this comment.
Thank you a lot for this new contribution! I left a few comments fill free to contradict my request if you think differently :)
2 participants
Variable stiffness: support full 6×6 matrix (back-compatible)
Summary
During the use of the new feature, I quickly realised that exposing only the diagonal elements of the stiffness matrix is very limiting. This PR extends the variable stiffness topic interface in both
CartesianController(impedance) andCartesianAdmittanceControllerto accept a full 6×6 matrix in addition to the existing 6-element diagonal form. Adds symmetry validation and per-block magnitude bounds for the new path.Topic protocol
target_stiffnessandtarget_admittance_stiffness(std_msgs/Float64MultiArray) now accept either:[k_pos_x, k_pos_y, k_pos_z, k_rot_x, k_rot_y, k_rot_z](unchanged from main)6x6 checks & clamping
Symmetry check — reject if
max |K - K^T| > 1e-6(logs the deviation and tolerance). Downstream code can assumeKis symmetric.Per-element clamp — every entry is clamped according to its block:
j < 3(trans)j ≥ 3(rot)i < 3(trans)variable_max_*_stiffness.translationalvariable_max_*_stiffness.crossi ≥ 3(rot)variable_max_*_stiffness.crossvariable_max_*_stiffness.rotational[0, max](stiffness must be non-negative).[-max, max](coupling can be negative; magnitude bounded).(i,j)/(j,i), so clamping preserves the symmetry check.New YAML parameters
A single new
crossfield added to each existing stiffness-bound block. Defaults are conservative; existing diagonal-only configs are unaffected.cartesian_controllervariable_max_stiffness.cross500.0cartesian_admittance_controllervariable_max_impedance_stiffness.cross500.0cartesian_admittance_controllervariable_max_admittance_stiffness.cross100.0All three accept the bound
[0, 5000].