Implement streaming command in position-direct mode w/ fast solvers

[PeterBowman]

The last command scheduled at #105 and present in old asibot-main code (ref) was temporarily excluded due to current limitations. As pointed out in the title, it's meant to perform non-linear movements in position mode, that is, it's the streaming counterpart of movj.

The proposed signature is (no name yet):

void xxx(const std::vector<double> & x, double interval);

Due to the high-frequency nature of streaming commands, I'd propose switching to the yarp::dev::IPositionDirect to bypass trajectory generation at the iPOS level, hence the interval parameter intended for sampling point coordinates of the desired path (similarly to ICartesianControl::pose). However, this won't work on our simulator code (yet) unless we update the control boards to handle this interface.

[jgvictores]

For TEO we implement yarp::dev::IPositionDirect via the Technosoft iPos PT Mode, which turns out to work quite nicely.

For the simulator, we could map IPositionDirect to IPositionControl or IPositionControl2. Regarding simulators, we've gone through quite a bit of iterations. Which one are you using currently, teoSim or the yarp-openrave-plugins via python loader?

PS: Put an issue on the https://github.com/roboticslab-uc3m/openrave-yarp-plugins repo if you want this implementation!

[PeterBowman]

One drawback of the implementation I had in mind is that it would require high frequency computation of the inverse kinematics, which is most probably out of the question in KdlSolver (reminder: this solver currently works with numeric solutions). As a workaround, I'd consider exploring screw theory and available KDL solver interfaces (might end up creating our own implementation).

[jgvictores]

Obviously screw theory is beautiful. :-)
However, I'd first look into:

1. Velocity/torque control via Jacobian (think of how to connect this into PT Mode or PVT Mode).
2. Wrap OpenRAVE's IKFast in our solvers.
3. AFAIK, OpenRAVE's IKFast generates analytical solvers given the robot geometry. I've seen more software that does similar, oriented to small robots that use microcontrollers (Bioloid, etc). Found: https://github.com/AversivePlusPlus/ik and also have asked a contact I recall using a similar software.

[PeterBowman]

1. Perhaps through infinitesimal displacement twists (Bruyninckx, Robot Kinematics and Dynamics, 4.1.8), which somehow resemble virtual displacements, and then solve the usual delta_q = J^-1 * delta_x equation (note the deltas instead of time derivatives).

[jgvictores]

However, this won't work on our simulator code (yet) unless we update the control boards to handle this interface.

Implemented on YarpOpenraveControlboard via roboticslab-uc3m/openrave-yarp-plugins@14da8de (example launch: 1 and 2).

[jgvictores]

have asked a contact I recall using a similar software

https://vanadiumlabs.github.io/pypose/nuke-intro.html [thanks @JavierIH !!]

[jgvictores]

I'd consider exploring screw theory and available KDL solver interfaces (might end up creating our own implementation).

In https://github.com/robotology/codyco-superbuild they use https://github.com/ocra-recipes/eigen_lgsm

[PeterBowman]

@jgvictores proposes that we use movi (immediate move) as the name for the new command, following the convention of movj (joint move) and movl (linear move).

[PeterBowman]

See also #155 (comment) (ikfast).

[PeterBowman]

Thoughts on implementing a screw theory-based solver in KDL (mutually exclusive options):

• Parse geometric data from DH-information stored in current .ini files as is. Most elaborate way, has to search for valid Paden-Kahan solutions through an iterative process - just like manually solving a math equation through variable isolation and any necessary intermediate steps.

• Provide some hints along with the DH parameters, e.g. which PK problem is most suitable, how to find coplanar axes and so on. Somewhat less convolute than iterating through every single point in the problem space and checking for valid paths.

• Add a new parameter group in the .ini files so that a full direct PK algorithm is provided to the solver. No step generation is needed, just follow the ones specified by the user, carefully.

The last two options require that users know how to define a screw theory-based problem, leading to data duplication: in the same .ini (or even in separate files), both DH params and ST information would be hosted. Any change that affects the geometry should be reflected (and manually computed/recalculated) for two different problem definitions.

I'd choose the hard way, see first option.

[PeterBowman]

Iterative equation based on the POE:

With j = {1, 2, ..., N}, k = {1, 2, ..., N-1}, 1 <= j+k <= N.

[PeterBowman]

[PeterBowman]

I am happy to announce that the Screw Theory algorithm is close to being complete. First results have been obtained for the ABB IRB 120 robot (following @DrPardosGotor's lecture notes). Considering an initial home pose (same as the initial guess) and q_i = pi/2 as the desired pose, the ST inverse kinematics solver performs 5-6 times faster than a numeric Newton-Raphson algorithm (as implemented in KDL) and produces all 8 available solutions. Execution times vary between 0.04-0.12 milliseconds.

[DrPardosGotor]

Good for you Bartek! Dr.Eng. Jose M. PARDOS-GOTOR, PhD. Associate Professor Systems Engineering & Automation UC3M M. +34 625 606 337

…

On 4 Dec 2018, at 23:05, Bartek Łukawski ***@***.***> wrote: I am happy to announce that the Screw Theory algorithm is close to being complete. First results have been obtained for the ABB IRB 120 robot (following @DrPardosGotor's lecture notes). Considering an initial home pose (same as the initial guess) and q_i = pi/2 as the desired pose, the ST inverse kinematics solver performs 5-6 times faster than a numeric Newton-Raphson algorithm (as implemented in KDL) and produces all 8 available solutions. Execution times vary between 0.04-0.12 milliseconds. — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub, or mute the thread.

[DrPardosGotor]

Many Thanks Bartek, I would propose to make a presentation to understand in detail wha you have done. I suppose this mount is going to be complicated, but as soon as you can I believe It will be very interesting for all involved. I wait for an appointment to the group (perhaps some Thursday at 19:00h). Dr.Eng. Jose Manuel PARDOS-GOTOR, PhD. Associate Professor SYSTEMS ENGINEERING AND AUTOMATION

…

On 2018Dec4, at 23:05, Bartek Łukawski ***@***.***> wrote: I am happy to announce that the Screw Theory algorithm is close to being complete. First results have been obtained for the ABB IRB 120 robot (following @DrPardosGotor's lecture notes). Considering an initial home pose (same as the initial guess) and q_i = pi/2 as the desired pose, the ST inverse kinematics solver performs 5-6 times faster than a numeric Newton-Raphson algorithm (as implemented in KDL) and produces all 8 available solutions. Execution times vary between 0.04-0.12 milliseconds. — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub, or mute the thread.

[jgvictores]

I guess we'll be closing this issue soon thanks to #169, so whatever commented here will probably be buried in time... however, may be worthy to mention.

Regarding #169 (comment): 5-10 times faster than a numeric Newton-Raphson solver provided by KDL. Kind of leaves me asking if this is the order of magnitude of acceleration expected, and if we would do any better using some kind of GPU-accelerated solver such as cuSOLVER.

[PeterBowman]

Regarding #169 (comment): 5-10 times faster than a numeric Newton-Raphson solver provided by KDL. Kind of leaves me asking if this is the order of magnitude of acceleration expected

Regarding MATLAB code and as stated in the reference I cited therein, a numeric solver (Robotics Toolbox) is about 1000x slower than the proposed Screw Theory implementation (0.1~1ms). Looks like KDL/C++ performs far better, although Screw Theory/C++ is still much faster.

[jgvictores]

Done:

• Info documented at 35b1499
• Proposal followup at IK through infinitesimal displacement twists #171

To do:

• Make sure the doxygen of the newly developed classes have all the info here: diagrams, 5-10 times faster than a numeric Newton-Raphson solver provided by KDL, etc.
• Add example app that demonstrates the use of this new library.
• Allow ScrewTheoryLib to select the optimal solution.
• Add the movi streaming command.

[PeterBowman]

Added example app at e777bc8.

[PeterBowman]

* Make sure the doxygen of the newly developed classes have all the info here: diagrams (...)

@jgvictores I might have some useful diagrams generated in a couple of weeks. For now, though, I don't think #119 (comment) could be of any use. I did some work on describing the class internals either with Doxygen (preferable) or through inline comments only seen in source code, so that said diagram (severely WIP back then) has been already expressed in text/formula form. It would be much easier if you could state what exactly is missing as of the current state of the documentation in ScrewTheoryLib. That performance notice could fit inside KdlSolver (ST-KDL wrapper).

[jgvictores]

1. Really cool: http://robots.uc3m.es/dox-kinematics-dynamics/classroboticslab_1_1ScrewTheoryIkSubproblem.html#ad2d0dbe398d37d2e2582e8730299dfa6
2. I understand you say Implement streaming command in position-direct mode w/ fast solvers #119 (comment) is already there in some updated but different format, could you please indicate (here on this same issue) a URL or how to reach it?
3. http://robots.uc3m.es/dox-kinematics-dynamics/group__ScrewTheoryLib.html or click on More... to read http://robots.uc3m.es/dox-kinematics-dynamics/group__ScrewTheoryLib.html#details which is empty. I'm missing a small comment saying "this is faster and may provide better solutions than numerical methods because XXXX [ref]". I know there is a reference already, but I believe this is kind of the "first stop" before someone will decide to actually give a shot at the library (at least my own personal case).

[PeterBowman]

I understand you say #119 (comment) is already there in some updated but different format, could you please indicate (here on this same issue) a URL or how to reach it?

Mostly inline comments scattered all over here plus that iterative formula you linked to in 1. Please mind that I never considered that implementation to be stable yet, several details would benefit from a 👽2👽 discussion and, in that vein, most of those comments could/should be rewritten.

I'm missing a small comment saying "this is faster and may provide better solutions than numerical methods because XXXX [ref]".

👍

[PeterBowman]

Docs improved at 9c580a5.

[PeterBowman]

Allow ScrewTheoryLib to select the optimal solution.

Done at 19b37b8. This is heavily based on the AsibotConfiguration class. Optimal means that the closest pose to an initial qGuess one (usually, the current configuration of the robot prior to executing any movement) will be chosen out of all available solutions, provided joint limits are not exceeded. Usage:

1. We don't want to keep an internal state, i.e. let's pick the robot configuration that fits best at every scenario (while keeping joint limits in mind). Suitable for point-to-point non-linear trajectories such as movj. Instantiate this class via ConfigurationSelectorFactory::create before feeding it with IK-computed solutions.

2. Remember the first valid configuration (within joint limits) and use that for the entire lifetime of an ConfigurationSelector instance, i.e. discard all solutions but one. Suitable for trajectories based on streams of tiny increments (such as the would-be movi command) since we don't want the manipulator to suddenly change its configuration when limits are hit; implemented in KdlSolver and exampleScrewTheoryTrajectory.

[jgvictores]

Thanks a lot, I think it's great right now. Ok to close issue!

[PeterBowman]

* Add the `movi` streaming command.

Done at b9ca4d6, closing.