KDL Segment/Chain representation

Hi,

Here at Willow Garage we have experimented with KDL in the past, but we mostly used our own kinematics/dynamics implementations. We are now in the process of setting up a parallel system using KDL, and if things work out, we want to completely switch to KDL. So I'll be posting on this mailinglist from time to time about the issues we are having :-)


The first thing that came up has to do with how KDL builds Segments and Chains: in KDL a Segment describes a link (the rigid body between two joints), and the joint that connects this link to the previous link. The problem we run into is that the reference frame of each link has to lie on the joint axes, with either its x-y or z axes aligned with the joint axes. This is good for KDL internally, because it allows for more efficient operations on Segments, but it is unintuitive for the user: if you calculate for example the forward position kinematics of a Segment, you are calculating the pose of a (for the user) meaningless frame. It is also less intuitive / more difficult to specify an inertia matrix in this reference frame.


When you are only dealing with the end-effector, this is not really an issue, because you can freely choose the reference frame of your last segment (because you don't need to attach a joint to the last segment). But if you work with the intermediate links, this becomes very awkward.


Could KDL allow the user to freely choose a reference frame for each link, while internally still converting to the current efficient representation? The constructor of a Segment could look something like this:

Segment(reference_frame, joint_point, joint_axes)

With reference_frame the reference frame of the link, relative to the reference frame of the previous link; joint_point a point on the joint axes; joint_axes a vector describing the orientation of the joint axes. Internally, the segment could still convert this to the current representation, but keep track of the user-reference frame.



Cheers, Wim

Ruben Smits's picture

KDL Segment/Chain representation

Begin forwarded message:

> From: Ruben Smits <ruben [dot] smits [..] ...>
> Date: Thu 18 Dec 2008 09:25:51 GMT+01:00
> To: Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>
> Subject: Re: [Orocos-Dev] KDL Segment/Chain representation
>
>
> On 18 Dec 2008, at 08:57, Herman Bruyninckx wrote:
>
>> On Wed, 17 Dec 2008, Wim Meeussen wrote:
>>
>> [...]
>>>>> Could KDL allow the user to freely choose a reference frame for
>>>>> each link, while internally still converting to the current
>>>>> efficient
>>>>> representation? The constructor of a Segment could look something
>>>>> like this:
>>>
>>>> You mean the same kind of constructors we also have for Frames and
>>>> Rotations, where we support different kind of "representations" but
>>>> internally directly convert everything to our "effecient"
>>>> represenation.
>>>
>>> Yes, but as Herman pointed out, you don't want to deal with the
>>> extra
>>> overhead of calculating back and forth between the 'user reference
>>> frame' and the 'internal KDL reference frame'. So the question would
>>> be: can KDL use the user reference frame for its internal
>>> representation, without compromising on efficiency?
>>
>> I don't know, yet... Do you have a suggestion in this direction?
>
> I think we will have to compromise on efficiency a very little bit,
> because we need at least one extra pose transformation for each
> segment pose calculation, because the joint is not located at the
> segments base reference frame anymore.
> But it is located at some arbitrary point in the reference frame of
> the previous segment.
>
> I think this is the only extra calculation for the forward kinematics.
>
> Than you have to define:
>
> Segment(joint,joint_location,refframe_location,inertia_in_refframe)
>
> Calculating the pose of the segment now needs three pose
> transformations, previously we only needed two.
>
> The same holds for the twist calculation, depending on the choice of
> the reference point of the twist, which was previously the endpoint of
> the segment, but in my opinion will change to the reference frame
> origin.
>
> For the twist we will need to do a reference frame and reference point
> transformation from the joints reference frame/point to the segments
> reference frame. Previously we only needed to do a reference point
> transformation from the joints to the segment endpoint.
>
>
> In my opinion this overhead is no reason not to support the more user-
> friendly interface for the joint and the segment.
>
> Ruben
>
>

KDL Segment/Chain representation

On Tue, 16 Dec 2008, Wim Meeussen wrote:

> Here at Willow Garage we have experimented with KDL in the past, but we
> mostly used our own kinematics/dynamics implementations. We are now in the
> process of setting up a parallel system using KDL, and if things work out,
> we want to completely switch to KDL. So I'll be posting on this mailinglist
> from time to time about the issues we are having :-)
>
> The first thing that came up has to do with how KDL builds Segments and
> Chains: in KDL a Segment describes a link (the rigid body between two
> joints), and the joint that connects this link to the previous link. The
> problem we run into is that the reference frame of each link has to lie on
> the joint axes, with either its x-y or z axes aligned with the joint axes.
> This is good for KDL internally, because it allows for more efficient
> operations on Segments, but it is unintuitive for the user: if you calculate
> for example the forward position kinematics of a Segment, you are
> calculating the pose of a (for the user) meaningless frame. It is also less
> intuitive / more difficult to specify an inertia matrix in this reference
> frame.
>
> When you are only dealing with the end-effector, this is not really an
> issue, because you can freely choose the reference frame of your last
> segment (because you don't need to attach a joint to the last segment). But
> if you work with the intermediate links, this becomes very awkward.
>
> Could KDL allow the user to freely choose a reference frame for each link,
> while internally still converting to the current efficient representation?
> The constructor of a Segment could look something like this:
>
> Segment(reference_frame, joint_point, joint_axes)
> With reference_frame the reference frame of the link, relative to the
> reference frame of the previous link; joint_point a point on the joint axes;
> joint_axes a vector describing the orientation of the joint axes.
> Internally, the segment could still convert this to the current
> representation, but keep track of the user-reference frame.

This kind of functionality is certainly useful! But it should go in a
"utilities" or "factory" class (or something with a similar name): we want
to keep the "right" functionality in the right classes, and the current
classes are designed for optimal solver speed. Please think with us about
how to best extend the current functionality while maintaining optimal
decoupling :-)

Herman

PS I am thinking (since quite some time) about a similar improved
decoupling design for BFL...

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

Ruben Smits's picture

KDL Segment/Chain representation

On 16 Dec 2008, at 19:01, Wim Meeussen wrote:

> Hi,
>
> Here at Willow Garage we have experimented with KDL in the past, but
> we mostly used our own kinematics/dynamics implementations. We are
> now in the process of setting up a parallel system using KDL, and if
> things work out, we want to completely switch to KDL. So I'll be
> posting on this mailinglist from time to time about the issues we
> are having :-)
>
> The first thing that came up has to do with how KDL builds Segments
> and Chains: in KDL a Segment describes a link (the rigid body
> between two joints), and the joint that connects this link to the
> previous link. The problem we run into is that the reference frame
> of each link has to lie on the joint axes, with either its x-y or z
> axes aligned with the joint axes. This is good for KDL internally,
> because it allows for more efficient operations on Segments, but it
> is unintuitive for the user: if you calculate for example the
> forward position kinematics of a Segment, you are calculating the
> pose of a (for the user) meaningless frame.

Why is this pose meaningless???

> It is also less intuitive / more difficult to specify an inertia
> matrix in this reference frame.
>

This is true. But at this time the segment is not yet fully supporting
inertia matrices and center of gravities

> When you are only dealing with the end-effector, this is not really
> an issue, because you can freely choose the reference frame of your
> last segment (because you don't need to attach a joint to the last
> segment). But if you work with the intermediate links, this becomes
> very awkward.
>

Can you give an example on how this becomes awkward.

> Could KDL allow the user to freely choose a reference frame for each
> link, while internally still converting to the current efficient
> representation? The constructor of a Segment could look something
> like this:
>

You mean the same kind of constructors we also have for Frames and
Rotations, where we support different kind of "representations" but
internally directly convert everything to our "effecient" represenation.

> Segment(reference_frame, joint_point, joint_axes)
>

But this would only support rotational joints?? How would you define a
joint_axes for a translational joint?

> With reference_frame the reference frame of the link, relative to
> the reference frame of the previous link; joint_point a point on the
> joint axes; joint_axes a vector describing the orientation of the
> joint axes. Internally, the segment could still convert this to the
> current representation, but keep track of the user-reference frame.
>

To do this we would have to change the internal representation,
because if you want to support a arbitrary joint_axis, which is a good
idea btw, you need to put a frame between the previous segment and the
joint.

I think you are mixing different things here, the creation of a joint
with an arbitrary direction, the creation of a segment that allows the
definition of the inertia matrix in another reference frame than the
end-frame of the previous segment and using another reference frame
than the end of a segment in the forward position kinematics.

I would suggest the following:

Change the internal representation of a Joint to allow the creation of
joints with an arbitrary axis, this does not affect the efficiency of
the calculations in the joint.

Joint(Vector axis, Type rot_or_trans);

Add one frame in the internal representation of a segment for the
inertia matrix, this is something we will have to do if we want good
support for inertia matrices.

Segment(Joint joint, Frame end_of_segment_wrt_previous, Frame
reference_for_inertia, Inertia inertia)

I don't know if this is sufficient for you, it does not solve your
"problem" with FK, which i don't really understand.

Ruben

>
> Cheers, Wim
>
>

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

KDL Segment/Chain representation

> Why is this pose meaningless???

Ok, it is not actually meaningless :-) But a user would like to choose
his own reference frame on each link, at a for the user meaningful
location such as the center of mass, or with one axes aligned with the
link, or at some obvious reference point on top of the link, or ....
KDL does not allow you to choose where you put your reference frame.

>> Could KDL allow the user to freely choose a reference frame for each link, while internally still converting to the current efficient
>> representation? The constructor of a Segment could look something like this:

> You mean the same kind of constructors we also have for Frames and Rotations, where we support different kind of "representations" but
> internally directly convert everything to our "effecient" represenation.

Yes, but as Herman pointed out, you don't want to deal with the extra
overhead of calculating back and forth between the 'user reference
frame' and the 'internal KDL reference frame'. So the question would
be: can KDL use the user reference frame for its internal
representation, without compromising on efficiency?

Of course, once you allow for an arbitrary reference frame on each
link, you also need to support for arbitrary joint positions-axes.

> Add one frame in the internal representation of a segment for the
> inertia matrix, this is something we will have to do if we want good
> support for inertia matrices.

That would make it very easy to specify inertia matrices! And as long
as you once convert the inertia matrix to the internal reference
frame, you don't have any extra overhead.

Wim

KDL Segment/Chain representation

On Wed, 17 Dec 2008, Wim Meeussen wrote:

[...]
>>> Could KDL allow the user to freely choose a reference frame for each link, while internally still converting to the current efficient
>>> representation? The constructor of a Segment could look something like this:
>
>> You mean the same kind of constructors we also have for Frames and Rotations, where we support different kind of "representations" but
>> internally directly convert everything to our "effecient" represenation.
>
> Yes, but as Herman pointed out, you don't want to deal with the extra
> overhead of calculating back and forth between the 'user reference
> frame' and the 'internal KDL reference frame'. So the question would
> be: can KDL use the user reference frame for its internal
> representation, without compromising on efficiency?

I don't know, yet... Do you have a suggestion in this direction?

> Of course, once you allow for an arbitrary reference frame on each
> link, you also need to support for arbitrary joint positions-axes.

>> Add one frame in the internal representation of a segment for the
>> inertia matrix, this is something we will have to do if we want good
>> support for inertia matrices.
>
> That would make it very easy to specify inertia matrices! And as long
> as you once convert the inertia matrix to the internal reference
> frame, you don't have any extra overhead.
Indeed. Therefore this functionality should be part of the "build factory",
and not of the "solvers". Which is not difficult to do! :-)

Herman

Ruben Smits's picture

KDL Segment/Chain representation

On 18 Dec 2008, at 08:57, Herman Bruyninckx wrote:

> On Wed, 17 Dec 2008, Wim Meeussen wrote:
>
> [...]
>>>> Could KDL allow the user to freely choose a reference frame for
>>>> each link, while internally still converting to the current
>>>> efficient
>>>> representation? The constructor of a Segment could look something
>>>> like this:
>>
>>> You mean the same kind of constructors we also have for Frames and
>>> Rotations, where we support different kind of "representations" but
>>> internally directly convert everything to our "effecient"
>>> represenation.
>>
>> Yes, but as Herman pointed out, you don't want to deal with the extra
>> overhead of calculating back and forth between the 'user reference
>> frame' and the 'internal KDL reference frame'. So the question would
>> be: can KDL use the user reference frame for its internal
>> representation, without compromising on efficiency?
>
> I don't know, yet... Do you have a suggestion in this direction?

I think we will have to compromise on efficiency a very little bit,
because we need at least one extra pose transformation for each
segment pose calculation, because the joint is not located at the
segments base reference frame anymore.
But it is located at some arbitrary point in the reference frame of
the previous segment.

I think this is the only extra calculation for the forward kinematics.

Than you have to define:

Segment(joint,joint_location,refframe_location,inertia_in_refframe)

Calculating the pose of the segment now needs three pose
transformations, previously we only needed two.

The same holds for the twist calculation, depending on the choice of
the reference point of the twist, which was previously the endpoint of
the segment, but in my opinion will change to the reference frame
origin.

For the twist we will need to do a reference frame and reference point
transformation from the joints reference frame/point to the segments
reference frame. Previously we only needed to do a reference point
transformation from the joints to the segment endpoint.

In my opinion this overhead is no reason not to support the more user-
friendly interface for the joint and the segment.

Ruben

KDL Segment/Chain representation

On Wed, 17 Dec 2008, Ruben Smits wrote:

[...]
>> With reference_frame the reference frame of the link, relative to
>> the reference frame of the previous link; joint_point a point on the
>> joint axes; joint_axes a vector describing the orientation of the
>> joint axes. Internally, the segment could still convert this to the
>> current representation, but keep track of the user-reference frame.
>>
>
> To do this we would have to change the internal representation,
> because if you want to support a arbitrary joint_axis, which is a good
> idea btw, you need to put a frame between the previous segment and the
> joint.
[...]
> Change the internal representation of a Joint to allow the creation of
> joints with an arbitrary axis, this does not affect the efficiency of
> the calculations in the joint.
>
> Joint(Vector axis, Type rot_or_trans);

Implicitly, both of your definitions of joint axis assume all joints have
always just one degree of freedom. This is not true, at least not for many
passive joints, such as the gimbal or spherical joints in parallel designs.
So, I think these are also two needed generalizations:
1. An indication of whether or not a joint is actuated
2. An indication of how many degrees of freedom a joint has.

> Add one frame in the internal representation of a segment for the
> inertia matrix, this is something we will have to do if we want good
> support for inertia matrices.
If this frame is different from the one in which joint forces are going to
be applied, extra computations will be introduced in each "sweep" of a
dynamics solver...

Herman

KDL Segment/Chain representation

On Thu, 18 Dec 2008, Ruben Smits wrote:

[...]
>>> In my opinion this overhead is no reason not to support the more
>>> user-
>>> friendly interface for the joint and the segment.
>>
>> In my opinion it _is_ :-) Why? Because the whole problem can be
>> avoided by
>> separating the "factory" aspects and functionalities from those of the
>> "runtime execution".
>
> I do not agree, because in the case of a seperate reference frame for
> the joint and the segment we do not have a choice. We need to add the
> transformation between the joint reference frame and the segment
> reference frame. If you want your forward kinematics to support
> arbitrary reference frames for segments, this is.
>
>> Doing three operations where we did only two previously is a _huge_
>> loss in efficiency...
>
> If we want to support this kind of functionality we will have to. We
> can off-course always choose not to support this functionality.

I think we are mixing up several things in the same discussion :-)

What I mean is that there are reference frames that are used for different
purposes, and keeping the purposes decoupled in the KDL code is a Good
Thing to do:
- frames that are needed to describe the kinematic chain, and that solvers
need to compute kinematics and dynamics _of the chain_. These frames are
implemented to improve computational efficiency.
- frames that are needed to support user-defined tasks. (We call them
"feature frames".) These frames are chosen to make it as easy as possible
for the human user to specify the robot's task. Such feature frames are
typically attached to the kinematic chain in one way or another, but they
need not necessarily be involved in every kinematic/dynamic iteration of
the kin/dyn solvers.
These feature frames belong to task specification (and visualisation, and
control, ...) and should be defined there; the data structures used there
should allow to point to a kinematic chain, _if_ this knowledge is needed
in the task; we should _not_ do it the opposite way, i.e., adding
"pointers" (let alone new data structures) to the kinematic chain class to
serve yet another application!

Hence, I see both concerns being satisfiable in the most complete and
efficient way. This discussion helped me to make some decouplings a bit
more explicit, thanks! :-)

Herman

PS Ruben and myself are having a similar "design discussion" about the
"joint value" objects that one wants to attach to a kinematic chain: joint
limits, desired joint values, joint accelerations, etc. The idea there is
to leave the current JointArray data structure, since an array makes the
implicit assumption that each joint can be perfectly represented by one
single float or double. And that's not the case, especially not for the
more complex multi-DOF joints that I mentioned before (gimbal, spherical,
human shoulder or knee, ...)

KDL Segment/Chain representation

> - frames that are needed to describe the kinematic chain, and that solvers
> need to compute kinematics and dynamics _of the chain_. These frames are
> implemented to improve computational efficiency.

> - frames that are needed to support user-defined tasks. (We call them
> "feature frames".) These frames are chosen to make it as easy as possible
> for the human user to specify the robot's task. Such feature frames are
> typically attached to the kinematic chain in one way or another, but they
> need not necessarily be involved in every kinematic/dynamic iteration of
> the kin/dyn solvers.

Yes, feature frames are task specific, and do not belong in the chain.
We certainly do not want to attach multiple feature frames to a
segment. The question is: does KDL allow you to choose your own robot
description (robot description = the link reference frames, joint
positions, joint orientations, link inertia, etc.), or does KDL force
you to use a robot description that matches its internal data
structure. We should not confuse allowing a general robot description
with attaching feature frames to a chain. I would like KDL to allow me
to build _my choice_ of robot description (so I choose where I put my
reference frames). And then, for each specific task, I can attach
specific feature frames to _my_ chain.

Allowing this makes KDL much more general and easy to use, and does
not have to compromise efficiency. Allowing the user to choose any
robot description, does not mean KDL cannot use it's own efficient
internal data structure. It would only mean that the solver needs to
change the reference frame at the beginning and at the end of the
chain. This would mean about two extra operations for each solver,
which does not compromise efficiency.

Wim

KDL Segment/Chain representation

On Thu, 18 Dec 2008, Wim Meeussen wrote:

>> - frames that are needed to describe the kinematic chain, and that solvers
>> need to compute kinematics and dynamics _of the chain_. These frames are
>> implemented to improve computational efficiency.
>
>> - frames that are needed to support user-defined tasks. (We call them
>> "feature frames".) These frames are chosen to make it as easy as possible
>> for the human user to specify the robot's task. Such feature frames are
>> typically attached to the kinematic chain in one way or another, but they
>> need not necessarily be involved in every kinematic/dynamic iteration of
>> the kin/dyn solvers.
>
> Yes, feature frames are task specific, and do not belong in the chain.
> We certainly do not want to attach multiple feature frames to a
> segment. The question is: does KDL allow you to choose your own robot
> description (robot description = the link reference frames, joint
> positions, joint orientations, link inertia, etc.), or does KDL force
> you to use a robot description that matches its internal data
> structure. We should not confuse allowing a general robot description
> with attaching feature frames to a chain. I would like KDL to allow me
> to build _my choice_ of robot description (so I choose where I put my
> reference frames). And then, for each specific task, I can attach
> specific feature frames to _my_ chain.

This is the subtle balance that we have to find a good design for! Of
course, I would say "yes!" to your question, but not before I am sure that
it does not compromise the complexity, efficiency and maintainability of
the solvers that we want to support. So, _why exactly_ is the current KDL
"standard" not flexible enough for you?

> Allowing this makes KDL much more general and easy to use,
Prove this!!!! "Anarchy" is not the same thing as "generality", mind you...

(And "ease of use" is a too person-dependent concept to be able to take it
into account seriously :-)

> and does
> not have to compromise efficiency. Allowing the user to choose any
> robot description, does not mean KDL cannot use it's own efficient
> internal data structure. It would only mean that the solver needs to
> change the reference frame at the beginning and at the end of the
> chain. This would mean about two extra operations for each solver,
> which does not compromise efficiency.

Aha! This is something _very_ different from what you said before! Now, you
only want to select _your_ end effector_ and _base_ frames of the whole
chain, instead of any frame on any of the _segments_ in the chain. (Correct
me if I interpret your last paragraph wrongly...) But then the issue comes
back to the difference between _solver_-relevant frames and _task_-relevant
frames: the "frames at the beginning and at the end of the chain" are
indeed _task_-frames, not solver frames, and spending the effort of an
extra transformation from and to these frames is a natural thing to do.

Herman

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

KDL Segment/Chain representation

> So, _why exactly_ is the current KDL "standard" not flexible enough for you?

Let me illustrate with our situation at Willow: we have an xml robot
description format, which allows us to describe a kinematic tree, with
reference frames, joints, inertia parameters etc. We now generate our
own 'chain' object out of this robot description. This chain exactly
matches the parameters from the robot description. Now we want to try
KDL, and run into the problem that KDL only wants to accept some very
specific robot descriptions (where the reference frames are located at
the joints, with one axes along the joint axes). So either we have to
(i) change our robot description so it matches the KDL subset of robot
descriptions, or (ii) we have to write a wrapper around KDL to have a
chain object that matches our robot description. Solution (i) would
mean to reduce our 'general' robot description to a KDL-only robot
description. We don't want to define and maintain a separate robot
description format for each kin/dyn library we use. Solution (ii)
obviously is not the way to go, so hence this discussion on the
mailinglist ;-)

>> Allowing this makes KDL much more general and easy to use,

> Prove this!!!! "Anarchy" is not the same thing as "generality", mind you...

More general in the sense that the subset of robot descriptions it can
represent is larger. And hopefully we can get to a situation where one
robot description can be used to generate chains with many different
kin/dyn libraries, as opposed to the situation where every kin/dyn
library is limited to its own subset of robot descriptions.

> (And "ease of use" is a too person-dependent concept to be able to take it
> into account seriously :-)

Agreed, but just because it is so person-dependent, you would like
each person to choose its own 'easy' reference frames.

>> It would only mean that the solver needs to
>> change the reference frame at the beginning and at the end of the
>> chain. This would mean about two extra operations for each solver,
>> which does not compromise efficiency.

> Aha! This is something _very_ different from what you said before! Now, you
> only want to select _your_ end effector_ and _base_ frames of the whole
> chain, instead of any frame on any of the _segments_ in the chain. (Correct
> me if I interpret your last paragraph wrongly...)

My paragraph wasn't too clear... I meant that when I apply a solver
from segment A in a chain to segment B in a chain, the solver only
needs to change the reference frame at A and at B, but not at the
intermediate segments.

>> Yes, feature frames are task specific, and do not belong in the chain.
>> We certainly do not want to attach multiple feature frames to a
>> segment. The question is: does KDL allow you to choose your own robot
>> description (robot description = the link reference frames, joint
>> positions, joint orientations, link inertia, etc.), or does KDL force
>> you to use a robot description that matches its internal data
>> structure. We should not confuse allowing a general robot description
>> with attaching feature frames to a chain. I would like KDL to allow me
>> to build _my choice_ of robot description (so I choose where I put my
>> reference frames). And then, for each specific task, I can attach
>> specific feature frames to _my_ chain.

> This is the subtle balance that we have to find a good design for! Of
> course, I would say "yes!" to your question, but not before I am sure that
> it does not compromise the complexity, efficiency and maintainability of
> the solvers that we want to support.

Ruben would probably be in the best position to see what this all
means for complexity, efficiency and maintainability.

Wim

KDL Segment/Chain representation

On Fri, 19 Dec 2008, Wim Meeussen wrote:

>> So, _why exactly_ is the current KDL "standard" not flexible enough for you?
>
> Let me illustrate with our situation at Willow: we have an xml robot
> description format, which allows us to describe a kinematic tree, with
> reference frames, joints, inertia parameters etc. We now generate our
> own 'chain' object out of this robot description. This chain exactly
> matches the parameters from the robot description. Now we want to try
> KDL, and run into the problem that KDL only wants to accept some very
> specific robot descriptions (where the reference frames are located at
> the joints, with one axes along the joint axes). So either we have to
> (i) change our robot description so it matches the KDL subset of robot
> descriptions, or (ii) we have to write a wrapper around KDL to have a
> chain object that matches our robot description. Solution (i) would
> mean to reduce our 'general' robot description to a KDL-only robot
> description. We don't want to define and maintain a separate robot
> description format for each kin/dyn library we use. Solution (ii)
> obviously is not the way to go, so hence this discussion on the
> mailinglist ;-)

Try to look at it from the "solver programmer" point of view: how can one
write a generic solver if allowing so much flexibility in how to represent
a kinematic chain...? Maybe (hopefully!) it _is_ possible, but I have no
clear idea how, for the time being...

>>> Allowing this makes KDL much more general and easy to use,
>
>> Prove this!!!! "Anarchy" is not the same thing as "generality", mind you...
>
> More general in the sense that the subset of robot descriptions it can
> represent is larger. And hopefully we can get to a situation where one
> robot description can be used to generate chains with many different
> kin/dyn libraries, as opposed to the situation where every kin/dyn
> library is limited to its own subset of robot descriptions.

I agree! We "just" have to find a good way to fulfill this hope :-)

>> (And "ease of use" is a too person-dependent concept to be able to take it
>> into account seriously :-)
>
> Agreed, but just because it is so person-dependent, you would like
> each person to choose its own 'easy' reference frames.

I would! But just not at the low-level, efficiency-driven, solver level!
I still don't see why the "wrapping" (solution (ii) you described above) is
not an option: it _is_ the same thing as the "factory" functionality that I
have been suggestion a couple of time... I think it would be possible to
make a generic wrapper code (in the sense that it can take any of the
"flexible" formats you mention and turn it automatically into a "standard"
kinematic chain description for our solvers...

>>> It would only mean that the solver needs to
>>> change the reference frame at the beginning and at the end of the
>>> chain. This would mean about two extra operations for each solver,
>>> which does not compromise efficiency.
>
>> Aha! This is something _very_ different from what you said before! Now, you
>> only want to select _your_ end effector_ and _base_ frames of the whole
>> chain, instead of any frame on any of the _segments_ in the chain. (Correct
>> me if I interpret your last paragraph wrongly...)
>
> My paragraph wasn't too clear... I meant that when I apply a solver
> from segment A in a chain to segment B in a chain, the solver only
> needs to change the reference frame at A and at B, but not at the
> intermediate segments.

Ok, so my optimism was misplaced :-(

>>> Yes, feature frames are task specific, and do not belong in the chain.
>>> We certainly do not want to attach multiple feature frames to a
>>> segment. The question is: does KDL allow you to choose your own robot
>>> description (robot description = the link reference frames, joint
>>> positions, joint orientations, link inertia, etc.), or does KDL force
>>> you to use a robot description that matches its internal data
>>> structure. We should not confuse allowing a general robot description
>>> with attaching feature frames to a chain. I would like KDL to allow me
>>> to build _my choice_ of robot description (so I choose where I put my
>>> reference frames). And then, for each specific task, I can attach
>>> specific feature frames to _my_ chain.
>
>> This is the subtle balance that we have to find a good design for! Of
>> course, I would say "yes!" to your question, but not before I am sure that
>> it does not compromise the complexity, efficiency and maintainability of
>> the solvers that we want to support.
>
> Ruben would probably be in the best position to see what this all
> means for complexity, efficiency and maintainability.

Herman

>
>
> Wim
>

Ruben Smits's picture

KDL Segment/Chain representation

On 20 Dec 2008, at 11:42, Herman Bruyninckx wrote:

> On Fri, 19 Dec 2008, Wim Meeussen wrote:
>
>>> So, _why exactly_ is the current KDL "standard" not flexible
>>> enough for you?
>>
>> Let me illustrate with our situation at Willow: we have an xml robot
>> description format, which allows us to describe a kinematic tree,
>> with
>> reference frames, joints, inertia parameters etc. We now generate our
>> own 'chain' object out of this robot description. This chain exactly
>> matches the parameters from the robot description. Now we want to try
>> KDL, and run into the problem that KDL only wants to accept some very
>> specific robot descriptions (where the reference frames are located
>> at
>> the joints, with one axes along the joint axes). So either we have to
>> (i) change our robot description so it matches the KDL subset of
>> robot
>> descriptions, or (ii) we have to write a wrapper around KDL to have a
>> chain object that matches our robot description. Solution (i) would
>> mean to reduce our 'general' robot description to a KDL-only robot
>> description. We don't want to define and maintain a separate robot
>> description format for each kin/dyn library we use. Solution (ii)
>> obviously is not the way to go, so hence this discussion on the
>> mailinglist ;-)
>
> Try to look at it from the "solver programmer" point of view: how
> can one
> write a generic solver if allowing so much flexibility in how to
> represent
> a kinematic chain...? Maybe (hopefully!) it _is_ possible, but I
> have no
> clear idea how, for the time being...
>
>>>> Allowing this makes KDL much more general and easy to use,
>>
>>> Prove this!!!! "Anarchy" is not the same thing as "generality",
>>> mind you...
>>
>> More general in the sense that the subset of robot descriptions it
>> can
>> represent is larger. And hopefully we can get to a situation where
>> one
>> robot description can be used to generate chains with many different
>> kin/dyn libraries, as opposed to the situation where every kin/dyn
>> library is limited to its own subset of robot descriptions.
>
> I agree! We "just" have to find a good way to fulfill this hope :-)
>
>>> (And "ease of use" is a too person-dependent concept to be able to
>>> take it
>>> into account seriously :-)
>>
>> Agreed, but just because it is so person-dependent, you would like
>> each person to choose its own 'easy' reference frames.
>
> I would! But just not at the low-level, efficiency-driven, solver
> level!
> I still don't see why the "wrapping" (solution (ii) you described
> above) is
> not an option: it _is_ the same thing as the "factory" functionality
> that I
> have been suggestion a couple of time... I think it would be
> possible to
> make a generic wrapper code (in the sense that it can take any of the
> "flexible" formats you mention and turn it automatically into a
> "standard"
> kinematic chain description for our solvers...
>
>>>> It would only mean that the solver needs to
>>>> change the reference frame at the beginning and at the end of the
>>>> chain. This would mean about two extra operations for each solver,
>>>> which does not compromise efficiency.
>>
>>> Aha! This is something _very_ different from what you said before!
>>> Now, you
>>> only want to select _your_ end effector_ and _base_ frames of the
>>> whole
>>> chain, instead of any frame on any of the _segments_ in the chain.
>>> (Correct
>>> me if I interpret your last paragraph wrongly...)
>>
>> My paragraph wasn't too clear... I meant that when I apply a solver
>> from segment A in a chain to segment B in a chain, the solver only
>> needs to change the reference frame at A and at B, but not at the
>> intermediate segments.
>
> Ok, so my optimism was misplaced :-(
>
>>>> Yes, feature frames are task specific, and do not belong in the
>>>> chain.
>>>> We certainly do not want to attach multiple feature frames to a
>>>> segment. The question is: does KDL allow you to choose your own
>>>> robot
>>>> description (robot description = the link reference frames, joint
>>>> positions, joint orientations, link inertia, etc.), or does KDL
>>>> force
>>>> you to use a robot description that matches its internal data
>>>> structure. We should not confuse allowing a general robot
>>>> description
>>>> with attaching feature frames to a chain. I would like KDL to
>>>> allow me
>>>> to build _my choice_ of robot description (so I choose where I
>>>> put my
>>>> reference frames). And then, for each specific task, I can attach
>>>> specific feature frames to _my_ chain.
>>
>>> This is the subtle balance that we have to find a good design for!
>>> Of
>>> course, I would say "yes!" to your question, but not before I am
>>> sure that
>>> it does not compromise the complexity, efficiency and
>>> maintainability of
>>> the solvers that we want to support.
>>
>> Ruben would probably be in the best position to see what this all
>> means for complexity, efficiency and maintainability.
>

It would not be so hard to add another member to the segment or chain
class. But where will this end. Maybe someone else would like his kind
of description supported too. I'm currently completely rethinking the
joint-segment-chain structure. Maybe we can find a good solution
there, but i don't like the idea of supporting every kind of kinematic
description for a robot out there. As long as there is no standard.

Ruben

KDL Segment/Chain representation

> It would not be so hard to add another member to the segment or chain class.
> But where will this end. Maybe someone else would like his kind of
> description supported too. I'm currently completely rethinking the
> joint-segment-chain structure. Maybe we can find a good solution there, but
> i don't like the idea of supporting every kind of kinematic description for
> a robot out there. As long as there is no standard.

This is not about supporting a specific robot description. I would
certainly not want KDL to support our specific robot description. All
robot descriptions use their own specific convention of where you
should place the reference frame, where you place the joint etc. The
conversion from a robot description to a KDL chain should be done
outside of KDL. But when KDL allows you to freely choose your
reference frame, it becomes possible to convert any robot description
into a KDL chain. Now only some specific robot descriptions can be
converted into a KDL chain (without wrapping the KDL chain).

Wim

Ruben Smits's picture

KDL Segment/Chain representation

On 17 Dec 2008, at 11:03, Herman Bruyninckx wrote:

> On Wed, 17 Dec 2008, Ruben Smits wrote:
>
> [...]
>>> With reference_frame the reference frame of the link, relative to
>>> the reference frame of the previous link; joint_point a point on the
>>> joint axes; joint_axes a vector describing the orientation of the
>>> joint axes. Internally, the segment could still convert this to the
>>> current representation, but keep track of the user-reference frame.
>>>
>>
>> To do this we would have to change the internal representation,
>> because if you want to support a arbitrary joint_axis, which is a
>> good
>> idea btw, you need to put a frame between the previous segment and
>> the
>> joint.
> [...]
>> Change the internal representation of a Joint to allow the creation
>> of
>> joints with an arbitrary axis, this does not affect the efficiency of
>> the calculations in the joint.
>>
>> Joint(Vector axis, Type rot_or_trans);
>
> Implicitly, both of your definitions of joint axis assume all joints
> have
> always just one degree of freedom. This is not true, at least not
> for many
> passive joints, such as the gimbal or spherical joints in parallel
> designs.
> So, I think these are also two needed generalizations:
> 1. An indication of whether or not a joint is actuated
> 2. An indication of how many degrees of freedom a joint has.

This is because we always used the approach, one degree of freedom for
one joint, if you want more, put them in series, which makes the
choice of order of rotations explicit for the user.

Adding the indication whether or not a joint is actuated is indeed
something we miss, but this is not so hard to add.

So we could change it in:

Joint(Vector axis, Type rot_or_trans, Actuation on_or_off)

>
>
>> Add one frame in the internal representation of a segment for the
>> inertia matrix, this is something we will have to do if we want good
>> support for inertia matrices.
> If this frame is different from the one in which joint forces are
> going to
> be applied, extra computations will be introduced in each "sweep" of a
> dynamics solver...
>

Is this also the case if we internally store the inertia matrix
transformed to the reference frame of the joint? Because this is what
i had in mind.

Ruben

> Herman

KDL Segment/Chain representation

On Wed, 17 Dec 2008, Ruben Smits wrote:

[...]
>> Implicitly, both of your definitions of joint axis assume all joints
>> have
>> always just one degree of freedom. This is not true, at least not
>> for many
>> passive joints, such as the gimbal or spherical joints in parallel
>> designs.
>> So, I think these are also two needed generalizations:
>> 1. An indication of whether or not a joint is actuated
>> 2. An indication of how many degrees of freedom a joint has.
>
> This is because we always used the approach, one degree of freedom for
> one joint, if you want more, put them in series, which makes the
> choice of order of rotations explicit for the user.

This does _not_ work for _real_ multi-DOF joints! Because your approach
then gives problems about which inertia to assign to this "virtual segment"
between these two joints! This is a _fundamental_ problem that all of the
current implementations have...

> Adding the indication whether or not a joint is actuated is indeed
> something we miss, but this is not so hard to add.
>
> So we could change it in:
>
> Joint(Vector axis, Type rot_or_trans, Actuation on_or_off)

The arguments you give ("rot_or_trans", "on_or_off") are good as
_illustrations_. But we should discuss about which types we offer. I would
like to see KDL used (eventually) also to simulate human dynamics, and
there you have the concept of "multi-joint actuation", that is, one muscle
that actuates more than one joint, _and_ the same joint is actuated by more
than one muscle...

>>> Add one frame in the internal representation of a segment for the
>>> inertia matrix, this is something we will have to do if we want good
>>> support for inertia matrices.
>> If this frame is different from the one in which joint forces are
>> going to
>> be applied, extra computations will be introduced in each "sweep" of a
>> dynamics solver...
>
> Is this also the case if we internally store the inertia matrix
> transformed to the reference frame of the joint? Because this is what
> i had in mind.
I agree.

Herman

Ruben Smits's picture

KDL Segment/Chain representation

On 17 Dec 2008, at 16:07, Herman Bruyninckx wrote:

> On Wed, 17 Dec 2008, Ruben Smits wrote:
>
> [...]
>>> Implicitly, both of your definitions of joint axis assume all joints
>>> have
>>> always just one degree of freedom. This is not true, at least not
>>> for many
>>> passive joints, such as the gimbal or spherical joints in parallel
>>> designs.
>>> So, I think these are also two needed generalizations:
>>> 1. An indication of whether or not a joint is actuated
>>> 2. An indication of how many degrees of freedom a joint has.
>>
>> This is because we always used the approach, one degree of freedom
>> for
>> one joint, if you want more, put them in series, which makes the
>> choice of order of rotations explicit for the user.
>
> This does _not_ work for _real_ multi-DOF joints! Because your
> approach
> then gives problems about which inertia to assign to this "virtual
> segment"
> between these two joints! This is a _fundamental_ problem that all
> of the
> current implementations have...

Do you have a suggestion on how to cope with this using the current
implementation? Do we go on with an intermediate solution using the
current implementation, or do you suggest to drastically change the
implementation to find a solution for the _real_ multi-DOF joints.

>
>
>> Adding the indication whether or not a joint is actuated is indeed
>> something we miss, but this is not so hard to add.
>>
>> So we could change it in:
>>
>> Joint(Vector axis, Type rot_or_trans, Actuation on_or_off)
>
> The arguments you give ("rot_or_trans", "on_or_off") are good as
> _illustrations_. But we should discuss about which types we offer. I
> would
> like to see KDL used (eventually) also to simulate human dynamics, and
> there you have the concept of "multi-joint actuation", that is, one
> muscle
> that actuates more than one joint, _and_ the same joint is actuated
> by more
> than one muscle...
>

Can't you use external forces on the joints/segments to simulate this?

>>>> Add one frame in the internal representation of a segment for the
>>>> inertia matrix, this is something we will have to do if we want
>>>> good
>>>> support for inertia matrices.
>>> If this frame is different from the one in which joint forces are
>>> going to
>>> be applied, extra computations will be introduced in each "sweep"
>>> of a
>>> dynamics solver...
>>
>> Is this also the case if we internally store the inertia matrix
>> transformed to the reference frame of the joint? Because this is what
>> i had in mind.
> I agree.
>

I can work on a proposal patch to change the current implementation of
the joint and segment class, without having to change the solver
classes.

Ruben

KDL Segment/Chain representation

On Thu, 18 Dec 2008, Ruben Smits wrote:

> On 18 Dec 2008, at 08:57, Herman Bruyninckx wrote:
>
>> On Wed, 17 Dec 2008, Wim Meeussen wrote:
>>
>> [...]
>>>>> Could KDL allow the user to freely choose a reference frame for
>>>>> each link, while internally still converting to the current
>>>>> efficient
>>>>> representation? The constructor of a Segment could look something
>>>>> like this:
>>>
>>>> You mean the same kind of constructors we also have for Frames and
>>>> Rotations, where we support different kind of "representations" but
>>>> internally directly convert everything to our "effecient"
>>>> represenation.
>>>
>>> Yes, but as Herman pointed out, you don't want to deal with the extra
>>> overhead of calculating back and forth between the 'user reference
>>> frame' and the 'internal KDL reference frame'. So the question would
>>> be: can KDL use the user reference frame for its internal
>>> representation, without compromising on efficiency?
>>
>> I don't know, yet... Do you have a suggestion in this direction?
>
> I think we will have to compromise on efficiency a very little bit,
> because we need at least one extra pose transformation for each
> segment pose calculation, because the joint is not located at the
> segments base reference frame anymore.
> But it is located at some arbitrary point in the reference frame of
> the previous segment.
>
> I think this is the only extra calculation for the forward kinematics.
>
> Than you have to define:
>
> Segment(joint,joint_location,refframe_location,inertia_in_refframe)
>
> Calculating the pose of the segment now needs three pose
> transformations, previously we only needed two.
>
> The same holds for the twist calculation, depending on the choice of
> the reference point of the twist, which was previously the endpoint of
> the segment, but in my opinion will change to the reference frame
> origin.
>
> For the twist we will need to do a reference frame and reference point
> transformation from the joints reference frame/point to the segments
> reference frame. Previously we only needed to do a reference point
> transformation from the joints to the segment endpoint.
>
>
> In my opinion this overhead is no reason not to support the more user-
> friendly interface for the joint and the segment.

In my opinion it _is_ :-) Why? Because the whole problem can be avoided by
separating the "factory" aspects and functionalities from those of the
"runtime execution".

Doing three operations where we did only two previously is a _huge_ loss in
efficiency...

Herman

Ruben Smits's picture

KDL Segment/Chain representation

On 18 Dec 2008, at 12:33, Herman Bruyninckx wrote:

> On Thu, 18 Dec 2008, Ruben Smits wrote:
>
>> On 18 Dec 2008, at 08:57, Herman Bruyninckx wrote:
>>
>>> On Wed, 17 Dec 2008, Wim Meeussen wrote:
>>>
>>> [...]
>>>>>> Could KDL allow the user to freely choose a reference frame for
>>>>>> each link, while internally still converting to the current
>>>>>> efficient
>>>>>> representation? The constructor of a Segment could look something
>>>>>> like this:
>>>>
>>>>> You mean the same kind of constructors we also have for Frames and
>>>>> Rotations, where we support different kind of "representations"
>>>>> but
>>>>> internally directly convert everything to our "effecient"
>>>>> represenation.
>>>>
>>>> Yes, but as Herman pointed out, you don't want to deal with the
>>>> extra
>>>> overhead of calculating back and forth between the 'user reference
>>>> frame' and the 'internal KDL reference frame'. So the question
>>>> would
>>>> be: can KDL use the user reference frame for its internal
>>>> representation, without compromising on efficiency?
>>>
>>> I don't know, yet... Do you have a suggestion in this direction?
>>
>> I think we will have to compromise on efficiency a very little bit,
>> because we need at least one extra pose transformation for each
>> segment pose calculation, because the joint is not located at the
>> segments base reference frame anymore.
>> But it is located at some arbitrary point in the reference frame of
>> the previous segment.
>>
>> I think this is the only extra calculation for the forward
>> kinematics.
>>
>> Than you have to define:
>>
>> Segment(joint,joint_location,refframe_location,inertia_in_refframe)
>>
>> Calculating the pose of the segment now needs three pose
>> transformations, previously we only needed two.
>>
>> The same holds for the twist calculation, depending on the choice of
>> the reference point of the twist, which was previously the endpoint
>> of
>> the segment, but in my opinion will change to the reference frame
>> origin.
>>
>> For the twist we will need to do a reference frame and reference
>> point
>> transformation from the joints reference frame/point to the segments
>> reference frame. Previously we only needed to do a reference point
>> transformation from the joints to the segment endpoint.
>>
>>
>> In my opinion this overhead is no reason not to support the more
>> user-
>> friendly interface for the joint and the segment.
>
> In my opinion it _is_ :-) Why? Because the whole problem can be
> avoided by
> separating the "factory" aspects and functionalities from those of the
> "runtime execution".
>

I do not agree, because in the case of a seperate reference frame for
the joint and the segment we do not have a choice. We need to add the
transformation between the joint reference frame and the segment
reference frame. If you want your forward kinematics to support
arbitrary reference frames for segments, this is.

> Doing three operations where we did only two previously is a _huge_
> loss in
> efficiency...
>

If we want to support this kind of functionality we will have to. We
can off-course always choose not to support this functionality.

Ruben

KDL Segment/Chain representation

On Wed, 17 Dec 2008, Ruben Smits wrote:

> On 17 Dec 2008, at 16:07, Herman Bruyninckx wrote:
>
>> On Wed, 17 Dec 2008, Ruben Smits wrote:
>>
>> [...]
>>>> Implicitly, both of your definitions of joint axis assume all joints
>>>> have
>>>> always just one degree of freedom. This is not true, at least not
>>>> for many
>>>> passive joints, such as the gimbal or spherical joints in parallel
>>>> designs.
>>>> So, I think these are also two needed generalizations:
>>>> 1. An indication of whether or not a joint is actuated
>>>> 2. An indication of how many degrees of freedom a joint has.
>>>
>>> This is because we always used the approach, one degree of freedom
>>> for
>>> one joint, if you want more, put them in series, which makes the
>>> choice of order of rotations explicit for the user.
>>
>> This does _not_ work for _real_ multi-DOF joints! Because your
>> approach
>> then gives problems about which inertia to assign to this "virtual
>> segment"
>> between these two joints! This is a _fundamental_ problem that all
>> of the
>> current implementations have...
>
> Do you have a suggestion on how to cope with this using the current
> implementation? Do we go on with an intermediate solution using the
> current implementation, or do you suggest to drastically change the
> implementation to find a solution for the _real_ multi-DOF joints.
>
The solution is indeed as simple as having a specific solver that can deal
with this situation. What this solver has to be able to do is to solve a
(very local) "loop" in the tree structure of the kinematic chain. In other
words, solve a linear system of n equations in n unknowns, where n is the
number of collocated joints. _Conceptually_ simple, but always a bit
tougher in practice than one thinks...

>>> Adding the indication whether or not a joint is actuated is indeed
>>> something we miss, but this is not so hard to add.
>>>
>>> So we could change it in:
>>>
>>> Joint(Vector axis, Type rot_or_trans, Actuation on_or_off)
>>
>> The arguments you give ("rot_or_trans", "on_or_off") are good as
>> _illustrations_. But we should discuss about which types we offer. I
>> would
>> like to see KDL used (eventually) also to simulate human dynamics, and
>> there you have the concept of "multi-joint actuation", that is, one
>> muscle
>> that actuates more than one joint, _and_ the same joint is actuated
>> by more
>> than one muscle...
>>
>
> Can't you use external forces on the joints/segments to simulate this?

Yes. But that is like throwing the problem over a wall, for the next guy to
solve :-) And the further away from the internal data structures you are
(i.e., the more walls you throw your problem over) the more difficult it
will get to provide _efficient_ solutions. And I have a fundamental
objection: the muscles are not really "external" forces when simulating the
human motor system :-)

>>>>> Add one frame in the internal representation of a segment for the
>>>>> inertia matrix, this is something we will have to do if we want
>>>>> good
>>>>> support for inertia matrices.
>>>> If this frame is different from the one in which joint forces are
>>>> going to
>>>> be applied, extra computations will be introduced in each "sweep"
>>>> of a
>>>> dynamics solver...
>>>
>>> Is this also the case if we internally store the inertia matrix
>>> transformed to the reference frame of the joint? Because this is what
>>> i had in mind.
>> I agree.

> I can work on a proposal patch to change the current implementation of
> the joint and segment class, without having to change the solver
> classes.

Proposal patches are always good to have and discuss! :-)
Of course, not changing the existing solvers is nice to have, but this
"legacy" should not be an obstable to fundamental progress...

Herman

Ruben Smits's picture

KDL Segment/Chain representation

On 17 Dec 2008, at 17:08, Herman Bruyninckx wrote:

> On Wed, 17 Dec 2008, Ruben Smits wrote:
>
>> On 17 Dec 2008, at 16:07, Herman Bruyninckx wrote:
>>
>>> On Wed, 17 Dec 2008, Ruben Smits wrote:
>>>
>>> [...]
>>>>> Implicitly, both of your definitions of joint axis assume all
>>>>> joints
>>>>> have
>>>>> always just one degree of freedom. This is not true, at least not
>>>>> for many
>>>>> passive joints, such as the gimbal or spherical joints in parallel
>>>>> designs.
>>>>> So, I think these are also two needed generalizations:
>>>>> 1. An indication of whether or not a joint is actuated
>>>>> 2. An indication of how many degrees of freedom a joint has.
>>>>
>>>> This is because we always used the approach, one degree of freedom
>>>> for
>>>> one joint, if you want more, put them in series, which makes the
>>>> choice of order of rotations explicit for the user.
>>>
>>> This does _not_ work for _real_ multi-DOF joints! Because your
>>> approach
>>> then gives problems about which inertia to assign to this "virtual
>>> segment"
>>> between these two joints! This is a _fundamental_ problem that all
>>> of the
>>> current implementations have...
>>
>> Do you have a suggestion on how to cope with this using the current
>> implementation? Do we go on with an intermediate solution using the
>> current implementation, or do you suggest to drastically change the
>> implementation to find a solution for the _real_ multi-DOF joints.
>>
> The solution is indeed as simple as having a specific solver that
> can deal
> with this situation. What this solver has to be able to do is to
> solve a
> (very local) "loop" in the tree structure of the kinematic chain. In
> other
> words, solve a linear system of n equations in n unknowns, where n
> is the
> number of collocated joints. _Conceptually_ simple, but always a bit
> tougher in practice than one thinks...

Adding multi-dof joints makes it harder to find a generic state-less
interface for the joint (and the segment), now the pose and twist of a
joint/segment are function of one variable (q, qdot), this makes it
easy for every solver who needs this informations, if we want to
support multi-dof joints (having more than one input) finding a
generic interface (implementation wise) will be a lot harder.

If we stick to the current interface the problem has to be solved at
the solver level, and the joint only contains the needed information
for the solver to solve this problem, being, is this joint collocated
with another one or not. Or is this not a solution with enough
decoupling?

>
>
>>>> Adding the indication whether or not a joint is actuated is indeed
>>>> something we miss, but this is not so hard to add.
>>>>
>>>> So we could change it in:
>>>>
>>>> Joint(Vector axis, Type rot_or_trans, Actuation on_or_off)
>>>
>>> The arguments you give ("rot_or_trans", "on_or_off") are good as
>>> _illustrations_. But we should discuss about which types we offer. I
>>> would
>>> like to see KDL used (eventually) also to simulate human dynamics,
>>> and
>>> there you have the concept of "multi-joint actuation", that is, one
>>> muscle
>>> that actuates more than one joint, _and_ the same joint is actuated
>>> by more
>>> than one muscle...
>>>
>>
>> Can't you use external forces on the joints/segments to simulate
>> this?
>
> Yes. But that is like throwing the problem over a wall, for the next
> guy to
> solve :-) And the further away from the internal data structures you
> are
> (i.e., the more walls you throw your problem over) the more
> difficult it
> will get to provide _efficient_ solutions. And I have a fundamental
> objection: the muscles are not really "external" forces when
> simulating the
> human motor system :-)

But then we are not talking about a joint primitive anymore (i think)
but about a kinematic loop in a kinematic description, where there are
a lot of passive joints which are actuated (in kinematic loops) by
parallel segments (the muscle). So adding multi-joint actuation is
actually solved at the level of the kinematic description, or am i
missing the ball completely here?

Ruben