petercorke/robotics-toolbox-python

Robot.Models.KinovaGen3( ) Produces invalid solutions with IK solver although many valid solutions exist

s-m-asjad opened this issue · 0 comments

Describe the bug

Whilst using the roboticstoolbox.models.KinovaGen3() with any IK solver, solutions that should exist are also returned as not found.

To Reproduce
Steps to reproduce the behavior

import roboticstoolbox as rtb
import spatialmath

robot = rtb.models.KinovaGen3()
T = spatialmath.SE3([0.4, 0, 0.6])

print("Using GN Method, Attempt 1")
print(robot.ik_GN(T))
print(" ")

print("Using GN Method, Attempt 2")
print(robot.ik_GN(T))
print(" ")

print("Using NR Method, Attempt 1")
print(robot.ik_NR(T))
print(" ")

print("Using NR Method, Attempt 2")
print(robot.ik_NR(T))
print(" ")

Output
All attempts return unsuccessful. We also tried passing the returned joint angles to our robot in RViZ and it did not reach the expected end-effector pose.

Using GN Method, Attempt 1
(array([ 5.30999786e-310, -9.01630577e-001,  5.30999786e-310,
       -1.07499523e+000,  5.30999786e-310, -1.38677629e+000,
        5.30999786e-310]), 0, 1145, 101, 5.6412503361213866e-08)
 
Using GN Method, Attempt 2
(array([5.30999786e-310, 1.79832544e+000, 5.30999786e-310, 1.15716627e+000,
       5.30999786e-310, 6.69991991e-001, 5.30999786e-310]), 0, 1020, 101, 1.0028539789796468e-10)
 
Using NR Method, Attempt 1
(array([ 5.30999786e-310,  1.44941148e+000,  5.30999786e-310,
       -1.70698408e+000,  5.30999786e-310, -2.38067397e-001,
        5.30999786e-310]), 0, 1083, 101, 1.2436712029831807e-07)
 
Using NR Method, Attempt 2
(array([ 5.30999786e-310, -5.89984253e-001,  5.30999786e-310,
       -1.01100790e+000,  5.30999786e-310, -2.18828825e+000,
        5.30999786e-310]), 0, 961, 101, 1.860480014301843e-11)

Expected behavior
The IK solver should be able to return a valid solution for the Kinova Gen 3 arm when a valid solution exists.

Environment:

  • Ubuntu 22.04.
  • Python 3.10.12
  • roboticstoolbox-python 1.1.0

Other Information
We have experimented with other toolboxes, and Newton Raphson returns valid IK solutions for not only the pose in the given example but also for other valid poses. I double checked the DH Parameters in the roboticstoolbox-python for the KinovaGen3 and they seem to match with the URDF of the KinovaGen3 I am using with my other solver. I shall attach a link to a code repository in the comments once I clean-up my code to demonstrate working of the solver (we have verified the working of the solution on RViz and the physical robot).