descartes
descartes copied to clipboard
ros-industrial-consortium
Extra IK-Fast Solutions
Descartes more or less requires IK-Fast to work. My understanding of IK-Fast is that it gives you the "unique" solutions that correspond to different robot configurations reaching for the same point.
For many robots, there are 8 solutions returned for a given pose. For the IK-Fast models I've been working with, these 8 solutions have joint values between -PI and PI. If a robot joint can rotate further than that, say +/- 2 PI, then there are extra solutions which are not currently captured.
Some of you may have already figured this out, but our existing robot models DO NOT take this into account. Effectively all Descartes robot models have artificial joint limits at +/- PI.
The brute force way to incorporate these is to try adding/subtracting 2 PI to every possible joint that can move beyond PI for every solution that we get from IKFast and keeping any that are valid. This applies to every possible permutation of those joints. So we could end up with a lot more solutions for some robots (e.g., UR5).
There may be a more intelligent way to search and store these "alternate" solutions, but I can't think of it right now.
:+1: to this issue overall (regardless of where it belongs). I feel like this needs to be addressed at a deeper level considering many available industrial robots have wrists that rotate past +/-pi (and some like the UR have extended rotation on two or more joints...).
@Jmeyer1292 where have you found the best place to add +/-2pi? I would like to hack a solution together in the interim for testing. I am currently running into issues of severely limited Descartes performance with the Agilus series of robots that have the following limits: A1 +/-170° A2 +45° to -190° A3 +156° to -120° A4 +/-185° A5 +/-120° A6 +/-350° The artificial limitation on the wrist (A6) is a killer....
It seems at first glance that I could modify the generated IKFast plugin code to add and test the extra desired solutions within IKFastKinematicPlugin::getPositionIK... then nothing in Descartes changes? (it might need to go in a few other places but I haven't gotten that far...)
This approach would follow with @jrgnicho's comment that this belongs in the solver.
@Jmeyer1292, thoughts?
@BrettHemes I think that's the simplest solution. In some ways, the lower it goes the better. In playing around I've put extra searches on 6 and sometimes 4 with the robots I have. In the kinematics plugin, as you mentioned, is probably best.
I have hacked it before in the state adapter, but at least it shows you what I'm doing here.
My thought with putting it higher was that I might eventually do something more clever with collision checking equivalent joint solutions once. I think you go with your initial thought though.
FYI, implemented the IKFast plugin modification and it works beautifully. Thanks for the help @Jmeyer1292
Hrm. I'm not sure how I feel about this change. I assume that Descartes is the primary user of that overload and currently I much prefer to do my own resolution of those joint values so I can pick the ones I care about (4 & 6 usually). We don't have a "seed state" so for me, it's just more work that MoveIt does that I don't want.
I feel like I may eventually want or need to get rid of MoveIt's IKFast interface and just use my own.