KDL's Jacobian and cartesian pseduo-inverse controllers

I have noticed that the Jacobian class in KDL doesn't have any transpose or inverse operations, which are necessary for, say, cartesian rate controllers on redundant (or not) manipulators doing pseudo-inverse style operations. Has this simply not been a necessity to date, is there some other way for this sequence of operations to be done with the existing Jacobian class that I haven't seen, or .... am I missing something?

Just curious ... before someone here points the finger at me and says "make it happen" .... :-(

Cheers
S

KDL's Jacobian and cartesian pseduo-inverse lers

On Tue, 30 Sep 2008, kiwi [dot] net [..] ... wrote:

> I have noticed that the Jacobian class in KDL doesn't have any transpose or
> inverse operations, which are necessary for, say, cartesian rate controllers
> on redundant (or not) manipulators doing pseudo-inverse style operations. Has
> this simply not been a necessity to date, is there some other way for this
> sequence of operations to be done with the existing Jacobian class that I
> haven't seen, or .... am I missing something?
>
> Just curious ... before someone here points the finger at me and says "make
> it happen" .... :-(
>
My stance on this is the following: the approach of proving explicit matrix
manipulation functionalities is contrary to the realtime, efficiency
motivation behind KDL. I know of no case where you really want to do matrix
manipulations, especially not online. If for example, you want to calculate
joint torques \tau as a function of the end-effector force F, via \tau =
J^T F, then KDL's approach is to make a _solver_ for that, i.e. an
algorithm that makes the calculations efficiently, not by explicit matrix
manipulation. In the example case, this would happen via an inward
recursive sweep over all links, for example.

If KDL would introduce the "Jacobian transpose" in its API, then I am quite
sure someone will stand up and ask for a "pseudo-inverse" addition
( (J^T J)^{-1} J^T ) to the API, or (shrudder!) a "manipulability index"
addition (\sqrt(\det(J^T J))). While all of these, and many more, can be
computed via efficient recursive algorithms, with much better numerical
conditioning. Ruben has a couple of damped-least squares implementations
ready, and more are in the pipeline. In addition, there is already an
efficient (and possible iteratively (i.e., in various sample steps of a
controller) applied) SVD decomposition available, which can be used for all
pseudo-inverse, singularity and manipulability purposes.

In summary, I would like to keep KDL lean, and add these "functionalities"
elsewhere, but not in the realtime-ready part of Orocos.

However, I am not the maintainer, and I am open to be proven wrong! :-)

Herman

Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm

KDL's Jacobian and cartesian pseduo-inverse lers

On Oct 1, 2008, at 02:30 , Herman Bruyninckx wrote:

> On Tue, 30 Sep 2008, kiwi [dot] net [..] ... wrote:
>
>> I have noticed that the Jacobian class in KDL doesn't have any
>> transpose or inverse operations, which are necessary for, say,
>> cartesian rate controllers on redundant (or not) manipulators doing
>> pseudo-inverse style operations. Has this simply not been a
>> necessity to date, is there some other way for this sequence of
>> operations to be done with the existing Jacobian class that I
>> haven't seen, or .... am I missing something?
>>
>> Just curious ... before someone here points the finger at me and
>> says "make it happen" .... :-(
>>
> My stance on this is the following: the approach of proving explicit
> matrix
> manipulation functionalities is contrary to the realtime, efficiency
> motivation behind KDL. I know of no case where you really want to do
> matrix
> manipulations, especially not online. If for example, you want to
> calculate
> joint torques \tau as a function of the end-effector force F, via
> \tau =
> J^T F, then KDL's approach is to make a _solver_ for that, i.e. an
> algorithm that makes the calculations efficiently, not by explicit
> matrix
> manipulation. In the example case, this would happen via an inward
> recursive sweep over all links, for example.

I agree completely with the specialized solver case. Might cost you a
little more in up front development, but provides better code down the
line. The problem simply was that the solver I was looking for isn't
yet in the online documentation, so I didn't know that it existed.

> In summary, I would like to keep KDL lean, and add these
> "functionalities"
> elsewhere, but not in the realtime-ready part of Orocos.

Yes yes yes!
S

KDL's Jacobian and cartesian pseduo-inverse controllers

On Oct 1, 2008, at 03:03 , Ruben Smits wrote:

> On Tuesday 30 September 2008 23:05:04 kiwi [dot] net [..] ... wrote:
>> I have noticed that the Jacobian class in KDL doesn't have any
>> transpose or
>> inverse operations, which are necessary for, say, cartesian rate
>> controllers on redundant (or not) manipulators doing pseudo-inverse
>> style
>> operations. Has this simply not been a necessity to date, is there
>> some
>> other way for this sequence of operations to be done with the
>> existing
>> Jacobian class that I haven't seen, or .... am I missing something?
>>
>> Just curious ... before someone here points the finger at me and
>> says "make
>> it happen" .... :-(

> how you calculate the inverse kinematics from this is up to you, we
> have
> different solvers that use this in a different way:
>
> * chainiksolvervel_pinv uses the svd to calculate the moore-penrose
> generalised inverse of the jacobian
> * chainiksolvervel_wdls uses the svd to calculate the weighted
> damped least
> squares solution.

The _wdls version is exactly what I was looking for. Because it isn't
in the online API doc's, I didn't even know it was there. I also
eventually found the undocumented differentiation/integration
functions that I needed, which saves more work.

> I am thinking to use boost::ublas for the internal representation of
> our
> primitives, so we could use their operations in the background. As
> you can see
> I already used it behind the scenes in the solvers.

We've been doing the exact same thing with our internal code. Better
to reuse than to rewrite (and probably get it wrong!).
S

KDL's Jacobian and cartesian pseduo-inverse controllers

On Oct 2, 2008, at 07:33 , Stephen Roderick wrote:

> On Oct 1, 2008, at 03:03 , Ruben Smits wrote:
>
>> On Tuesday 30 September 2008 23:05:04 kiwi [dot] net [..] ... wrote:

>>
>>> how you calculate the inverse kinematics from this is up to you,
>>> we have
>> different solvers that use this in a different way:
>>
>> * chainiksolvervel_pinv uses the svd to calculate the moore-penrose
>> generalised inverse of the jacobian
>> * chainiksolvervel_wdls uses the svd to calculate the weighted
>> damped least
>> squares solution.
>
> The _wdls version is exactly what I was looking for. Because it
> isn't in the online API doc's, I didn't even know it was there. I
> also eventually found the undocumented differentiation/integration
> functions that I needed, which saves more work.

So a success story to relate ... we replaced our RobOOP-based
compliance controller with one based on the vel_wdls IK solver in KDL.
Bar some remaining minor oscillations in certain scenarios (which were
present in the RobOOP version too, so we don't believe they're related
to KDL), things are very good with the KDL version. The KDL version is
ten (10x) times faster. Yes, we knocked off 90% of the runtime! It's
also about 1/2 or less the code size, and probably took me 1/4 the
time to develop than the RobOOP one would have been from scratch.

So basically ... you KDL guys are awesome!!

Oh ... well ... Peter and that RTT/OCL crowd ... they aren't too bad
either ... ;-)

Seriously, thanks once again.

Wanted to add that to the archive, so future perusers would have some
idea of how things can improve for the better ...
An Average User :-)

KDL's Jacobian and cartesian pseduo-inverse controllers

On Thu, 2 Oct 2008, S Roderick wrote:

> On Oct 2, 2008, at 07:33 , Stephen Roderick wrote:
>> On Oct 1, 2008, at 03:03 , Ruben Smits wrote:
>>> On Tuesday 30 September 2008 23:05:04 kiwi [dot] net [..] ... wrote:
>>>> how you calculate the inverse kinematics from this is up to you, we have
>>> different solvers that use this in a different way:
>>>
>>> * chainiksolvervel_pinv uses the svd to calculate the moore-penrose
>>> generalised inverse of the jacobian
>>> * chainiksolvervel_wdls uses the svd to calculate the weighted damped
>>> least
>>> squares solution.
>>
>> The _wdls version is exactly what I was looking for. Because it isn't in
>> the online API doc's, I didn't even know it was there. I also eventually
>> found the undocumented differentiation/integration functions that I needed,
>> which saves more work.
>
> So a success story to relate ... we replaced our RobOOP-based compliance
> controller with one based on the vel_wdls IK solver in KDL. Bar some
> remaining minor oscillations in certain scenarios (which were present in the
> RobOOP version too, so we don't believe they're related to KDL),

I am interested in finding out with you what the real cause of thise
oscillations is! Because the kinematics solver is only half of the story,
or rather, one third: also the task specification and the control laws
influence the overall behaviour of your robot...

> things are
> very good with the KDL version. The KDL version is ten (10x) times faster.
> Yes, we knocked off 90% of the runtime! It's also about 1/2 or less the code
> size, and probably took me 1/4 the time to develop than the RobOOP one would
> have been from scratch.
>
> So basically ... you KDL guys are awesome!!
>
> Oh ... well ... Peter and that RTT/OCL crowd ... they aren't too bad either
> ... ;-)
>
> Seriously, thanks once again.
Much appreciated!

> Wanted to add that to the archive, so future perusers would have some idea of
> how things can improve for the better ...
> An Average User :-)

Herman

KDL's Jacobian and cartesian pseduo-inverse controllers

On Oct 3, 2008, at 04:29 , Herman Bruyninckx wrote:

> On Thu, 2 Oct 2008, S Roderick wrote:
>
>> On Oct 2, 2008, at 07:33 , Stephen Roderick wrote:
>>> On Oct 1, 2008, at 03:03 , Ruben Smits wrote:
>>>> On Tuesday 30 September 2008 23:05:04 kiwi [dot] net [..] ... wrote:
>>>>> how you calculate the inverse kinematics from this is up to you,
>>>>> we have
>>>> different solvers that use this in a different way:
>>>> * chainiksolvervel_pinv uses the svd to calculate the moore-penrose
>>>> generalised inverse of the jacobian
>>>> * chainiksolvervel_wdls uses the svd to calculate the weighted
>>>> damped least
>>>> squares solution.
>>> The _wdls version is exactly what I was looking for. Because it
>>> isn't in the online API doc's, I didn't even know it was there. I
>>> also eventually found the undocumented differentiation/integration
>>> functions that I needed, which saves more work.
>>
>> So a success story to relate ... we replaced our RobOOP-based
>> compliance controller with one based on the vel_wdls IK solver in
>> KDL. Bar some remaining minor oscillations in certain scenarios
>> (which were present in the RobOOP version too, so we don't believe
>> they're related to KDL),
>
> I am interested in finding out with you what the real cause of thise
> oscillations is! Because the kinematics solver is only half of the
> story,
> or rather, one third: also the task specification and the control laws
> influence the overall behaviour of your robot...

Unfortunately, it was primarily because my controls guy and I
originally convinced ourselves that the undocumented CartToJnt() in
the ChainIkSolverVel_wdls class was working in tool frame. Wrong!
Works in base frame ... we finally dug that out of the documented
JntToJac solver ... so I'll be sending in a documentation patch for
this one! It _almost_ worked in our case, given the joint pose and
velocities, which made it much harder to track down ...

Also, our lambda wasn't quite right. We think this was exacerbated by,
or hidden by, the problem's in the previous RobOOP implementation
which we had replaced.

Much better now ... much much better ...
S

KDL's Jacobian and cartesian pseduo-inverse controllers

On Thu, 2 Oct 2008, Stephen Roderick wrote:

> On Oct 1, 2008, at 03:03 , Ruben Smits wrote:
>
>> On Tuesday 30 September 2008 23:05:04 kiwi [dot] net [..] ... wrote:
>>> I have noticed that the Jacobian class in KDL doesn't have any transpose
>>> or
>>> inverse operations, which are necessary for, say, cartesian rate
>>> controllers on redundant (or not) manipulators doing pseudo-inverse style
>>> operations. Has this simply not been a necessity to date, is there some
>>> other way for this sequence of operations to be done with the existing
>>> Jacobian class that I haven't seen, or .... am I missing something?
>>>
>>> Just curious ... before someone here points the finger at me and says
>>> "make
>>> it happen" .... :-(
>
>
>
>> how you calculate the inverse kinematics from this is up to you, we have
>> different solvers that use this in a different way:
>>
>> * chainiksolvervel_pinv uses the svd to calculate the moore-penrose
>> generalised inverse of the jacobian
>> * chainiksolvervel_wdls uses the svd to calculate the weighted damped least
>> squares solution.
>
> The _wdls version is exactly what I was looking for. Because it isn't in the
> online API doc's, I didn't even know it was there. I also eventually found
> the undocumented differentiation/integration functions that I needed, which
> saves more work.

So, with respect to the current lag in documentation with respect to code,
it might now be time for me to say: "make it happen" .... :-)

Anyway, I agree with you that that solver should be one of the most popular
ones in KDL...

Herman