Incorrect TF broadcast for robots-on-tracks

[gavanderhoorn]

After some discussion in #169, it appears the current implementation of TF broadcasting for robots on tracks is not working as it should.

Jogging a robot on a track does not update the world -> r1/base transform. Only robot pose changes are reflected in the r1/base -> r1/tool0 (and r1/base -> r1/tcp_0) transforms.

[gavanderhoorn]

somehow I didn't notice this, even though I have a robot-on-a-track here.

Most of the tools we use (also to test) don't consume the TF broadcast by MotoROS2, which could explain this.

As to a possible solution: if the Yaskawa Base to Yaskawa RF transform could be included in the world -> r1/base transform (on the ROS side), that would seem to fix this issue.

---

As to a possible solution: if the Yaskawa Base to Yaskawa RF transform could be included in the world -> r1/base transform (on the ROS side), that would seem to fix this issue.

Edit: a difficulty here would be that without a calibration of the robot's TCP origin to the track's "origin", this transform would likely still only be partially correct (ie: it would only provide an offset in X (or whichever axis the track is configured to move along)).

[gavanderhoorn]

Note that the FK performed by the robot_state_publisher (together with a correct URDF (including the robot and the track)) is correct, as it's based on joint angles.

[ted-miller]

Now I'm really confused. Now I think that it should actually work.

Turns out that the offsetFromBaseToRobotOrigin is taken into account.

motoros2/src/PositionMonitor.c

Lines 306 to 313 in 62ac8ed

	mpZYXeulerToFrame(&coordWorldToBase, &frameWorldToTrack);
	mpZYXeulerToFrame(&coordTrackTravel, &frameTrackTravel);
	mpZYXeulerToFrame(&group->baseTrackInfo.offsetFromBaseToRobotOrigin, &frameTrackToRobot);
	mpMulFrame(&frameWorldToTrack, &frameTrackTravel, &frameWorldToTravel);
	mpMulFrame(&frameWorldToTravel, &frameTrackToRobot, &frameWorldToRobot);
	mpFrameToZYXeuler(&frameWorldToRobot, &coordWorldToBase);

I'll take a detailed look at this later today.

[gavanderhoorn]

Maybe something here is not happy with my setup?

motoros2/src/CtrlGroup.c

Lines 88 to 109 in 62ac8ed

	if (Ros_CtrlGroup_IsRobot(ctrlGroup))
	{
	mpGetToolNo(ctrlGroup->groupId, &retToolData);
	//TODO: need to update this value when selected tool changes
	ctrlGroup->tool = retToolData.sToolNo;
	int baseIdOffset = (int)ctrlGroup->groupId - (int)MP_R1_GID;
	ctrlGroup->baseTrackGroupIndex = mpCtrlGrpId2GrpNo((MP_GRP_ID_TYPE)baseIdOffset + MP_B1_GID);
	if (ctrlGroup->baseTrackGroupIndex != ERROR)
	{
	ctrlGroup->baseTrackGroupId = (MP_GRP_ID_TYPE)(baseIdOffset + MP_B1_GID);
	GP_getBaseAxisInfo(ctrlGroup->baseTrackGroupIndex, &ctrlGroup->baseTrackInfo);
	}
	else
	ctrlGroup->baseTrackGroupId = (MP_GRP_ID_TYPE)-1;
	}
	else
	{
	ctrlGroup->tool = 0;
	ctrlGroup->baseTrackGroupId = (MP_GRP_ID_TYPE)-1;
	ctrlGroup->baseTrackGroupIndex = -1;
	}