Bug Forward kinematics results don't correspond to naoqi getPosition method results

[jupbarrera]

When comparing the nao forward kinematics results of this library and those from naoqi, I noticed a bug in the Z axis value.

The code:

    //Right Hand
    joints[L_ARM+SHOULDER_PITCH]=0;
    joints[L_ARM+SHOULDER_ROLL]=0;
    joints[L_ARM+ELBOW_YAW]=0;
    joints[L_ARM+ELBOW_ROLL]=0;
    joints[L_ARM+WRIST_YAW]=0;
    //Right Hand
    joints[R_ARM+SHOULDER_PITCH]=0;
    joints[R_ARM+SHOULDER_ROLL]=0;
    joints[R_ARM+ELBOW_YAW]=0;
    joints[R_ARM+ELBOW_ROLL]=0;
    joints[R_ARM+WRIST_YAW]=0;

    //Now we can set the joints to the class
    nkin.setJoints(joints);

    output1= nkin.getForwardEffector((NAOKinematics::Effectors)CHAIN_L_ARM);

    //Right Hand
    output2 = nkin.getForwardEffector((NAOKinematics::Effectors)CHAIN_R_ARM);

    std::cout << "x = " << output1(0,3) << " y = " << output1(1,3) << " z = " << output1(2,3) <<  std::endl;
    std::cout << "x = " << output2(0,3) << " y = " << output2(1,3) << " z = " << output2(2,3) <<  std::endl;

The output:

x = 218.7 y = 113 z = 112.31 x = 218.7 y = -113 z = 112.31

Naoqi output for the Left arm in the same configuration in meters:

x = 0.21852196753025055, y = 0.11063870042562485, z = 0.08774302154779434,

The bug correspons to the HandOffsetZ constant when used to calculate the constant translation matrix. According to the aldebaran documentation HandOffsetZ is negative in the z axis.

[eldr4d]

Merged and closed, thanks for your correction