new user

Hello,

I'm writing a virtual reality application and I need to move the avatar
skeleton using various sensor devices (kinect and razer hydra). I was
trying to use orocos_kdl for inverse kinematics but I'm a bit lost...
Basically what I need is to position the arms chains using a target
position. That target position might be out of reach, in this case I want
the arm to point towards the target (similar to what is done in blender for
example). If possible I'd like to give a hint for the elbow position. And
that needs to be real-time to follow the user movements.

What IK solver should I use ?

Ruben Smits's picture

new user

On Tue, Sep 16, 2014 at 6:28 PM, Charles Prévot <prevot [..] ...> wrote:

> Hello,
>
> I'm writing a virtual reality application and I need to move the avatar
> skeleton using various sensor devices (kinect and razer hydra). I was
> trying to use orocos_kdl for inverse kinematics but I'm a bit lost...
> Basically what I need is to position the arms chains using a target
> position. That target position might be out of reach, in this case I want
> the arm to point towards the target (similar to what is done in blender for
> example). If possible I'd like to give a hint for the elbow position. And
> that needs to be real-time to follow the user movements.
>
>
I would propose to use either

* the ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls underneath with a
lambda != 0 to make sure the stretching out motion gets damped or
* the ChainIKSolverPos_LMA (I have no idea myself how this one behaves if
the structure gets stretched out)

More examples can be found on www.orocos.org/kdl/examples,

for instance http://www.orocos.org/kdl/examples#comment-1724 might be of
your interest.

Ruben

What IK solver should I use ?
>
>
>
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...
> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>
>

new user

2014-09-18 11:50 GMT+02:00 Ruben Smits <ruben [dot] smits [..] ...>:

>
>
> On Tue, Sep 16, 2014 at 6:28 PM, Charles Prévot <prevot [..] ...>
> wrote:
>
>> Hello,
>>
>> I'm writing a virtual reality application and I need to move the avatar
>> skeleton using various sensor devices (kinect and razer hydra). I was
>> trying to use orocos_kdl for inverse kinematics but I'm a bit lost...
>> Basically what I need is to position the arms chains using a target
>> position. That target position might be out of reach, in this case I want
>> the arm to point towards the target (similar to what is done in blender for
>> example). If possible I'd like to give a hint for the elbow position. And
>> that needs to be real-time to follow the user movements.
>>
>>
> I would propose to use either
>
> * the ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls underneath with
> a lambda != 0 to make sure the stretching out motion gets damped or
> * the ChainIKSolverPos_LMA (I have no idea myself how this one behaves if
> the structure gets stretched out)
>
> More examples can be found on www.orocos.org/kdl/examples,
>
> for instance http://www.orocos.org/kdl/examples#comment-1724 might be of
> your interest.
>
> Ruben
>

I already tried ChainIkSolverPos_LMA and ChainIkSolverPos_NR (with
ChainFkSolverPos_recursive and ChainIkSolverVel_pinv) like the example you
pointed out, but I don't manage to get stretching. When specifying an out
of range target I get -3 withe first one and -1 ("Failed to converge") with
the second, am I missing something ? Should I set up a stretching bone of
some sort to achieve this ?

I will try a ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls, thanks
for the advice.

>
> What IK solver should I use ?
>>
>>
>>
>> --
>> Orocos-Users mailing list
>> Orocos-Users [..] ...
>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>
>>
>
>
> --
> Ruben Smits, CTO
> +32 479 511 786
> Intermodalics - Kapeldreef 60, 3001 Heverlee - BELGIUM
> www.intermodalics.eu
>
>
> ---------------------------------------------------------------------------------------------------------------------------------------
> This email and any attached files are confidential and may be legally
> privileged. Any copy, print or forward of this email, without the agreement
> of sender or addressee, is strictly prohibited. Misuse is a violation of
> the law on personal data protection (D. Lgs. 196/2003) and on secrecy of
> correspondence (art. 616 cp). If you have received this transmission in
> error please notify the sender immediately and then delete this email and
> any attached files.
>

Ruben Smits's picture

new user

On Thu, Sep 18, 2014 at 12:03 PM, Charles Prévot <prevot [..] ...> wrote:

>
>
> 2014-09-18 11:50 GMT+02:00 Ruben Smits <ruben [dot] smits [..] ...>:
>
>>
>>
>> On Tue, Sep 16, 2014 at 6:28 PM, Charles Prévot <prevot [..] ...>
>> wrote:
>>
>>> Hello,
>>>
>>> I'm writing a virtual reality application and I need to move the avatar
>>> skeleton using various sensor devices (kinect and razer hydra). I was
>>> trying to use orocos_kdl for inverse kinematics but I'm a bit lost...
>>> Basically what I need is to position the arms chains using a target
>>> position. That target position might be out of reach, in this case I want
>>> the arm to point towards the target (similar to what is done in blender for
>>> example). If possible I'd like to give a hint for the elbow position. And
>>> that needs to be real-time to follow the user movements.
>>>
>>>
>> I would propose to use either
>>
>> * the ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls underneath with
>> a lambda != 0 to make sure the stretching out motion gets damped or
>> * the ChainIKSolverPos_LMA (I have no idea myself how this one behaves if
>> the structure gets stretched out)
>>
>> More examples can be found on www.orocos.org/kdl/examples,
>>
>> for instance http://www.orocos.org/kdl/examples#comment-1724 might be of
>> your interest.
>>
>> Ruben
>>
>
> I already tried ChainIkSolverPos_LMA and ChainIkSolverPos_NR (with
> ChainFkSolverPos_recursive and ChainIkSolverVel_pinv) like the example you
> pointed out, but I don't manage to get stretching. When specifying an out
> of range target I get -3 withe first one and -1 ("Failed to converge") with
> the second, am I missing something ? Should I set up a stretching bone of
> some sort to achieve this ?
>
>
The failed to converge is expected (also the -3 is the resulted of the
failed to converge) , since the target is out of reach, did you check the
resulting jointvalues to see if you got the desired stretching behaviour
(Especially the LMA output, the NR output could be random since no damping
is introduced)?

> I will try a ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls, thanks
> for the advice.
>

Please report if you were successful,

Ruben

>
>
>
>>
>> What IK solver should I use ?
>>>
>>>
>>>
>>> --
>>> Orocos-Users mailing list
>>> Orocos-Users [..] ...
>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>
>>>
>>
>>
>> --
>> Ruben Smits, CTO
>> +32 479 511 786
>> Intermodalics - Kapeldreef 60, 3001 Heverlee - BELGIUM
>> www.intermodalics.eu
>>
>>
>> ---------------------------------------------------------------------------------------------------------------------------------------
>> This email and any attached files are confidential and may be legally
>> privileged. Any copy, print or forward of this email, without the agreement
>> of sender or addressee, is strictly prohibited. Misuse is a violation of
>> the law on personal data protection (D. Lgs. 196/2003) and on secrecy of
>> correspondence (art. 616 cp). If you have received this transmission in
>> error please notify the sender immediately and then delete this email and
>> any attached files.
>>
>
>

new user

2014-09-18 13:22 GMT+02:00 Ruben Smits <ruben [dot] smits [..] ...>:

>
>
> On Thu, Sep 18, 2014 at 12:03 PM, Charles Prévot <prevot [..] ...>
> wrote:
>
>>
>>
>> 2014-09-18 11:50 GMT+02:00 Ruben Smits <ruben [dot] smits [..] ...>:
>>
>>>
>>>
>>> On Tue, Sep 16, 2014 at 6:28 PM, Charles Prévot <prevot [..] ...>
>>> wrote:
>>>
>>>> Hello,
>>>>
>>>> I'm writing a virtual reality application and I need to move the avatar
>>>> skeleton using various sensor devices (kinect and razer hydra). I was
>>>> trying to use orocos_kdl for inverse kinematics but I'm a bit lost...
>>>> Basically what I need is to position the arms chains using a target
>>>> position. That target position might be out of reach, in this case I want
>>>> the arm to point towards the target (similar to what is done in blender for
>>>> example). If possible I'd like to give a hint for the elbow position. And
>>>> that needs to be real-time to follow the user movements.
>>>>
>>>>
>>> I would propose to use either
>>>
>>> * the ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls underneath
>>> with a lambda != 0 to make sure the stretching out motion gets damped or
>>> * the ChainIKSolverPos_LMA (I have no idea myself how this one behaves
>>> if the structure gets stretched out)
>>>
>>> More examples can be found on www.orocos.org/kdl/examples,
>>>
>>> for instance http://www.orocos.org/kdl/examples#comment-1724 might be
>>> of your interest.
>>>
>>> Ruben
>>>
>>
>> I already tried ChainIkSolverPos_LMA and ChainIkSolverPos_NR (with
>> ChainFkSolverPos_recursive and ChainIkSolverVel_pinv) like the example you
>> pointed out, but I don't manage to get stretching. When specifying an out
>> of range target I get -3 withe first one and -1 ("Failed to converge") with
>> the second, am I missing something ? Should I set up a stretching bone of
>> some sort to achieve this ?
>>
>>
> The failed to converge is expected (also the -3 is the resulted of the
> failed to converge) , since the target is out of reach, did you check the
> resulting jointvalues to see if you got the desired stretching behaviour
> (Especially the LMA output, the NR output could be random since no damping
> is introduced)?
>

You're right, it works with LMA (I don't why I thought the values were
strange the first time I tried). With NR it seems random, and with NR_JL
everything is at minimum, but as I don't know the theory behind I might
have done something wrong...

How am I supposed to "give hints" on the elbow position ? should I tune the
initial JntArray, or is there a way to express constraints ?

>
>
>> I will try a ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls, thanks
>> for the advice.
>>
>
> Please report if you were successful,
>
> Ruben
>
>
>>
>>
>>
>>>
>>> What IK solver should I use ?
>>>>
>>>>
>>>>
>>>> --
>>>> Orocos-Users mailing list
>>>> Orocos-Users [..] ...
>>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>>
>>>>
>>>
>>>
>>> --
>>> Ruben Smits, CTO
>>> +32 479 511 786
>>> Intermodalics - Kapeldreef 60, 3001 Heverlee - BELGIUM
>>> www.intermodalics.eu
>>>
>>>
>>> ---------------------------------------------------------------------------------------------------------------------------------------
>>> This email and any attached files are confidential and may be legally
>>> privileged. Any copy, print or forward of this email, without the agreement
>>> of sender or addressee, is strictly prohibited. Misuse is a violation of
>>> the law on personal data protection (D. Lgs. 196/2003) and on secrecy of
>>> correspondence (art. 616 cp). If you have received this transmission in
>>> error please notify the sender immediately and then delete this email and
>>> any attached files.
>>>
>>
>>
>
>
> --
> Ruben Smits, CTO
> +32 479 511 786
> Intermodalics - Kapeldreef 60, 3001 Heverlee - BELGIUM
> www.intermodalics.eu
>
>
> ---------------------------------------------------------------------------------------------------------------------------------------
> This email and any attached files are confidential and may be legally
> privileged. Any copy, print or forward of this email, without the agreement
> of sender or addressee, is strictly prohibited. Misuse is a violation of
> the law on personal data protection (D. Lgs. 196/2003) and on secrecy of
> correspondence (art. 616 cp). If you have received this transmission in
> error please notify the sender immediately and then delete this email and
> any attached files.
>

In case it can be of any use to someone here is my small example:
#include <chain.hp

#include <chainfksolver.hp

#include <chainfksolverpos_recursive.hp

#include <chainiksolverpos_lma.hp

#include <chainiksolvervel_pinv.hp

#include <chainiksolverpos_nr.hp

#include <chainiksolverpos_nr_jl.hp

#include <chainiksolvervel_wdls.hp

#include <frames_io.hp

#include <stdio.h>
#include <iostream>

using namespace KDL;

int main( int argc, char** argv )
{
KDL::Chain chain;

chain.addSegment(Segment(Joint(Joint::RotZ)));
chain.addSegment(Segment(Joint(Joint::RotY)));

chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1.0))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
chain.addSegment(Segment(Joint(Joint::RotY)));

chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,1.0,0.0))));
//add Sphere to discard target orientation
chain.addSegment(Segment(Joint(Joint::RotZ)));
chain.addSegment(Segment(Joint(Joint::RotY)));
chain.addSegment(Segment(Joint(Joint::RotX)));
//Creation of jntarrays:
JntArray q(chain.getNrOfJoints());
JntArray q_init(chain.getNrOfJoints());

// Create solver based on kinematic chain
ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
#define SOLVER 2
#if SOLVER==0
ChainIkSolverVel_pinv iksolver1v(chain);
ChainIkSolverPos_NR solver(chain,fksolver,iksolver1v,100,1e-6);
#else
# if SOLVER==1
ChainIkSolverPos_LMA solver(chain);
# else
JntArray q_min(chain.getNrOfJoints());
for(size_t i=chain.getNrOfJoints(); i--; )
q_min.data[i]=-M_PI;
JntArray q_max(chain.getNrOfJoints());
for(size_t i=chain.getNrOfJoints(); i--; )
q_max.data[i]=M_PI;
ChainIkSolverVel_wdls iksolver1v(chain);
iksolver1v.setLambda(0.5);
ChainIkSolverPos_NR_JL solver(chain, q_max, q_min, fksolver,
iksolver1v);
# endif
#endif

Frame f(Rotation::Identity(), Vector(0.0,1.0,2.0));

std::cout << f << std::endl;
int ret = solver.CartToJnt(q_init,f,q);
std::cout << "result=" << solver.strError(ret) <<" "<<ret<< std::endl
<< q.data << std::endl;

ret = fksolver.JntToCart(q,f);
std::cout << f << std::endl;
ret = solver.CartToJnt(q_init,f,q);
std::cout << "result=" << solver.strError(ret) <<" "< << q.data << std::endl;

}
>

Ruben Smits's picture

new user

>From the API documentation (
http://docs.ros.org/indigo/api/orocos_kdl/html/classKDL_1_1ChainIkSolver...
):

Detailed Description

Solver for the inverse position kinematics that uses Levenberg-Marquardt.

The robustness and speed of this solver is improved in several ways:

by using a Levenberg-Marquardt method that automatically adapts the damping
when computing the inverse damped least squares inverse velocity kinematics.
by using an internal implementation of forward position kinematics and the
Jacobian kinematics. This implementation is more numerically robust, is
able to cache previous computations, and implements an $ \mathcal{O}(N) $
algorithm for the computation of the Jacobian (with $N$, the number of
joints, and for a fixed size task space).
by providing a way to specify the weights in task space, you can weigh
rotations wrt translations. This is important e.g. to specify that
rotations do not matter for the problem at hand, or to specify how
important you judge rotations w.r.t. translations, typically in S.I.-units,
([m],[rad]), the rotations are over-specified, this can be avoided using
the weight matrix. Weights also make the solver more robust .
only the constructors call memory allocation.
De general principles behind the optimisation is inspired on: Jorge
Nocedal, Stephen J. Wright, Numerical Optimization,Springer-Verlag New
York, 1999.

int KDL::ChainIkSolverPos_LMA::CartToJnt ( const KDL::JntArray & q_init,
const KDL::Frame & T_base_goal,
KDL::JntArray & q_out
) [virtual]
computes the inverse position kinematics.

Parameters:
q_init initial joint position.
T_base_goal goal position expressed with respect to the robot base.
q_out joint position that achieves the specified goal position (if
successful).
Returns:
0 if successful, -1 the gradient of $ E $ towards the joints is to small,
-2 if joint position increments are to small, -3 if number of iterations is
exceeded.

Ruben

On Thu, Sep 18, 2014 at 4:59 PM, Charles Prévot <prevot [..] ...> wrote:

>
>
> 2014-09-18 13:22 GMT+02:00 Ruben Smits <ruben [dot] smits [..] ...>:
>
>>
>>
>> On Thu, Sep 18, 2014 at 12:03 PM, Charles Prévot <prevot [..] ...>
>> wrote:
>>
>>>
>>>
>>> 2014-09-18 11:50 GMT+02:00 Ruben Smits <ruben [dot] smits [..] ...>:
>>>
>>>>
>>>>
>>>> On Tue, Sep 16, 2014 at 6:28 PM, Charles Prévot <prevot [..] ...>
>>>> wrote:
>>>>
>>>>> Hello,
>>>>>
>>>>> I'm writing a virtual reality application and I need to move the
>>>>> avatar skeleton using various sensor devices (kinect and razer hydra). I
>>>>> was trying to use orocos_kdl for inverse kinematics but I'm a bit lost...
>>>>> Basically what I need is to position the arms chains using a target
>>>>> position. That target position might be out of reach, in this case I want
>>>>> the arm to point towards the target (similar to what is done in blender for
>>>>> example). If possible I'd like to give a hint for the elbow position. And
>>>>> that needs to be real-time to follow the user movements.
>>>>>
>>>>>
>>>> I would propose to use either
>>>>
>>>> * the ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls underneath
>>>> with a lambda != 0 to make sure the stretching out motion gets damped or
>>>> * the ChainIKSolverPos_LMA (I have no idea myself how this one behaves
>>>> if the structure gets stretched out)
>>>>
>>>> More examples can be found on www.orocos.org/kdl/examples,
>>>>
>>>> for instance http://www.orocos.org/kdl/examples#comment-1724 might be
>>>> of your interest.
>>>>
>>>> Ruben
>>>>
>>>
>>> I already tried ChainIkSolverPos_LMA and ChainIkSolverPos_NR (with
>>> ChainFkSolverPos_recursive and ChainIkSolverVel_pinv) like the example you
>>> pointed out, but I don't manage to get stretching. When specifying an out
>>> of range target I get -3 withe first one and -1 ("Failed to converge") with
>>> the second, am I missing something ? Should I set up a stretching bone of
>>> some sort to achieve this ?
>>>
>>>
>> The failed to converge is expected (also the -3 is the resulted of the
>> failed to converge) , since the target is out of reach, did you check the
>> resulting jointvalues to see if you got the desired stretching behaviour
>> (Especially the LMA output, the NR output could be random since no damping
>> is introduced)?
>>
>
> You're right, it works with LMA (I don't why I thought the values were
> strange the first time I tried). With NR it seems random, and with NR_JL
> everything is at minimum, but as I don't know the theory behind I might
> have done something wrong...
>
> How am I supposed to "give hints" on the elbow position ? should I tune
> the initial JntArray, or is there a way to express constraints ?
>
>
>>
>>
>>> I will try a ChainIKSolverPos_NR_JL with a ChainIkSolverVel_wdls, thanks
>>> for the advice.
>>>
>>
>> Please report if you were successful,
>>
>> Ruben
>>
>>
>>>
>>>
>>>
>>>>
>>>> What IK solver should I use ?
>>>>>
>>>>>
>>>>>
>>>>> --
>>>>> Orocos-Users mailing list
>>>>> Orocos-Users [..] ...
>>>>> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>>>>>
>>>>>
>>>>
>>>>
>>>> --
>>>> Ruben Smits, CTO
>>>> +32 479 511 786
>>>> Intermodalics - Kapeldreef 60, 3001 Heverlee - BELGIUM
>>>> www.intermodalics.eu
>>>>
>>>>
>>>> ---------------------------------------------------------------------------------------------------------------------------------------
>>>> This email and any attached files are confidential and may be legally
>>>> privileged. Any copy, print or forward of this email, without the agreement
>>>> of sender or addressee, is strictly prohibited. Misuse is a violation of
>>>> the law on personal data protection (D. Lgs. 196/2003) and on secrecy of
>>>> correspondence (art. 616 cp). If you have received this transmission in
>>>> error please notify the sender immediately and then delete this email and
>>>> any attached files.
>>>>
>>>
>>>
>>
>>
>> --
>> Ruben Smits, CTO
>> +32 479 511 786
>> Intermodalics - Kapeldreef 60, 3001 Heverlee - BELGIUM
>> www.intermodalics.eu
>>
>>
>> ---------------------------------------------------------------------------------------------------------------------------------------
>> This email and any attached files are confidential and may be legally
>> privileged. Any copy, print or forward of this email, without the agreement
>> of sender or addressee, is strictly prohibited. Misuse is a violation of
>> the law on personal data protection (D. Lgs. 196/2003) and on secrecy of
>> correspondence (art. 616 cp). If you have received this transmission in
>> error please notify the sender immediately and then delete this email and
>> any attached files.
>>
>
> In case it can be of any use to someone here is my small example:
> #include <chain.hp

> #include <chainfksolver.hp

> #include <chainfksolverpos_recursive.hp

> #include <chainiksolverpos_lma.hp

> #include <chainiksolvervel_pinv.hp

> #include <chainiksolverpos_nr.hp

> #include <chainiksolverpos_nr_jl.hp

> #include <chainiksolvervel_wdls.hp

> #include <frames_io.hp

> #include <stdio.h>
> #include <iostream>
>
> using namespace KDL;
>
>
> int main( int argc, char** argv )
> {
> KDL::Chain chain;
>
> chain.addSegment(Segment(Joint(Joint::RotZ)));
> chain.addSegment(Segment(Joint(Joint::RotY)));
>
> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1.0))));
> chain.addSegment(Segment(Joint(Joint::RotZ)));
> chain.addSegment(Segment(Joint(Joint::RotY)));
>
> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,1.0,0.0))));
> //add Sphere to discard target orientation
> chain.addSegment(Segment(Joint(Joint::RotZ)));
> chain.addSegment(Segment(Joint(Joint::RotY)));
> chain.addSegment(Segment(Joint(Joint::RotX)));
> //Creation of jntarrays:
> JntArray q(chain.getNrOfJoints());
> JntArray q_init(chain.getNrOfJoints());
>
> // Create solver based on kinematic chain
> ChainFkSolverPos_recursive fksolver =
> ChainFkSolverPos_recursive(chain);
> #define SOLVER 2
> #if SOLVER==0
> ChainIkSolverVel_pinv iksolver1v(chain);
> ChainIkSolverPos_NR solver(chain,fksolver,iksolver1v,100,1e-6);
> #else
> # if SOLVER==1
> ChainIkSolverPos_LMA solver(chain);
> # else
> JntArray q_min(chain.getNrOfJoints());
> for(size_t i=chain.getNrOfJoints(); i--; )
> q_min.data[i]=-M_PI;
> JntArray q_max(chain.getNrOfJoints());
> for(size_t i=chain.getNrOfJoints(); i--; )
> q_max.data[i]=M_PI;
> ChainIkSolverVel_wdls iksolver1v(chain);
> iksolver1v.setLambda(0.5);
> ChainIkSolverPos_NR_JL solver(chain, q_max, q_min, fksolver,
> iksolver1v);
> # endif
> #endif
>
> Frame f(Rotation::Identity(), Vector(0.0,1.0,2.0));
>
> std::cout << f << std::endl;
> int ret = solver.CartToJnt(q_init,f,q);
> std::cout << "result=" << solver.strError(ret) <<" "<<ret<< std::endl
> << q.data << std::endl;
>
> ret = fksolver.JntToCart(q,f);
> std::cout << f << std::endl;
> ret = solver.CartToJnt(q_init,f,q);
> std::cout << "result=" << solver.strError(ret) <<" "<<ret<< std::endl
> << q.data << std::endl;
>
> }
>