Fix the zero configuration in the MICO URDF

[mkoval]

We currently add joint offsets (hardcoded_pos_offsets) to all joint angles. This is necessary because the zero configuration reported by the MICO hardware does not match the zero configuration in our URDF file. We should re-export the URDF file to have the correct zero configuration and remove these offsets.

This zero configuration may be in self-collision. If so, we should also verify that the OpenRAVE doesn't incorrectly disable any pairs of links when loading the model in the default configuration.

[psigen]

Relevant functions:

void SetZeroConfiguration()

Sets the joint offsets so that the current configuration becomes the new zero state of the robot. http://openrave.org/docs/latest_stable/coreapihtml/classOpenRAVE_1_1KinBody.html#a6beba633ec468650e450b4e51e90b89a

void SetNonCollidingConfiguration()

Treats the current pose as a pose not in collision, which sets the adjacent pairs of links. http://openrave.org/docs/latest_stable/coreapihtml/classOpenRAVE_1_1KinBody.html#ad4e071ae35acac1594343f1865bcbb4d

On Mon, Mar 23, 2015 at 2:26 PM, Michael Koval notifications@github.com wrote:

We currently add joint offsets (hardcoded_pos_offsets https://github.com/personalrobotics/mico_hardware/blob/master/include/MicoRobot.h#L28) to all joint angles. This is necessary because the zero configuration reported by the MICO hardware does not match the zero configuration in our URDF file. We should re-export the URDF file to have the correct zero configuration and remove these offsets.

This zero configuration may be in self-collision. If so, we should also verify that the OpenRAVE doesn't incorrectly disable any pairs of links when loading the model in the default configuration.

— Reply to this email directly or view it on GitHub https://github.com/personalrobotics/mico_hardware/issues/1.

[mklingen]

I just go some interesting info from @Stefanos19 . At northwestern, the robot had a different zero offset and he changed the hardcoded positions on their software to account for this. Now that I think about it, at Imperial London, their two robots (one older, with old firmware, one newer with new firmware) had different zero offsets from each other as well.

This points to the firmware or some other hardware quirk causes different robots to have different zero offsets.

[mklingen]

Here are the offsets mico_hardware is giving for our robot at home configuration on startup btw:

    wrapping j0 : pos: 7.938681 offset: 3.141593
    wrapping j1 : pos: 4.632502 offset: 1.570796
    wrapping j3 : pos: 7.388693 offset: 3.141593
    wrapping j4 : pos: 4.611239 offset: 3.141593

[mkoval]

I believe the Windows GUI shipped with the MICO has an option for setting the arm's home configuration. This is primarily intended for use with the joystick, which has a "go home" button, but it may introduce joint offsets.

It's possible that we need to set this to a known value. Maybe libkindrv has a function that does this?

@herlant @Stefanos19 Am I remembering correctly? @timn @kb0n Have you run into this problem on your arms?

[mklingen]

I just pushed a new URDF file with manually adjusted joint offsets as mico-modified-manual.urdf. I just went through and re-did the rpy elements of each of the links/joints until they aligned with the real robot. On most occasions, it required a +/- 1/2 PI yaw. The only exception is the wrist and hand, which also need to be translated in the opposite direction (which can be accomplished by rotating the visual/collision models rather than the joint).