Correct `/tf` calculation for base tracks #172

ted-miller · 2023-10-05T20:45:42Z

Fixes #170

Two problems:

The wrong parameters were being read in the lib
Scaling was wrong on the coordinates. For example, 1.234 m was getting converted to 1 um.

gavanderhoorn · 2023-10-06T06:21:29Z

I'll do a full review + test later, but as a first high-level comment: #78 includes the start of a more fleshed out M+ struct <-> ROS msg conversion library, of which MPCOORD -> geometry_msgs/Pose and geometry_msgs/Transform could be part. See this.

We could consider making that part of this PR instead: would help reduce the duplication, and reflect intent better.

(note: the referenced code obviously is also incorrect, just as the code changed/fixed by this PR, but that's a detail)

gavanderhoorn · 2023-10-06T07:57:21Z

I'll probably refactor this a bit so we can add tests.

That would also increase trust in these parts of the code.

gavanderhoorn · 2023-10-06T12:33:33Z

A quick test of the TF now being broadcast:

X axis displacement is now taken into account, so the main issue seems resolved.

We might want to open a discussion to see whether it'd be possible to include a Y and/or Z component as well (either through the config file, or via some other calibration that we may have available on the controller).

As-is, this is currently not usable by applications, unless another transform (broadcast by something else) adds the Z offset -- but making another frame parent of world feels at least conceptually 'weird'.

Edit: we could perhaps rename world to base_link, which would follow conventions (base_link is the start of any kinematic chain / urdf model). Together with the tf_prefix setting, this could be used to make a sane and unique TF (sub)tree.

Example: setting tf_prefix="my_robot/" would result in:

my_robot/base_link
  -> my_robot/r1/base
    -> my_robot/r1/flange
      --> my_robot/r1/tool0
      \-> my_robot/r1/tcp_0

another option could be the allow renaming all TF frames, instead of just prefixing.

ted-miller · 2023-10-06T13:00:51Z

We might want to open a discussion to see whether it'd be possible to include a Y and/or Z component as well (either through the config file, or via some other calibration that we may have available on the controller).

This is determined by the configuration of the robot during initial configuration.

Then the direction of travel is taken into account here:

motoros2/src/PositionMonitor.c

Lines 296 to 304 in 9492838

    
           switch (group->baseTrackInfo.motionType[i]) 
        
           { 
        
           case MOTION_TYPE_X: coordTrackTravel.x = METERS_TO_MICROMETERS(track_pos_meters[i]); break; 
        
           case MOTION_TYPE_Y: coordTrackTravel.y = METERS_TO_MICROMETERS(track_pos_meters[i]); break; 
        
           case MOTION_TYPE_Z: coordTrackTravel.z = METERS_TO_MICROMETERS(track_pos_meters[i]); break; 
        
           case MOTION_TYPE_RX: coordTrackTravel.rx = RAD_TO_DEG_0001(track_pos_meters[i]); break; 
        
           case MOTION_TYPE_RY: coordTrackTravel.ry = RAD_TO_DEG_0001(track_pos_meters[i]); break; 
        
           case MOTION_TYPE_RZ: coordTrackTravel.rz = RAD_TO_DEG_0001(track_pos_meters[i]); break; 
        
           }

While I was debugging, I configured a RECT-XY track and did verify that I could jog along both axes.

I don't think it makes sense to make the direction "configurable" from the software side. The robot is going to be physically mounted to the track in a certain orientation. Plus we want the position reported by ROS to match what is shown on the teach pendant. (If you look at current-position on your pendant in base-frame, it will show travel along the x axis.)

As-is, this is currently not usable by applications, unless another transform (broadcast by something else) adds the Z offset

I'm not sure what you mean here. Are you referring to the height of the track relative to the floor?

The offset from the track-origin to the robot-origin is configured by Yaskawa during integration/commissioning. This is the baseTrackInfo.offsetFromBaseToRobotOrigin.

motoros2/src/PositionMonitor.c

Line 308 in 9492838

    
           mpZYXeulerToFrame(&group->baseTrackInfo.offsetFromBaseToRobotOrigin, &frameTrackToRobot);

we could perhaps rename world to base_link, which would follow conventions

I'm going to rely on you to dictate naming conventions.

another option could be the allow renaming all TF frames, instead of just prefixing.

This seems excessive.

gavanderhoorn · 2023-10-06T14:46:00Z

We might want to open a discussion to see whether it'd be possible to include a Y and/or Z component as well (either through the config file, or via some other calibration that we may have available on the controller).

This is determined by the configuration of the robot during initial configuration.

Then the direction of travel is taken into account here:

motoros2/src/PositionMonitor.c

Lines 296 to 304 in 9492838

switch (group->baseTrackInfo.motionType[i])

{

case MOTION_TYPE_X: coordTrackTravel.x = METERS_TO_MICROMETERS(track_pos_meters[i]); break;

case MOTION_TYPE_Y: coordTrackTravel.y = METERS_TO_MICROMETERS(track_pos_meters[i]); break;

case MOTION_TYPE_Z: coordTrackTravel.z = METERS_TO_MICROMETERS(track_pos_meters[i]); break;

case MOTION_TYPE_RX: coordTrackTravel.rx = RAD_TO_DEG_0001(track_pos_meters[i]); break;

case MOTION_TYPE_RY: coordTrackTravel.ry = RAD_TO_DEG_0001(track_pos_meters[i]); break;

case MOTION_TYPE_RZ: coordTrackTravel.rz = RAD_TO_DEG_0001(track_pos_meters[i]); break;

}

While I was debugging, I configured a RECT-XY track and did verify that I could jog along both axes.

See below.

I don't think it makes sense to make the direction "configurable" from the software side. The robot is going to be physically mounted to the track in a certain orientation. Plus we want the position reported by ROS to match what is shown on the teach pendant. (If you look at current-position on your pendant in base-frame, it will show travel along the x axis.)

No, that's not what I was thinking of.

And I agree with what you write.

As-is, this is currently not usable by applications, unless another transform (broadcast by something else) adds the Z offset

I'm not sure what you mean here. Are you referring to the height of the track relative to the floor?

The offset from the track-origin to the robot-origin is configured by Yaskawa during integration/commissioning. This is the baseTrackInfo.offsetFromBaseToRobotOrigin.

motoros2/src/PositionMonitor.c

Line 308 in 9492838

mpZYXeulerToFrame(&group->baseTrackInfo.offsetFromBaseToRobotOrigin, &frameTrackToRobot);

This was what I was thinking of.

It looks like this just wasn't done for my robot + track here.

So from the ROS 2 perspective, my robot is always "on the floor".

I'll just need to update that info on my controller.

we could perhaps rename world to base_link, which would follow conventions

I'm going to rely on you to dictate naming conventions.

I'll think about it a bit more, but I believe it would make sense.

world is really intended to be the singular root / origin of everything in the cell.

There's a very good chance more complex applications would have that not coincident with the world frame broadcast by MR2.

another option could be the allow renaming all TF frames, instead of just prefixing.

This seems excessive.

Users using a URDF or XACRO (from one of our description packages fi) would have that freedom though.

Only support MP_COORD -> geometry_msgs/{Pose,Transform} for now, but others can be added later.

Reduce duplication of conversion code.

So they can more easily be reused by other tests (in other files).

A naive first implementation of some tests.

gavanderhoorn · 2023-10-06T16:47:15Z

@ted-miller: I've slightly refactored the MP_COORD to geometry_msgs/msg/Transform code (5236faf, 51afeae).

I've also added a couple of tests for the new functions, and while doing that, I've refactored some of the utility functions and put them in a separate file.

Output of the unit tests:

Performing unit tests
Testing CtrlGroup ConvertToRosPos - 6 DOF style: PASS
Testing CtrlGroup ConvertToMotoPos - 6 DOF style: PASS
Testing CtrlGroup ConvertToRosPos - Delta style: PASS
Testing CtrlGroup ConvertToMotoPos - Delta style: PASS
Testing CtrlGroup ConvertToRosPos - SIA style: PASS
Testing CtrlGroup ConvertToMotoPos - SIA style: PASS
Testing Ros_MpCoord_To_GeomMsgsPose: PASS
Testing Ros_MpCoord_To_GeomMsgsTransform: PASS
Testing SUCCESSFUL

It's just some very simple tests, but they should show the new functions are not doing anything really crazy.

I've also verified again the TF broadcast by this refactored version and everything still looks like it did in #172 (comment).

Seeing as I've now added commits to this PR as well: could you please review it and let me know whether you agree with the changes?

src/MathConstants.h

src/MotoROS2_AllControllers.vcxproj

gavanderhoorn · 2023-10-06T16:50:41Z

src/Tests_TestUtils.c

we might want to start thinking about migrating to a "real" C testing library.

To not drown in the rest of the output.

ted-miller · 2023-10-06T19:59:58Z

could you please review it and let me know whether you agree with the changes?

Everything looks good to me.

gavanderhoorn

Tested on a YRC1 + GP25 + TSL1000.

ted-miller added 3 commits October 5, 2023 16:17

GP_getBaseAxisInfo was using wrong group number when reading parameters

72e4c53

It's micro; not nano

f753fde

Fix unit scaling

9492838

ted-miller requested a review from gavanderhoorn October 5, 2023 20:45

ted-miller added this to the 0.1.2 milestone Oct 5, 2023

gavanderhoorn added 5 commits October 6, 2023 18:38

Introduce ROS<->M+ concept conversion functions

5236faf

Only support MP_COORD -> geometry_msgs/{Pose,Transform} for now, but others can be added later.

pos_mon: use new conv functions for Transforms

51afeae

Reduce duplication of conversion code.

Refactor: put test comparison functions in separate file

ead3864

So they can more easily be reused by other tests (in other files).

Add tests for new M+<->ROS conversion functions

3277555

A naive first implementation of some tests.

Include new tests in main test run

08fc073

gavanderhoorn reviewed Oct 6, 2023

View reviewed changes

Clearly demarcate test results in debug log

8950ca5

To not drown in the rest of the output.

gavanderhoorn force-pushed the debug_tf branch from 76d41e1 to 8950ca5 Compare October 6, 2023 16:52

gavanderhoorn mentioned this pull request Oct 7, 2023

Suggestion: create separate 'tests' VS project #173

Open

gavanderhoorn approved these changes Oct 7, 2023

View reviewed changes

gavanderhoorn merged commit f6f3f25 into main Oct 7, 2023
14 checks passed

gavanderhoorn deleted the debug_tf branch October 7, 2023 14:51

gavanderhoorn mentioned this pull request Oct 19, 2023

Refactor: introduce getter for "does group have a base track associated?" #183

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Correct `/tf` calculation for base tracks #172

Correct `/tf` calculation for base tracks #172

ted-miller commented Oct 5, 2023

gavanderhoorn commented Oct 6, 2023 •

edited

Loading

gavanderhoorn commented Oct 6, 2023

gavanderhoorn commented Oct 6, 2023 •

edited

Loading

ted-miller commented Oct 6, 2023 •

edited by gavanderhoorn

Loading

gavanderhoorn commented Oct 6, 2023 •

edited

Loading

gavanderhoorn commented Oct 6, 2023

gavanderhoorn Oct 6, 2023

ted-miller commented Oct 6, 2023

gavanderhoorn left a comment

Correct /tf calculation for base tracks #172

Correct /tf calculation for base tracks #172

Conversation

ted-miller commented Oct 5, 2023

gavanderhoorn commented Oct 6, 2023 • edited Loading

gavanderhoorn commented Oct 6, 2023

gavanderhoorn commented Oct 6, 2023 • edited Loading

ted-miller commented Oct 6, 2023 • edited by gavanderhoorn Loading

gavanderhoorn commented Oct 6, 2023 • edited Loading

gavanderhoorn commented Oct 6, 2023

gavanderhoorn Oct 6, 2023

Choose a reason for hiding this comment

ted-miller commented Oct 6, 2023

gavanderhoorn left a comment

Choose a reason for hiding this comment

Correct `/tf` calculation for base tracks #172

Correct `/tf` calculation for base tracks #172

gavanderhoorn commented Oct 6, 2023 •

edited

Loading

gavanderhoorn commented Oct 6, 2023 •

edited

Loading

ted-miller commented Oct 6, 2023 •

edited by gavanderhoorn

Loading

gavanderhoorn commented Oct 6, 2023 •

edited

Loading