Fix calculation of frame offset

[Naoki-Hiraoka]

Hello. I am Naoki Hiraoka, robot_calibration user.

This pull request fixes the method of applying frame offset in updateURDF and getChainFK.

In the previous code, frames are calculated as following

In updateURDF
[updated frame].x = [original frame].x + [offset frame].x
[updated frame].y = [original frame].y + [offset frame].y
[updated frame].z = [original frame].z + [offset frame].z
[updated frame].roll = [original frame].roll + [offset frame].roll
[updated frame].pitch = [original frame].pitch + [offset frame].pitch
[updated frame].yaw = [original frame].yaw + [offset frame].yaw

In getChainFK
[updated frame] = [offset frame] * [original frame]

There are two problems.
In updateURDF, since rotation is not integrable, output URDF generated by updateURDF is different from the one used by optimizer with getChainFK.
In getChainFK, since [offset frame] is applied from the left, [original frame].xyz is affected by [offset frame].rpy.

To avoid them, in this pull request, frames are calculated as

[updated frame] = [original frame] * [offset frame]

in both updateURDF and getChainFK.

[mikeferguson]

Ok, this one is going to take me a bit to look over more closely.

[mikeferguson]

This has been tested -- I'm pretty sure it breaks some tests, but running CI anyways and then I'll fix up tests later. Thanks!