Hard time

Hello,

I am trying to make a realtime inverse kinematics system using kdl in python. However i am having a hard time trying to understand how it works. Unfortunately the examples and api documentation didnt get me much further.

What i am trying to accomplish:
- Simulate a human arm
- 3d visualisation of the arm using cones (with panda3d, panda3d.org)
- using a 3d position (which i am getting trought a wiimote) to inverse calculate the arm bones position/rotation

As i understand, kdl should be able to calculate this for me.

What i currently understand (correct me if im wrong):
- a chain is built up from segments
- a segment contains a joint, which handles the rotation, and a frame, which defines the distance to the next segment.
- if i need a joint which rotates in multiple directions, i define multiple joints with a frame (0,0,0)

What i dont understand:
- how do i define a point to which the tip of the chain should move to (and thus calculate the pos/rot of the bones)
- how do i let it calculate the inverse kinematics
- how do i assign the results to the chain
- how do i get the position of the bones out of kdl to import it in another system (best would be Vec3-rotation, Vec3-position, relative to the previous segement, but could be Mat3x3 or Mat4x4 as well)

Tnx Hypnos

Ruben Smits's picture

Hard time

On Thursday March 27 2008 19:15:05 Hypnos wrote:
> Hello,
>
> I am trying to make a realtime inverse kinematics system using kdl in
> python. However i am having a hard time trying to understand how it works.
> Unfortunately the examples and api documentation didnt get me much further.
>
> What i am trying to accomplish:
> - Simulate a human arm
> - 3d visualisation of the arm using cones (with panda3d, panda3d.org)
> - using a 3d position (which i am getting trought a wiimote) to inverse
> calculate the arm bones position/rotation
>
> As i understand, kdl should be able to calculate this for me.
>
> What i currently understand (correct me if im wrong):
> - a chain is built up from segments
> - a segment contains a joint, which handles the rotation,

or translation for prismatic joints

> and a frame,
> which defines the distance to the next segment. - if i need a joint which
> rotates in multiple directions, i define multiple joints with a frame
> (0,0,0)

correct

> What i dont understand:
> - how do i define a point to which the tip of the chain should move to (and
> thus calculate the pos/rot of the bones) - how do i let it calculate the
> inverse kinematics

KDL has some solvers for generic chains,

Forward position kinematics can be solved using generic recursive solvers:

chainfksolverpos_recursive:

chainfksolvervel_recursive:

for inverse velocity kinematics we have a generic solver:

chainiksolvervel_pinv:

or chainiksolvervel_pinv_boost_givens:

these solvers use the pseudo-inverse of the jacobian to calculate inverse
kinematics. Expect problems near kinematic singularities.

For inverse position kinematics we have a generic solver:

chainiksolverpos_nr:

this solver uses a Newton-Rhapson method, an inverse velocity solver and a
forward position solver. Expect problems with local minima and near kinematic
singularities.

> - how do i assign the results to the chain

Like Herman said, the chain is stateless, you have keep the results youself

> - how do i get the position of the bones out of kdl to import it in another
> system (best would be Vec3-rotation, Vec3-position, relative to the
> previous segement, but could be Mat3x3 or Mat4x4 as well)

The intermediate bone positions can be calculated using the
chainfksolverpos_recursive and the result is stored in a KDL::Frame (which
contains a 3x3 rotation matrix and a 3x1 position vector), you can treat it
as a Mat4x4 using the operator(i,j).

see:
http://people.mech.kuleuven.be/~psoetens/orocos/devel/kdl/latest/api/html/classKDL_1_1Frame.html

Ruben

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

Hard time

On Thu, 27 Mar 2008, Hypnos wrote:

> I am trying to make a realtime inverse kinematics system using kdl in python.

Python and realtime....? maybe that doesn't go together too well :-)

> However i am having a hard time trying to understand how it works.
> Unfortunately the examples and api documentation didnt get me much further.
>
> What i am trying to accomplish:
> - Simulate a human arm
> - 3d visualisation of the arm using cones (with panda3d, panda3d.org)
> - using a 3d position (which i am getting trought a wiimote) to inverse
> calculate the arm bones position/rotation
>
> As i understand, kdl should be able to calculate this for me.
>
> What i currently understand (correct me if im wrong):
> - a chain is built up from segments
> - a segment contains a joint, which handles the rotation, and a frame, which
> defines the distance to the next segment.
> - if i need a joint which rotates in multiple directions, i define multiple
> joints with a frame (0,0,0)
Yes.

> What i dont understand:
> - how do i define a point to which the tip of the chain should move to (and
> thus calculate the pos/rot of the bones)

KDL currently only covers _instantaneous_ kinematics, so no trajectories,
but only velocities.

> - how do i let it calculate the inverse kinematics
There are abstract classes for all instantaneous kinematics routines, which
you have to implement for each particular chain.

> - how do i assign the results to the chain
The "state" of a chain is kept separate from the description of the chain.

> - how do i get the position of the bones out of kdl to import it in another
> system (best would be Vec3-rotation, Vec3-position, relative to the previous
> segement, but could be Mat3x3 or Mat4x4 as well)

Herman

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

Hard time

Herman Bruyninckx schrieb:
> On Thu, 27 Mar 2008, Hypnos wrote:
>
>> I am trying to make a realtime inverse kinematics system using kdl in
>> python.
>
> Python and realtime....? maybe that doesn't go together too well :-)
>
>> However i am having a hard time trying to understand how it works.
>> Unfortunately the examples and api documentation didnt get me much
>> further.
>>
>> What i am trying to accomplish:
>> - Simulate a human arm
>> - 3d visualisation of the arm using cones (with panda3d, panda3d.org)
>> - using a 3d position (which i am getting trought a wiimote) to
>> inverse calculate the arm bones position/rotation
>>
>> As i understand, kdl should be able to calculate this for me.
>>
>> What i currently understand (correct me if im wrong):
>> - a chain is built up from segments
>> - a segment contains a joint, which handles the rotation, and a
>> frame, which defines the distance to the next segment.
>> - if i need a joint which rotates in multiple directions, i define
>> multiple
>> joints with a frame (0,0,0)
> Yes.
>
>> What i dont understand:
>> - how do i define a point to which the tip of the chain should move
>> to (and thus calculate the pos/rot of the bones)
>
> KDL currently only covers _instantaneous_ kinematics, so no trajectories,
> but only velocities.
>
>> - how do i let it calculate the inverse kinematics
> There are abstract classes for all instantaneous kinematics routines,
> which
> you have to implement for each particular chain.
>
>> - how do i assign the results to the chain
> The "state" of a chain is kept separate from the description of the
> chain.
>
>> - how do i get the position of the bones out of kdl to import it in
>> another system (best would be Vec3-rotation, Vec3-position, relative
>> to the previous segement, but could be Mat3x3 or Mat4x4 as well)
>
>
> Herman
>
> Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm
>
>
Thanks a lot for your answer, however the only thing i learned from it,
is that the "state" of the chain is separate from the description...

If someone could give me a example of a implementation (not just checks
if the math of kdl work) i would greatly appreciate it. (i have seen the
forward kinematics example, but that help me much as well).

Reto

Hard time

On Friday 28 March 2008 11:56:22 Reto Spoerri wrote:
>
> Thanks a lot for your answer, however the only thing i learned from it,
> is that the "state" of the chain is separate from the description...
>
> If someone could give me a example of a implementation (not just checks
> if the math of kdl work) i would greatly appreciate it. (i have seen the
> forward kinematics example, but that help me much as well).

Ruben may have an answer for you, but he's in holidays until monday. The best
shot you've got is looking at how KDL is used in OCL (C++).

Interpolator:

Inverse kyn:

configureHook() sets up stuff, updateHook() does the periodic calculation.

Peter