Failed to use inverse kinematics service

[ashuRMS]

Description

I am using Kinova Gen3 lite arm. I have the latest firmwares as well. I have written code to solve the inverse kinematics, that will use the /gen3_lite/base/compute_inverse_kinematics service. I am getting the following error.

ashutoshsahu@pop-os:~/catkin_kinova$ rosrun kortex_examples inverse_kinematics.py 
Service call failed: service [/gen3_lite/base/compute_inverse_kinematics] responded with an error: b''
Inverse kinematics computed: None

[ INFO] [1707737253.276800009]: Kortex exception
[ INFO] [1707737253.276841243]: KINOVA exception error code: 3

[ INFO] [1707737253.276855913]: KINOVA exception error sub code: 1

[ INFO] [1707737253.276872062]: KINOVA exception description: Device error, Error sub type=METHOD_FAILED => <srv: 2, fct: 234, msgType: 3>
description: IK solution found but it is outside of joint position limits - Try to change initial guess to find a valid solution

I tried with different guess values but still the service was not able to solve inverse kinematics even for simple goal positions like the home position

Version

ROS distribution : I am using ROS Noetic

Branch and commit you are using : https://github.com/Kinovarobotics/ros_kortex/tree/noetic-devel

Steps to reproduce

 $ roslaunch kortex_driver kortex_driver.launch ip_address:=192.168.2.10 start_rviz:=false robot_name:=gen3_lite
 $ ashutoshsahu@pop-os:~/catkin_kinova$ rosrun kortex_examples inverse_kinematics.py 

Code example (if necessary)

#!/usr/bin/env python3
import numpy as np
import rospy
from kortex_driver.msg import Pose, IKData, JointAngles, JointAngle
from kortex_driver.srv import ComputeInverseKinematics, ComputeInverseKinematicsRequest

def call_inverse_kinematics_service():
    rospy.wait_for_service('/gen3_lite/base/compute_inverse_kinematics')
    try:
        inverse_kinematics = rospy.ServiceProxy('/gen3_lite/base/compute_inverse_kinematics', ComputeInverseKinematics)
        # Create a request object
        request = ComputeInverseKinematicsRequest()

        # The pose
        end_effec_pose = Pose()
        end_effec_pose.x = 0.439
        end_effec_pose.y = 0.193
        end_effec_pose.z = 0.448
        end_effec_pose.theta_x = 90.7 * (np.pi/180)
        end_effec_pose.theta_y = -1 * (np.pi/180)
        end_effec_pose.theta_z = 150 * (np.pi/180)

        # The angles
        jangle1 = JointAngle()
        jangle1.joint_identifier = 0
        jangle1.value = 0.2 # Adjusted initial guess within joint limits

        jangle2 = JointAngle()
        jangle2.joint_identifier = 1
        jangle2.value = 0.0  # Adjusted initial guess within joint limits

        jangle3 = JointAngle()
        jangle3.joint_identifier = 2
        jangle3.value = -0.3 # Adjusted initial guess within joint limits

        jangle4 = JointAngle()
        jangle4.joint_identifier = 3
        jangle4.value = 1.0  # Adjusted initial guess within joint limits

        jangle5 = JointAngle()
        jangle5.joint_identifier = 4
        jangle5.value = 1.14  # Adjusted initial guess within joint limits

        jangle6 = JointAngle()
        jangle6.joint_identifier = 5
        jangle6.value = 1.67  # Adjusted initial guess within joint limits

        jo_angles = [jangle1, jangle2, jangle3, jangle4, jangle5, jangle6]

        request.input.cartesian_pose = end_effec_pose
        request.input.guess.joint_angles = jo_angles

        # Call the service
        response = inverse_kinematics(request)
        return response
    except rospy.ServiceException as e:
        print("Service call failed:", e)

if __name__ == "__main__":
    rospy.init_node('inverse_kinematics_client')
    response = call_inverse_kinematics_service()
    print("Inverse kinematics computed:", response)

# This is not working

Expected behavior

It should return the joint angles for the given end effector pose

Any other information

Any other information you believe the maintainers need to know.

[martinleroux]

The IK function from the API calls the same numerical algorithm as the robot does during runtime. The algorithm does not look for a final solution right away but instead, every millisecond (or as fast as possible when not used while the robot is moving), the robot identifies a unit vector in the direction of the final destination and takes a single step along that unit vector such that:

dq = pinv(J)*dr

with dq = displacement of the joint J = jacobian dr = a step that the robot expects to be able to make in 1 ms.

While this algorithm is very flexible with the type of geometry it can support, it has the downside of being very sensitive to initial guess values because it cannot know whether or not it can reach the destination before it either gets there or runs into an issue.

Since you are hitting a joint limit, this means that the algorithm is virtually moving the robot in a direction which indeed hits a joint limit. Unfortunately, my only suggestion is to try even more guess values.

[ashuRMS]

Thank you @martinleroux I thought that if I specified the guess as the home position, I would be able to get the accurate solution, but it is not working. Anyway, I will look for some other approach.