UR inverse kinematics solution rotated 180 degrees

[johnwason]

The URInvKin solver is returning results that are 180 degrees rotated about Z compared to the forward kinematics result. I have code that tests KDL, OPW, and UR using the same test. The KDL and OPW work fine, but the UR is incorrect. I've tried fiddling with the parameters, but it seems like there might be an insistency in the solver.

The factory UR parameters have the robot stretched out along -X instead of +X like in ROS.

https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/

For their ROS description, they have a 180 degree rotation hacked in to make it match the +X ROS convention:

https://github.com/UniversalRobots/Universal_Robots_ROS2_Description/blob/c4d1724ac32a4f0e3bcfbcfa420f82608b07b17f/urdf/ur_macro.xacro#L270

I am not using a URDF for my code, so I am not entirely sure if this is a problem with my code or with the solver. The failing test is here:

https://github.com/johnwason/rpi_general_robotics_toolbox_py/blob/a5d80df88e89aa95ba4eabfe11a2894972c73900/test/test_tesseract_kinematics.py#L417

I suggest adding a URDF test that calculates forward and inverse kinematics of the UR robot and cross checks between them.

[johnwason]

I think the solution to this problem is to add offset transforms before and after the base and tip link for the inverse kinematic solvers that are specified in the yaml files. I am having issues with OPWInvKin as well where I need a slightly different link format than the solver expects.

@Levi-Armstrong do you have a problem with this? What format have you been using for transforms in yaml?

[Levi-Armstrong]

My understanding is that it should have a link called "base" where if you queried from base to tcp it should match the robot controller.

[Levi-Armstrong]

In the case of abb it has the following

    <!-- ROS base_link to ABB World Coordinates transform -->
    <link name="${prefix}base" />
    <joint name="${prefix}base_link-base" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <parent link="${prefix}base_link"/>
      <child link="${prefix}base"/>
    </joint>

[gavanderhoorn]

Ah. You didn't format it as base link, so I got confused.

That's here: https://github.com/ros-industrial/universal_robot/blob/addbe3fc2b8f4fd3afc65d2c8f9a5380324e04ba/ur_description/urdf/inc/ur_macro.xacro#L306-L317.

[Levi-Armstrong]

Okay, so I have been using the incorrect xacro file when testing. I will see if I can fix the ik solver so it does not have the 180 degree base rotation.

[gavanderhoorn]

Well the chain would be identical to the one in kinetic-devel. It also has the base link and the rotation.

[Levi-Armstrong]

I am not using a URDF for my code, so I am not entirely sure if this is a problem with my code or with the solver. The failing test is here:

There are test in tesseract for the UR which compare the IK to the Forward so I assume the issue is not within tesseract. Now I also had to add a base offset in the inverse kinematics solver for the UR to have it align with the URDF's within ur_descriptions. If you look in ur_inv_kin.cpp line 262 you will 180deg rotation around Z axis.