Manipulabilty barrier for singularity avoidance - #239
Conversation
sjahr
changed the title
sea-bass
left a comment
sea-bass
left a comment
There was a problem hiding this comment.
I tested this and the plotly stuff is really cool!
- The wrist and shoulder cases made sense/barrier worked as intended, though tracking was of course quite bad when the barrier was on.
- Elbow had some issues. Without the barrier, I never saw the print for singularities even though sigma did go below the minimum. With the barrier, it seemed like the trajectory wasn't being tracked properly at all even though sigma was way above the safe distance. Is something pushing it back prematurely?
Generally, it might be good to plot where the reference position of the robot is. Whether you plot a full trajectory line, or a "carrot on a stick" showing the instantaneous reference of the end effector, either would be helpful.
I also suspect that picking a manipulator with redundancy, like the Kinova/Franka, will yield better results because you could possibly use some of the redundancy to keep manipulability above the minimum value, while still tracking? But not sure...
| VelocityLimit, | ||
| ) | ||
|
|
||
| # Joint-space waypoints for each singularity type (UR5, 6 arm joints). |
There was a problem hiding this comment.
it might be more legible to couple the waypoints and labels into a data structure that captures them both together
| target_lock = threading.Lock() | ||
| running = True | ||
|
|
||
| # Build the C++ barrier list once — passed into every solveIk call. |
| current_q_arm_target = [ | ||
| waypoints_q[0].copy() | ||
| ] # joint-space reference for error plot |
There was a problem hiding this comment.
nit: put the comment above so the linter doesn't do this
| nodeenv = ">=1.10.0, <2" | ||
| tyro = ">=0.9.26,<0.10" | ||
| viser = ">=1.0.29,<2.0" # See https://github.com/viser-project/viser/issues/719 | ||
| plotly = ">=5.0,<6" |
There was a problem hiding this comment.
this is cool, but make sure you also add it to the ubuntu / ros dockers and check that it works there
| for (int i = 0; i < static_cast<int>(v_indices.size()); ++i) { | ||
| j_arm_.col(i) = full_jacobian_.col(v_indices[i]); | ||
| } |
There was a problem hiding this comment.
i feel like there must be a single Eigen operation that can replace doing it in a loop...
| Eigen::MatrixXd J_arm(6, v_indices.size()); | ||
| for (int i = 0; i < static_cast<int>(v_indices.size()); ++i) { | ||
| J_arm.col(i) = J.col(v_indices[i]); | ||
| } | ||
| const double sigma_min = Eigen::BDCSVD<Eigen::MatrixXd>(J_arm).singularValues().minCoeff(); |
There was a problem hiding this comment.
should we be calling computeSigmaMin() here too?
2 participants
Add manipulability barrier to prevent the robot from getting to close to a singularity. Part of this PR is a new example to test oink in singularities:
Manipulability and lm-damping have different impact on the singularity behavior and you can play around with both of them. Not sure if we should keep the example or remove.