problem with IK

Hi all,

I installed yesterday the KDL and I managed to get correct the forward
kinematics of a simple chain. However, I didn't have the same luck
with the IK. I guess I'm getting something wrong but the lines of code
are not so many to actually try to debug more. A problem is that I
can't yet find a working example on the web..
Could it be that I have to give an orientation for the destination frame?

The result that I get for my end-effector is : [0.00754234 0.203459 0.196122]
but I asked for : [0.1, 0.2, 0.1]

So my code is like that:
###############################################################################
Chain d_chain; /* The kinematic chain */

/* Setup the kinematic chain */
d_chain.addSegment(Segment(Joint(Joint::RotY),
Frame(Vector(0.0,0.0,0.0))));
d_chain.addSegment(Segment(Joint(Joint::RotX),
Frame(Vector(0.0,0.0,0.0))));
d_chain.addSegment(Segment(Joint(Joint::RotZ),
Frame(Vector(0.0,0.2,0.0))));
d_chain.addSegment(Segment(Joint(Joint::RotX),
Frame(Vector(0.0,0.2,0.0))));

/* Create solver based on kinematic chain*/
ChainFkSolverPos_recursive d_fksolver(d_chain);
ChainIkSolverVel_pinv d_iksolver_vel(d_chain);
ChainIkSolverPos_NR d_iksolver(d_chain, d_fksolver,
d_iksolver_vel);

JntArray q(d_chain.getNrOfJoints());
JntArray q_init(d_chain.getNrOfJoints());

for (int i = 0; i < 4; ++i)
{
q_init(i) = 0.0;
}

//Set destination frame's position
Frame F_dest(Vector(0.1, 0.2, 0.1));
d_iksolver.CartToJnt(q_init, F_dest, q);

for (int i = 0; i < 4; ++i)
{
cout << q(i) << " / ";
}
################################################################################

Thanks!
Kostas

problem with IK

On Fri, 10 Jun 2011, kostas [dot] karakasiliotis [..] ... wrote:

> Hi all,
>
> I installed yesterday the KDL and I managed to get correct the forward
> kinematics of a simple chain. However, I didn't have the same luck
> with the IK. I guess I'm getting something wrong but the lines of code
> are not so many to actually try to debug more. A problem is that I
> can't yet find a working example on the web..
> Could it be that I have to give an orientation for the destination frame?
>
> The result that I get for my end-effector is : [0.00754234 0.203459 0.196122]
> but I asked for : [0.1, 0.2, 0.1]

It seems your robot has only four joints, so the FK and IK will _in
general_ not correspond! Robot Kinematics 101: the general IK solver weighs
in one way or another the contribution of the four joints to try to reach a
6DOF Cartesian frame.

>
> So my code is like that:
> ###############################################################################
> Chain d_chain; /* The kinematic chain */
>
> /* Setup the kinematic chain */
> d_chain.addSegment(Segment(Joint(Joint::RotY),
> Frame(Vector(0.0,0.0,0.0))));
> d_chain.addSegment(Segment(Joint(Joint::RotX),
> Frame(Vector(0.0,0.0,0.0))));
> d_chain.addSegment(Segment(Joint(Joint::RotZ),
> Frame(Vector(0.0,0.2,0.0))));
> d_chain.addSegment(Segment(Joint(Joint::RotX),
> Frame(Vector(0.0,0.2,0.0))));
>
> /* Create solver based on kinematic chain*/
> ChainFkSolverPos_recursive d_fksolver(d_chain);
> ChainIkSolverVel_pinv d_iksolver_vel(d_chain);
> ChainIkSolverPos_NR d_iksolver(d_chain, d_fksolver,
> d_iksolver_vel);
>
> JntArray q(d_chain.getNrOfJoints());
> JntArray q_init(d_chain.getNrOfJoints());
>
> for (int i = 0; i < 4; ++i)
> {
> q_init(i) = 0.0;
> }
>
> //Set destination frame's position
> Frame F_dest(Vector(0.1, 0.2, 0.1));
> d_iksolver.CartToJnt(q_init, F_dest, q);
>
> for (int i = 0; i < 4; ++i)
> {
> cout << q(i) << " / ";
> }
> ################################################################################
>
> Thanks!
> Kostas

Herman

problem with IK

Quoting Herman Bruyninckx <Herman [dot] Bruyninckx [..] ...>:

> On Fri, 10 Jun 2011, kostas [dot] karakasiliotis [..] ... wrote:
>
>> Hi all,
>>
>> I installed yesterday the KDL and I managed to get correct the forward
>> kinematics of a simple chain. However, I didn't have the same luck
>> with the IK. I guess I'm getting something wrong but the lines of code
>> are not so many to actually try to debug more. A problem is that I
>> can't yet find a working example on the web..
>> Could it be that I have to give an orientation for the destination frame?
>>
>> The result that I get for my end-effector is : [0.00754234 0.203459
>> 0.196122]
>> but I asked for : [0.1, 0.2, 0.1]
>
> It seems your robot has only four joints, so the FK and IK will _in
> general_ not correspond! Robot Kinematics 101: the general IK solver weighs
> in one way or another the contribution of the four joints to try to reach a
> 6DOF Cartesian frame.
>

OK, so indeed I should specify the orientation of the end-effector.
I did a small test for that. I asked the solver to move (with small
steps and by updating the q_init with the last solution q) the
end-effector at a straight line on the y-axis with x and z at 0.0. The
result was a movement on the direction I wanted but not respecting the
specified line as it was trying to keep the initial orientation.
I don't know if people have already solved this 4-dof problem which
follows a specified trajectory in KDL (sounds easy a-priory) but I
guess I should try to use the FK with my own IK solver.

Thanks for the nice work anyway!
Kostas

>>
>> So my code is like that:
>> ###############################################################################
>> Chain d_chain; /* The kinematic chain */
>>
>> /* Setup the kinematic chain */
>> d_chain.addSegment(Segment(Joint(Joint::RotY),
>> Frame(Vector(0.0,0.0,0.0))));
>> d_chain.addSegment(Segment(Joint(Joint::RotX),
>> Frame(Vector(0.0,0.0,0.0))));
>> d_chain.addSegment(Segment(Joint(Joint::RotZ),
>> Frame(Vector(0.0,0.2,0.0))));
>> d_chain.addSegment(Segment(Joint(Joint::RotX),
>> Frame(Vector(0.0,0.2,0.0))));
>>
>> /* Create solver based on kinematic chain*/
>> ChainFkSolverPos_recursive d_fksolver(d_chain);
>> ChainIkSolverVel_pinv d_iksolver_vel(d_chain);
>> ChainIkSolverPos_NR d_iksolver(d_chain, d_fksolver,
>> d_iksolver_vel);
>>
>> JntArray q(d_chain.getNrOfJoints());
>> JntArray q_init(d_chain.getNrOfJoints());
>>
>> for (int i = 0; i < 4; ++i)
>> {
>> q_init(i) = 0.0;
>> }
>>
>> //Set destination frame's position
>> Frame F_dest(Vector(0.1, 0.2, 0.1));
>> d_iksolver.CartToJnt(q_init, F_dest, q);
>>
>> for (int i = 0; i < 4; ++i)
>> {
>> cout << q(i) << " / ";
>> }
>> ################################################################################
>>
>> Thanks!
>> Kostas
>
> Herman
>