model a robot with parallelogram / virtual joints / KDL can't solve graphs

Hi,

I'm working with a ABB 6400 robot that has a parallelogram ( -> a cycle in graph terminology ), hence, my requirement is being able to solve virtual joints. In order to model that robot properly, it can't be modelled as a chain or tree AFAIK, though only as a graph.

Unfortunately, solving a kinematic graph is not something that KDL can deal with. A bug report for this missing feature has been created [1] and vetted for in the ensuing discussion [2]. That discussion sparked a "one-line" design document as described by Herman:

<<<

    My one-line "design document" in this context: "solving" a hybrid chain is
    done in different phases:
    1. (instantaneously) finding a "spanning tree" over the topological graph;
    2. solving the tree, as a first approximation of the hybrid case;
    3. iterate until this approximation has reached the configured tolerance.
>>>

      j4-------------------------------------j5-------j6
      |
      |                                   
p2----j3 | | | | | | | | | | | | p1----j2
      |                                   
      |                                   
      |                                   
      |                                   
      j1                                  
I wonder how this applies to modeling the ABB 6400 robot. Does the spanning tree look like so?

          / j1 -> j2 -> p1 -> p2 -> j3 -> j4 -> j5 -> j6
root /
                \ j1 -> j2 -> j3 -> j4 -> j5 -> j6
Finally, is it true that in such a tree that joints would end up being defined twice? Thing is that when adding a segment to a Tree, you cannot refer to joints already present in the tree ( eg a method Tree::addSegment( "jnt2", "jnt3" ), Tree::addSegment( "p2", "jnt3" ) does not exist )

All in all I'm curious whether some of you have got some ideas for me to advance on this subject.

Best,

-jelle

[1] http://bugs.orocos.org/show_bug.cgi?id=967 [2] http://www.orocos.org/forum/rtt/rtt-dev/bug-967-new-using-path-specification-create-joints

model a robot with parallelogram / virtual joints / KDL can't so

On Mon, 19 Jan 2015, jelleferinga [..] ... wrote:

> Hi,
>
> I'm working with a ABB 6400 robot that has a parallelogram ( -> a cycle in
> graph terminology ), hence, my requirement is being able to solve virtual
> joints.
> In order to model that robot properly, it can't be modelled as a chain or
> tree AFAIK, though only as a graph.
>
> Unfortunately, solving a kinematic graph is not something that KDL can deal
> with.

Indeed! But if I remember correctly, the kinematics of the ABB 6400 are
100% equivalent to a loop-free equivalent structure... Namely one in
which the virtual q3 is a simple function of the measured q3 joint angle
and the value of q2. I even think that the function is trivial, and
q3 is just q3 :-)

Here is a document with more maths:
<www.pwilloughby.com/docs/2_05_ABB_Robot_project.pdf>

Taking the loop into account explicitly will only have an influence when
considering the dynamics of the whole system.

> A bug report for this missing feature has been created [1] and vetted for in
> the ensuing discussion [2].
> That discussion sparked a "one-line" design document as described by Herman:
>
> >
>
> j4-------------------------------------j5-------j6
> |
> |
> p2----j3
> | |
> | |
> | |
> | |
> | |
> | |
> p1----j2
> |
> |
> |
> |
> j1
>
> I wonder how this applies to modeling the ABB 6400 robot.
> Does the spanning tree look like so?
>
> / j1 -> j2 -> p1 -> p2 -> j3 -> j4 -> j5 -> j6
> root /
> \
> \ j1 -> j2 -> j3 -> j4 -> j5 -> j6

I would stop the first "branch" at j3; because it is there that the
"zero relative motion" constraint applies. As mentioned above, this
constraint can be solved simply(?) at the kinematic level; for dynamics,
the constraint is a zero relative acceleration, for which solvers like
Vereshchagin's provide a solution. There is a (somewhat outdated)
implementation of that one in the KDL library.

> Finally, is it true that in such a tree that joints would end up being
> defined twice?

In my suggestion, only j3 needs to be counted twice. Or rather (as you
found out yourself, see below), it is not the joints that are to be
taken into account twice, but the "end effector" after p2 is rigidly
connected to the link after(?) j3.

> Thing is that when adding a segment to a Tree, you cannot refer to joints
> already present in the tree ( eg a method Tree::addSegment( "jnt2", "jnt3" ),
> Tree::addSegment( "p2", "jnt3" ) does not exist )
>
> All in all I'm curious whether some of you have got some ideas for me to
> advance on this subject.

> Best,
>
> -jelle

Herman

>
> [1] http://bugs.orocos.org/show_bug.cgi?id=967
> [2]
> http://www.orocos.org/forum/rtt/rtt-dev/bug-967-new-using-path-specifica...

model a robot with parallelogram / virtual joints / KDL can't so

Dear Herman, Gianni,

>> Unfortunately, solving a kinematic graph is not something that KDL can deal
>> with.
>
> Indeed! But if I remember correctly, the kinematics of the ABB 6400 are
> 100% equivalent to a loop-free equivalent structure... Namely one in
> which the virtual q3 is a simple function of the measured q3 joint angle
> and the value of q2. I even think that the function is trivial, and
> q3 is just q3 :-)

Thing is that by convention the joint value reported on the teach pendent is not the angular value that your (correct) FK solution reports.
ABB’s convention is that the angular value for q3 on the teach pendent really is -q2 + q3
As such, when the arm is horizontal jogging q2 on the teach pendent will leave the arm horizontal

So, that’s where the confusion stems from: the assumption that the FK angles should match those reported on the teach pendent, and having unnecessarily little faith the DH parameters set.
Add some senseless eagerness to model the parallelogram kinematically stopped me from seeing the obvious…

> Here is a document with more maths:
> <www.pwilloughby.com/docs/2_05_ABB_Robot_project.pdf>

Thanks, so this hint ( the ABB convention ) follows on page #4

> Taking the loop into account explicitly will only have an influence when
> considering the dynamics of the whole system.
>
>> A bug report for this missing feature has been created [1] and vetted for in
>> the ensuing discussion [2].
>> That discussion sparked a "one-line" design document as described by Herman:
>>
>> >
>>
>> j4-------------------------------------j5-------j6
>> |
>> |
>> p2----j3
>> | |
>> | |
>> | |
>> | |
>> | |
>> | |
>> p1----j2
>> |
>> |
>> |
>> |
>> j1
>>
>> I wonder how this applies to modeling the ABB 6400 robot.
>> Does the spanning tree look like so?
>>
>> / j1 -> j2 -> p1 -> p2 -> j3 -> j4 -> j5 -> j6
>> root /
>> \
>> \ j1 -> j2 -> j3 -> j4 -> j5 -> j6
>
> I would stop the first "branch" at j3; because it is there that the
> "zero relative motion" constraint applies. As mentioned above, this
> constraint can be solved simply(?) at the kinematic level; for dynamics,
> the constraint is a zero relative acceleration, for which solvers like
> Vereshchagin's provide a solution. There is a (somewhat outdated)
> implementation of that one in the KDL library.

Great, that’s a valuable pointer...

>
>> Finally, is it true that in such a tree that joints would end up being
>> defined twice?
>
> In my suggestion, only j3 needs to be counted twice. Or rather (as you
> found out yourself, see below), it is not the joints that are to be
> taken into account twice, but the "end effector" after p2 is rigidly
> connected to the link after(?) j3
>
>> Thing is that when adding a segment to a Tree, you cannot refer to joints
>> already present in the tree ( eg a method Tree::addSegment( "jnt2", "jnt3" ),
>> Tree::addSegment( "p2", "jnt3" ) does not exist )

That’s very helpful Herman, many thanks.
Gianni, thanks for your feedback earlier today.

Much appreciated,

-jelle

model a robot with parallelogram / virtual joints / KDL can't so

Dear Jelle,

On 01/19/2015 03:03 PM, jelleferinga [..] ... wrote:
> Hi,
>
> I'm working with a ABB 6400 robot that has a parallelogram ( -> a cycle in
> graph terminology ), hence, my requirement is being able to solve virtual
> joints.
> In order to model that robot properly, it can't be modelled as a chain or
> tree AFAIK, though only as a graph.
>
> Unfortunately, solving a kinematic graph is not something that KDL can deal
> with.

To solve a similar problem (if you are interested in kinematics only),
I expressed the kinematics of the parallelogram (and the link following
the paralleogram, in your case the one btw j3 and j4) as a 2dof chain
and a linear transformation of joint angles.

(this is an ad hoc solution).

> A bug report for this missing feature has been created [1] and vetted for in
> the ensuing discussion [2].
> That discussion sparked a "one-line" design document as described by Herman:
>
> >
>
> j4-------------------------------------j5-------j6
> |
> |
> p2----j3
> | |
> | |
> | |
> | |
> | |
> | |
> p1----j2
> |
> |
> |
> |
> j1
>
> I wonder how this applies to modeling the ABB 6400 robot.
> Does the spanning tree look like so?
>
> / j1 -> j2 -> p1 -> p2 -> j3 -> j4 -> j5 -> j6
> root /
> \
> \ j1 -> j2 -> j3 -> j4 -> j5 -> j6
>
>
> Finally, is it true that in such a tree that joints would end up being
> defined twice?
> Thing is that when adding a segment to a Tree, you cannot refer to joints
> already present in the tree ( eg a method Tree::addSegment( "jnt2", "jnt3" ),
> Tree::addSegment( "p2", "jnt3" ) does not exist )
>
> All in all I'm curious whether some of you have got some ideas for me to
> advance on this subject.
>
> Best,
>
> -jelle
>
> [1] http://bugs.orocos.org/show_bug.cgi?id=967
> [2]
> http://www.orocos.org/forum/rtt/rtt-dev/bug-967-new-using-path-specifica...
>
Cheers,
Gianni.