Consider making xacros REP-103 compliant (coordinate systems)

[gavanderhoorn]

The current xacro macros (in duaro_description and rs_description) appear to follow the KHI convention of X+ to the right, Y+ forward, and Z+ up.

REP-103 (Standard Units of Measure and Coordinate Conventions) specifies a convention of: X+ forward, Y+ left, Z+ up (section: Coordinate Frame Conventions - Axis Orientation).

Adhering to this convention would make it easier to combine KHI robot models with other models in ROS, and in addition would make it easier to integrate them with existing ROS applications (that expect REP-103 compliant robots).

The suggestion in this ticket is to make the KHI xacro macros REP-103 compliant.

[gavanderhoorn]

Additional input: within ROS-Industrial we're preparing to submit a new REP to standardise robot model joint and link names, as well as coordinate conventions for use of ROS in industrial contexts.

The working title is Coordinate Frames for Serial Industrial Manipulators.

In this document, the section Coordinate Frames suggests to "map" coordinate systems from OEMs to ROS coordinate conventions by the introduction of so called base and tool0 frames. Those frames do not have to follow REP-103, but could be transformed to coincide with the coordinate systems of an OEM.

For KHI, this would mean that base (as a child of base_link) would be rotated by +90 degrees over the Z axis, such that it corresponds to the KHI Base coordinate system, and tool0 (as a child of flange, via link6) would also be rotated +90 degrees over the Z axis, such that it corresponds to all-zeros KHI Tool coordinates.

This way, the transform base->tool0 should correspond to what the KHI controller reports, while the robot model itself is still REP-103 compliant (ie: has X+ forward). Users would be able to relate Cartesian coordinates in ROS applications to KHI coordinates on the controller by using the base_link->base transformation.

---

Note: the new REP has not yet been ratified, so it is not a standard right now. It's entirely up to you whether you feel it makes sense to follow its advice. I just wanted to provide you with more information and an option to allow for transformations between KHI and ROS coordinate system conventions.

Adding the additional base frame would also address the issue with the difference between coordinate frame origins described in the readme. base would be located where it should be (ie: "Origin of base (KHI coordinate) is Robot Link1 origin.").

[matsui-hiro]

Thank you for explaining REP-103,REP-199. I worried about the difference ROS and KHI cordinate. So I want to follow Coordinate Frames for Serial Industrial Manipulators.

[gavanderhoorn]

Just a note: REP-199 is not the final number for this REP. It's just a place-holder.

[gavanderhoorn]

@matsui-hiro: I can start adding the base, flange and tool0 frames to the macros.

Will you take care of rotating the rest of the robot?

[matsui-hiro]

I was late for response.

Will you take care of rotating the rest of the robot?

In order to coexist with other robots of ROS, I think that it can not be helped to rotate the robot. I want to know more information and an option to allow for transformations between KHI and ROS coordinate system conventions.

[gavanderhoorn]

I want to know more information

Can you clarify what sort of information you are looking for?

and an option to allow for transformations between KHI and ROS coordinate system conventions.

I may not have been clear about this, but that is exactly what the base and tool0 frames are for.

base and tool0 would be KHI aligned. base_link and all the other links would be ROS aligned.

[matsui-hiro]

that is exactly what the base and tool0 frames are for.

I see.

Can you clarify what sort of information you are looking for?

I want to know the way for planning with the same pose as now, after adding the change(rotating the robots),

ex) /khi_rs007n_moveit_config/script/rs007n_demo.py

    group = MoveGroupCommander("manipulator")
    exec_vel = 0.5

    rospy.loginfo("pose1 start")
    group.set_max_velocity_scaling_factor(exec_vel)
    group.set_pose_target([0.5,-0.2,0.2,0.0,1.0,0.0])
    group.go()
    rospy.loginfo("pose1 end")

Can I plan by the coordinate that have base as origin and tool0 as end_effector? Should I change the element of manipulator group?

[gavanderhoorn]

MoveIt will always plan in a ROS-aligned coordinate system (ie: REP-103 compliant).

If you rotate your robot such that the robot's "forward" side is aligned with Y+ (as is the case now with all models in khi_rs_description and the Duaro), all poses "in front of the robot" will just have a Y+ coordinate (where they would otherwise have a similar X+ coordinate).

What changes with the introduction of base and tool0 would be that instead of specifying pose targets for MoveIt relative to base_link, you'd give target poses relative to base. Internally, MoveIt should first transform those poses to the base_link frame. That transformation will account for the 90 degree rotation between REP-103 and the KHI Base frame.

In the end the robot will go to the correct pose, and the pose expressed relative to base should correspond to what the KHI teach pendant shows. The ROS transform base_link->tool0 will not show the same values (but that would be expected: X and Y are interchanged).

MoveGroupCommander will -- by default -- use the EEF configured for the group that it wraps (or the last link if there is no EEF). So if the group runs from base->tool0, it should do the right thing if you call set_pose_target.

This last bit would have to be verified though. It's been a while since I've used MoveGroupCommander.

[matsui-hiro]

instead of specifying pose targets for MoveIt relative to base_link, you'd give target poses relative to base. Internally, MoveIt should first transform those poses to the base_link frame.

So if the group runs from base->tool0, it should do the right thing if you call set_pose_target.

All-zeros TCP on KHI controller is coincident with origin of link6 frame. So, to call set_pose_target with same pose values after introduction of base(rotated by +90 degrees over the Z axis) andtool0(rotated by +90 degrees over the Z axis) , I should call set_pose_reference_frame and set_end_effector_link like below.

ex) :modified /khi_rs007n_moveit_config/script/rs007n_demo.py

    group = MoveGroupCommander("manipulator")
    exec_vel = 0.5

    rospy.loginfo("pose1 start")
    group.set_pose_reference_frame("base")
    group.set_end_effector_link("tool0")
    group.set_max_velocity_scaling_factor(exec_vel)
    group.set_pose_target([0.5,-0.2,0.2,0.0,1.0,0.0])
    group.go()

Is this way usingMoveGroupCommander reasonable?

[gavanderhoorn]

@matsui-hiro wrote:

All-zeros TCP on KHI controller is coincident with origin of link6 frame.

So the transform between link6 and tool0 would be just a rotation of 90 degrees over Z in that case (as you write) to "compensate" for the REP-103 vs KHI difference.

So, to call set_pose_target with same pose values after introduction of base(rotated by +90 degrees over the Z axis) andtool0(rotated by +90 degrees over the Z axis) , I should call set_pose_reference_frame and set_end_effector_link like below. [..] Is this way usingMoveGroupCommander reasonable?

I would say "yes, it is". In fact, it would be best if everyone would first explicitly set pose reference frames and EEF links, but right now that is not common practice (that may be because there are not a lot of robots that have any frame other than X+ "forward" in common usage).

If I understand your example correctly this should result in the EEF ending up in the exact same location on both the KHI TP and "in ROS" (ie: when asking for the base->tool0 transform).

[matsui-hiro]

Thank you very much for explaining. I understood the way for transformations between KHI and ROS coordinate system conventions. So I'll create PR of REP-199 (adding the base and tool0).