I retrieve initial joint values, calculate FK, and then calculate IK to get joint values (J -> FK -> IK -> J). I expect that the output of IK should be equal to the initial joint values. This is a simple check. While it works for Kuka and others, this does not work for 7DOF dual-arm ABB YuMi. Am I missing something obvious? Here is the code:
Code:
from robodk import robolink, robomath
RDK = robolink.Robolink()
robot = RDK.Item("YuMi IRB 14000-0.5/0.5-R")
# robot = RDK.Item("Fanuc LR Mate 200iD/ER-4iA")
# robot = RDK.Item("KUKA LBR iiwa 14 R820")
# get current joint values
joints_values = robot.Joints()
print(joints_values)
# forward kinematics
# Returns the pose of the robot flange with respect to the robot base reference
fk = robot.SolveFK(joints_values)
# inverse kinematics
invk = robot.SolveIK(fk)
print(invk)