Confusion about inverse kinematics of Gen3Lite

[DawnQiu]

Dear sir,

I have encountered some issues while writing an inverse kinematics program in the Gen3Lite robot.

First, I ran the 01-compute-kinematics.cpp program, which worked successfully, and printed out the information:

Creating session for communication
Session created
Getting Angles for every joint...
Joint ID : Joint Angle
0 : 0.00445557
1 : 359.96
2 : 87.3231
3 : 359.997
4 : 359.985
5 : 0.00953674

Computing Foward Kinematics using joint angles...
Pose calculated : 
Coordinate (x, y, z)  : (0.482311, -0.0099066, 0.488392)
Theta (theta_x, theta_y, theta_z)  : (87.3632, -0.00609821, 90.0194)

Computing Inverse Kinematics using joint angles and pose...
Joint ID : Joint Angle
0 : -0.047702
1 : -0.0425111
2 : 87.3206
3 : 0.215779
4 : -0.0667446
5 : -0.206791

Then, I refer to the program to write my own "inverse kinematics" program, the following is the source code:

std::vector<float> KortexRobot::ComputeInverseKinematics(vector<float> &TargetPose, vector<float> &GuessAngle)
{
     //Create an IKData object input_IkData to store the data required for inverse kinematics calculation.
    Kinova::Api::Base::IKData input_IkData;

    //Assign the desired Cartesian pose, represented by the TargetPose vector, to the cartesian_pose field of input_IkData.
    input_IkData.mutable_cartesian_pose()->set_x(TargetPose[0]);
    input_IkData.mutable_cartesian_pose()->set_y(TargetPose[1]);
    input_IkData.mutable_cartesian_pose()->set_z(TargetPose[2]);
    input_IkData.mutable_cartesian_pose()->set_theta_x(TargetPose[3]);
    input_IkData.mutable_cartesian_pose()->set_theta_y(TargetPose[4]);
    input_IkData.mutable_cartesian_pose()->set_theta_z(TargetPose[5]);

   //Assign the initial guess angles, represented by the GuessAngle vector
    Kinova::Api::Base::JointAngle* jAngle;
    for (int i = 0; i < 6; ++i)
    {
        jAngle = input_IkData.mutable_guess()->add_joint_angles();
        jAngle->set_value(GuessAngle[i] - 1);
    }

    //Call the ComputeInverseKinematics method of the m_pBaseClient object, passing input_IkData as the parameter, 
    //to perform the inverse kinematics calculation. The computed result will be stored in the computed_joint_angles object.
    Kinova::Api::Base::JointAngles computed_joint_angles;
    try
    {
        computed_joint_angles = m_pBaseClient->ComputeInverseKinematics(input_IkData);
    }
    catch (Kinova::Api::KDetailedException& ex)
    {
        OnError(ex);
    }

    //Create a std::vector<float> named ComputedAngles to store the computed joint angles.
    std::vector<float> ComputedAngles;
    for (int j = 0; j < 6; ++j)
    {
        ComputedAngles.push_back(computed_joint_angles.joint_angles(j).value());
    }
    return ComputedAngles;
}

In the main function, define the desired pose TargetPose and the initial guess angles GuessAngle.
Call robot.ComputeInverseKinematics(TargetPose, GuessAngle) to perform the inverse kinematics calculation and store the result in the ComputedAngles vector.Use a loop to iterate over the ComputedAngles vector and print the value of each joint angle.

std::vector<float> TargetPose = {0.48f, 0.0f, 0.467f, 90.0f, 0.0f, 90.0f};  //X,Y,Z,Rx,Ry,Rz
std::vector<float> GuessAngle = {0.0f, 0.0f, 90.0f, 0.0f, 0.0f, 0.0f};
std::vector<float> ComputedAngles;
ComputedAngles = robot.ComputeInverseKinematics(TargetPose,GuessAngle);
 for (int i = 0; i < robot.GetNbDof(); i++)
{
   std::cout <<"ComputedAngles" << i  << ": " << ComputedAngles[i] << std::endl;
}

Finally, running the program reports the following error message:

KDetailedoption detected what: Device error, Error sub type=METHOD_FAILED => <srv: 2, fct: 234, msgType: 3>
description: The first convergence of the IK has failed
KError error_code: 3
KError sub_code: 1
KError sub_string: The first convergence of the IK has failed
Error code string equivalent: ERROR_DEVICE
[libprotobuf FATAL /home/robot/kortex-QLM/api_cpp/examples/kortex_api/include/google/protobuf/repeated_field.h:1522] CHECK failed: (index) < (current_size_): 
terminate called after throwing an instance of 'google::protobuf::FatalException'
  what():  CHECK failed: (index) < (current_size_): 

Process finished with exit code 134 (interrupted by signal 6: SIGABRT)

This problem has been bothering me for several days, and I would like to know how to solve it.

Thank you very much for your response!

Best Wishes！

[felixmaisonneuve]

Hi @DawnQiu,

Your code looks good, and from the error message, I feel like the error comes from the Inverse Kinematics computation.
Have you tried with other values for your TargetPose and GuessAngle?
I think that might be the issue.

I cannot test on a real Gen3-lite at the moment, but my next steps would be to try with a different TargetPose. If it works and you absolutely need this specific target pose, I would try with different variants of GuessAngle.

I feel like this specific pose is on the far edge of the arm's reach, so there might be an issue when computing where some values are out of bounds and the whole process fails because of it.

Best,
Felix

[DawnQiu]

Thank you for your response.
Regarding the specific target pose you mentioned, it is actually not located at the far edge of the robot arm's reachable workspace. It is approximately the pose achieved when the third joint of the robot arm is rotated to 90 degrees.
I need to perform some control tasks on this target pose, such as motion control and force control. Especially force control, where the robot will generate a new target pose based on external forces acting on the current target pose. Then, I need to convert the new target pose into joint angles using inverse kinematics, and send these angles to the robot to execute the corresponding actions.
Also, do you think there could be an error in the part where IKData is assigned? I am not fully familiar with IKData.
Thank you for your suggestions on this issue, and I look forward to your response.
Best regards

[felixmaisonneuve]

Yeah, you wrote "third joint of the robot arm is rotated to 90 degrees" and it clicked in my head. This is a singularity between joint 4 and 6. They are both on the exact same axis so the inverse kinematics cannot be computed since there are infinite solutions.

See the singularities section in the User Guide for more details.

Basically, you cannot compute inverse kinematics for this specific pose. If you send your arm to this exact cartesian pose, you will probably have small variation on every joint angles to avoid having actuators 4 and 6 on the same axis.

Best,
Felix