"dynamic" KDL::Frame

Hi

I have a kinematic chain describing my machine. Now I'd like to
model the tool offsets. The CartesianVelocityController adds a
KDL::Segment(KDL::Joint(KDL::Joint::None), TOOLFRAME) to the
chain for this purpose. This is actually almost what I need
(offsets for xyz and rpy), but I need it to be dynamic.
Eg. I'd like to dynamically change tool radius and tool legth
and tool orientation.

Do I have to do that with a KDL::Chain with 3 translational joints
and 3 rotational joints. Is there no possibility to dynamically
change the values of a frame and add it as a single Segment?

thanks for any replies
regards
Marc

"dynamic" KDL::Frame

On Thu, 5 Feb 2009, marc [dot] bodmer [..] ... wrote:

> I have a kinematic chain describing my machine. Now I'd like to
> model the tool offsets. The CartesianVelocityController adds a
> KDL::Segment(KDL::Joint(KDL::Joint::None), TOOLFRAME) to the
> chain for this purpose. This is actually almost what I need
> (offsets for xyz and rpy), but I need it to be dynamic.
> Eg. I'd like to dynamically change tool radius and tool legth
> and tool orientation.
>
> Do I have to do that with a KDL::Chain with 3 translational joints
> and 3 rotational joints. Is there no possibility to dynamically
> change the values of a frame and add it as a single Segment?
>
The easiest way would be to consider a TOOLFRAME as what it is: a frame.
Then you can dynamically (re)set the properties of that frame, i.e., its
position and orientation wrt to the end effector (= the last Segment) of
the chain. Modelling such an offset by a virtual kinematic chain is
overkill :-)

Herman

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

Thanks for this fast

Thanks for this fast reply.

I am a little slow with understanding. How can I dynamically reset the properties of that frame?
Right now I only see:
- removing the last segment of the chain
- creating a new frame with the new "reset" values
- adding a new segment containing this offset frame
- newly initialize the solvers with this new chain

I am probably messing some things up now...

Ruben Smits's picture

"dynamic" KDL::Frame

On Thursday 05 February 2009 15:37:15 Herman Bruyninckx wrote:
> On Thu, 5 Feb 2009, marc [dot] bodmer [..] ... wrote:
> > I have a kinematic chain describing my machine. Now I'd like to
> > model the tool offsets. The CartesianVelocityController adds a
> > KDL::Segment(KDL::Joint(KDL::Joint::None), TOOLFRAME) to the
> > chain for this purpose. This is actually almost what I need
> > (offsets for xyz and rpy), but I need it to be dynamic.
> > Eg. I'd like to dynamically change tool radius and tool legth
> > and tool orientation.
> >
> > Do I have to do that with a KDL::Chain with 3 translational joints
> > and 3 rotational joints. Is there no possibility to dynamically
> > change the values of a frame and add it as a single Segment?
>
> The easiest way would be to consider a TOOLFRAME as what it is: a frame.
> Then you can dynamically (re)set the properties of that frame, i.e., its
> position and orientation wrt to the end effector (= the last Segment) of
> the chain. Modelling such an offset by a virtual kinematic chain is
> overkill :-)
>

The problem is that the chain structure itself cannot contain this
information, because all solvers have there own copy of the chain, with there
own copies of the segments. This is to protect the chain and segments from
changing after the solvers are created. If you want support for a dynamic
toolframe you have to put it in your solver/controller, and not put it in the
chain as the CartesianVelocityController is doing. I don't think it should be
to hard to create a controller that can handle this.

Ruben

"dynamic" KDL::Frame

On Thu, 5 Feb 2009, Ruben Smits wrote:

> On Thursday 05 February 2009 15:37:15 Herman Bruyninckx wrote:
>> On Thu, 5 Feb 2009, marc [dot] bodmer [..] ... wrote:
>>> I have a kinematic chain describing my machine. Now I'd like to
>>> model the tool offsets. The CartesianVelocityController adds a
>>> KDL::Segment(KDL::Joint(KDL::Joint::None), TOOLFRAME) to the
>>> chain for this purpose. This is actually almost what I need
>>> (offsets for xyz and rpy), but I need it to be dynamic.
>>> Eg. I'd like to dynamically change tool radius and tool legth
>>> and tool orientation.
>>>
>>> Do I have to do that with a KDL::Chain with 3 translational joints
>>> and 3 rotational joints. Is there no possibility to dynamically
>>> change the values of a frame and add it as a single Segment?
>>
>> The easiest way would be to consider a TOOLFRAME as what it is: a frame.
>> Then you can dynamically (re)set the properties of that frame, i.e., its
>> position and orientation wrt to the end effector (= the last Segment) of
>> the chain. Modelling such an offset by a virtual kinematic chain is
>> overkill :-)
>>
>
> The problem is that the chain structure itself cannot contain this
> information, because all solvers have there own copy of the chain, with there
> own copies of the segments. This is to protect the chain and segments from
> changing after the solvers are created. If you want support for a dynamic
> toolframe you have to put it in your solver/controller, and not put it in the
> chain as the CartesianVelocityController is doing. I don't think it should be
> to hard to create a controller that can handle this.
>
> Ruben
>
By the way, ongoing research at our lab deals with very dynamically
changing TOOLFRAMEs, i.e., their position and orientation can change each
time instant because (i) it is influenced by a dynamic _process_
(flexibility in the tool, toolwear, ...), or (ii) it is _estimated_ from
sensor information. Our approach for such cases is, indeed, along the lines
of what you suggested in your original post: to introduce a virtual chain
to represent the moving TOOLFRAME.

In case you only have to deal with discrete changes in TOOLFRAMEs (such as
selection a new one after a tool change) it could make more sense to cope
with it in your solver, or in the "component-level FSM logic" that
configures your solver. (But then we are speaking at the level of
'activities' and not just class libraries.)

Herman

"dynamic" KDL::Frame

> By the way, ongoing research at our lab deals with very dynamically
> changing TOOLFRAMEs, i.e., their position and orientation can change each
> time instant because (i) it is influenced by a dynamic _process_
> (flexibility in the tool, toolwear, ...), or (ii) it is _estimated_ from

This is actually the problem I am trying to solve. The Tool
correction can change every timestep, since it is a dynamic process (kerf compensation is feed dependent).
So it would be best to create a virtual TOOL CHAIN instead of a TOOL FRAME?

Thanks, this was helpful

"dynamic" KDL::Frame

On Thu, 5 Feb 2009, marc [dot] bodmer [..] ... wrote:

>> By the way, ongoing research at our lab deals with very dynamically
>> changing TOOLFRAMEs, i.e., their position and orientation can change each
>> time instant because (i) it is influenced by a dynamic _process_
>> (flexibility in the tool, toolwear, ...), or (ii) it is _estimated_ from
>
> This is actually the problem I am trying to solve. The Tool
> correction can change every timestep, since it is a dynamic process (kerf compensation is feed dependent).
> So it would be best to create a virtual TOOL CHAIN instead of a TOOL FRAME?

yes, especially together with our more advanced "tool frame" (motion)
specification/estimation framework that is being developed. In case you are
interested in what this involves, you could take a look at a demo that we
have online:
<http://www.orocos.org/node/766>
where a prototype implementation is being demoed. It is not directly
visible in the video, but this controller has several "dynamic toolframes"
in it, each updated in realtime and online, on the basis of (i) a dynamic
model (which is simple in this demo) and (ii) online sensor information.

However, this demo involves more than just KDL: also BFL and the RTT (via
TaskComponents) are involved.

> Thanks, this was helpful

Great :-)

Herman

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