Issue 233-outside goal tolerance #241

yai-rosejo · 2024-04-16T19:47:13Z

This PR is to solve multiple issues dealing with goal tolerances referenced in #233

Received Goal tolerances can now be received out of order, the name of the tolerance is used to match it with the expected joint.
The final goal position is used instead of the desired position to deal with a small out of sync issue.
The special notations of -1.0, to ignore tolerance, and 0.0, to use default values, is now supported.

This PR does not fix the issues with the Ros_Debug_BroadcastMsg function and doubles.

Testing:
tolerance_issue_04162024.zip

Upload the mr2_yrc1_h.out to the controller and then restart the controller
Run the microros client in a terminal :
sudo docker run -it --net=host -v /dev:/dev --privileged microros/micro-ros-agent:humble udp4 -p 8888
Start the TrajMode:
ros2 service call /start_traj_mode motoros2_interfaces/srv/StartTrajMode
Run the fjt client:
python3 fjt_client.py
Optionally the debug_listener can also be ran to capture messages.

src/ActionServer_FJT.c

src/MotionControl.h

gavanderhoorn · 2024-04-18T15:58:04Z

@yai-rosejo: I've used your PR as a basis and restructured it a little. See this for the diff. All WIP right now.

It's basically the same as your changes, but split into a couple of functions. My idea is we'd be able to write some tests for the new functionality this way.

I've tested it on my YRC with GP25 and it (still) works.

High-level question: did you intentionally iterate over the maxAxes? Would limiting it to just g_Ros_Controller.totalAxesCount (or feedback_FollowJointTrajectory.feedback.joint_names.size) not work as well?

yai-rosejo · 2024-04-18T16:13:52Z

@yai-rosejo: I've used your PR as a basis and restructured it a little. See this for the diff. All WIP right now.

It's basically the same as your changes, but split into a couple of functions. My idea is we'd be able to write some tests for the new functionality this way.

I've tested it on my YRC with GP25 and it (still) works.

High-level question: did you intentionally iterate over the maxAxes? Would limiting it to just g_Ros_Controller.totalAxesCount (or feedback_FollowJointTrajectory.feedback.joint_names.size) not work as well?

I just did a cursory look through your solution; I think it looks much better than my current one.
I iterated over all of the maxAxes due to violators being the size of maxAxes and the 'for' loop checking the differences already using maxAxes.

motoros2/src/ActionServer_FJT.c

Lines 498 to 504 in daa47bd

    
           double violators[MAX_CONTROLLABLE_GROUPS * MP_GRP_AXES_NUM]; 
        
           bzero(violators, sizeof(violators)); 
        
           for (int axis = 0; axis < maxAxes; axis += 1) 
        
           { 
        
               diff = fabs(feedback_FollowJointTrajectory.feedback.desired.positions.data[axis] - feedback_FollowJointTrajectory.feedback.actual.positions.data[axis]);

gavanderhoorn · 2024-04-19T08:11:16Z

Ok. Thanks.

I'll add some tests, and then push some commits to this PR if that would be OK with you.

yai-rosejo · 2024-04-19T13:15:14Z

Ok. Thanks.

I'll add some tests, and then push some commits to this PR if that would be OK with you.

That works for me!

gavanderhoorn · 2024-04-19T14:42:54Z

Added some first batch of tests for Ros_ActionServer_FJT_Parse_GoalPosTolerances(..):

Click to expand

===
Performing unit tests
~~~
Ros_ActionServer_FJT_Parse_GoalPosTolerances: one or more inputs NULL, abort
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_null_args: goal_joint_tolerances NULL: PASS
Ros_ActionServer_FJT_Parse_GoalPosTolerances: one or more inputs NULL, abort
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_null_args: joint_names NULL: PASS
Ros_ActionServer_FJT_Parse_GoalPosTolerances: one or more inputs NULL, abort
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_null_args: posTolerances NULL: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_null_args: PASS
~~~
Ros_ActionServer_FJT_Parse_GoalPosTolerances: no joint tolerances specified, returning 6 defaults
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_empty_jtolerances: call: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_empty_jtolerances: all defaults: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_empty_jtolerances: PASS
~~~
Ros_ActionServer_FJT_Parse_GoalPosTolerances: pos tolerance output array too small: 1 < 2 (nr of joints)
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_out_array_too_small: out array too small: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_out_array_too_small: PASS
~~~
Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint5, 1.000000
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_more_jtol_than_jnames: call: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_more_jtol_than_jnames: all defaults: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_more_jtol_than_jnames: PASS
~~~
Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint0, 1.000000
Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint0
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot0: call: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot0: expected j0 pos tol: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot0: PASS
~~~
Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint0, 1.000000
Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint0
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot2: call: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot2: j0 pos tol at [2]: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot2: PASS
~~~
Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint1, 0.123000
Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint1
Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint2, 0.234000
Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint2
Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint1, 0.345000
Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint1
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: call: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: j0 pos tol at [0]: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: j2 pos tol at [1]: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: j1 pos tol at [2]: PASS
Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: PASS
~~~
Testing SUCCESSFUL
===

gavanderhoorn · 2024-04-22T12:58:05Z

@yai-rosejo: I've added tests for the two new functions I introduced, refactored things a bit and cleaned up the Git history. See the diff.

Your changes are now all squashed into the initial commit.

Could you take a look whether this would work for you? If you agree, I'll force-push these changes to your branch to update this PR.

gavanderhoorn · 2024-04-22T13:01:56Z

@yai-rosejo wrote:

@gavanderhoorn wrote:

High-level question: did you intentionally iterate over the maxAxes? Would limiting it to just g_Ros_Controller.totalAxesCount (or feedback_FollowJointTrajectory.feedback.joint_names.size) not work as well?

[..] I iterated over all of the maxAxes due to violators being the size of maxAxes and the 'for' loop checking the differences already using maxAxes.

motoros2/src/ActionServer_FJT.c

Lines 498 to 504 in daa47bd

double violators[MAX_CONTROLLABLE_GROUPS * MP_GRP_AXES_NUM];

bzero(violators, sizeof(violators));

for (int axis = 0; axis < maxAxes; axis += 1)

{

diff = fabs(feedback_FollowJointTrajectory.feedback.desired.positions.data[axis] - feedback_FollowJointTrajectory.feedback.actual.positions.data[axis]);

@ted-miller: would it be absolutely necessary to iterate over maxAxes, or would limiting those arrays and loops to just the configured nr of axes also work? I assume the latter, but wanted to make sure.

ted-miller · 2024-04-22T14:07:00Z

would it be absolutely necessary to iterate over maxAxes, or would limiting those arrays and loops to just the configured nr of axes also work? I assume the latter, but wanted to make sure.

Limiting the loops would be OK. I can't think of any justification for keeps maxaxes.

yai-rosejo · 2024-04-22T17:16:31Z

@yai-rosejo: I've added tests for the two new functions I introduced, refactored things a bit and cleaned up the Git history. See the diff.

Your changes are now all squashed into the initial commit.

Could you take a look whether this would work for you? If you agree, I'll force-push these changes to your branch to update this PR.

The changes look good to me. You can go ahead with the force push.

gavanderhoorn · 2024-04-26T12:10:04Z

Working on this a bit more and I'm beginning to wonder whether we shouldn't already parse the JointTolerance instances when first accepting the goal. If parsing fails -- due to unknown joints being referenced for instance -- we could refuse to execute the goal and provide a very clear error code and message to clients.

If we fail to parse at the end of an executed trajectory, we can't really do much anymore: failing the goal completely seems like a bit of a 'waste' of a good trajectory execution and failing so late for something almost 'unrelated' doesn't apply fail-early and least-surprise principles.

ted-miller · 2024-04-26T14:40:31Z

So you're suggesting separating the logic of validating the tolerance input from validating the position is within that tolerance? That makes sense to me.

We've got the Ros_MotionControl_InitTrajectory function which returns a custom Init_Trajectory_Status that could clearly identify an issue with the specified input.

My next question is how much of the goal message do we validate? So far, we've been ignoring much of the goal message, such as path_tolerance and the component_x_tolerance. Do those remain unused?

gavanderhoorn · 2024-04-26T15:20:32Z

So you're suggesting separating the logic of validating the tolerance input from validating the position is within that tolerance? That makes sense to me.

Indeed.

We've got the Ros_MotionControl_InitTrajectory function which returns a custom Init_Trajectory_Status that could clearly identify an issue with the specified input.

True, although the goal_tolerance field is not part of the trajectory.

My next question is how much of the goal message do we validate? So far, we've been ignoring much of the goal message, such as path_tolerance and the component_x_tolerance. Do those remain unused?

I would for now indeed suggest to fix & refactor the current implementation which only takes the goal_tolerance field into account, not extend it.

gavanderhoorn · 2024-05-01T13:36:28Z

Coming back to this: unknown joints in goal_tolerance will not immediately cause problems, they will simply be ignored by the current implementation.

Adding goal validation -- which would include goal_tolerance validation -- would be nice, but wouldn't be needed to resolve the current issue (ie: #233).

I'll create an issue to log the enhancement request.

@yai-rosejo: I've pushed some more commits to my version of your branch. If you could PTAL and let me know whether you'd still be ok with me force-pushing here? Thanks.

yai-rosejo · 2024-05-06T14:06:44Z

Coming back to this: unknown joints in goal_tolerance will not immediately cause problems, they will simply be ignored by the current implementation.

Adding goal validation -- which would include goal_tolerance validation -- would be nice, but wouldn't be needed to resolve the current issue (ie: #233).

I'll create an issue to log the enhancement request.

@yai-rosejo: I've pushed some more commits to my version of your branch. If you could PTAL and let me know whether you'd still be ok with me force-pushing here? Thanks.

I have taken a look and agree with pushing those onto this branch. This PR is focused on supporting goal functionalities, so I also agree that the goal validation should be a separate task.

src/ActionServer_FJT.c

Don't assume all joints have associated tolerances, and deal with potentially out-of-order tolerance specifications (compared to order of joints MotoROS2 uses internally).

Easier to test and potentially to reuse.

Not just when there are no JointTolerance instances.

Note: they are appended to the main ActionServer_FJT source file as the function(s) tested are local to that compilation unit (ie: static).

They're not needed outside this compilation unit.

Need space for at least the number of joint positions in the trajectory point.

Instead of the maximum possible number always.

Work-around doesn't seem needed anymore.

A debug log warning only right now. We could consider making it a fatal error and actually return it.

This is a temporary implementation, as it would be better to check the goal_tolerance field can be parsed when validating a new goal.

gavanderhoorn · 2024-05-14T14:53:52Z

@yai-rosejo and/or @ted-miller: could I perhaps ask you to test this implementation on your hw?

ted-miller · 2024-05-14T17:17:39Z

Definitely. I can't do it today (and probably not tomorrow), but I'll get it done. I realize this PR has lingered for quite a while now.

ted-miller · 2024-05-17T20:44:45Z

src/ActionServer_FJT.c

+    }
+
+    //it's OK if we are passed more JointTolerance instances than we have joints,
+    //as technically a client could send duplicate tolerance specs (ie: multiple


Is this common practice? It seems to me that this is a bad idea. If I accidently duplicate a joint, then I would be very confused when my intended tolerance isn't applied.

not common practice per se. I just didn't feel like making it an issue.

We could upgrade it to one though, and return a specific error code if duplicated joint names are passed as part of the goal.

src/ActionServer_FJT.c

ted-miller · 2024-05-17T20:49:45Z

src/ActionServer_FJT.c

+        //couldn't find joint
+        if (ptol_idx == joint_names->size)
+        {
+            //TODO(gavanderhoorn): make this a fatal error?


I assume (hope) that you mean fatal to the trajectory request (return not OK) and not a fatal assertion.

Yes, even more 'local' perhaps: fatal as in: exit this function with an appropriate return code.

src/ActionServer_FJT.c

ted-miller · 2024-05-17T21:26:46Z

Functionally, everything seems to be working well on hardware.

Co-authored-by: Ted Miller <ted.miller@motoman.com>

ted-miller

I'm fine with merging this.

yai-rosejo requested review from gavanderhoorn and ted-miller April 16, 2024 19:47

yai-rosejo self-assigned this Apr 16, 2024

gavanderhoorn reviewed Apr 18, 2024

View reviewed changes

src/ActionServer_FJT.c Outdated Show resolved Hide resolved

gavanderhoorn reviewed Apr 18, 2024

View reviewed changes

src/ActionServer_FJT.c Show resolved Hide resolved

gavanderhoorn reviewed Apr 18, 2024

View reviewed changes

src/MotionControl.h Outdated Show resolved Hide resolved

gavanderhoorn mentioned this pull request May 1, 2024

Improve FJT goal validation (include JointTolerance checks fi) #250

Open

gavanderhoorn force-pushed the issue_233_outside_goal_tolerance branch from 638063b to 64c6524 Compare May 8, 2024 08:25

gavanderhoorn reviewed May 8, 2024

View reviewed changes

src/ActionServer_FJT.c Outdated Show resolved Hide resolved

yai-rosejo and others added 7 commits May 14, 2024 16:46

fix parsing of goal_tolerance entries in goal

742e4fc

Don't assume all joints have associated tolerances, and deal with potentially out-of-order tolerance specifications (compared to order of joints MotoROS2 uses internally).

refactor: make parsing stand-alone functions

dfe98ec

Easier to test and potentially to reuse.

always set default joint position tolerances

d4e5c76

Not just when there are no JointTolerance instances.

add tests for Parse_GoalPosTolerance(..)

b385677

Note: they are appended to the main ActionServer_FJT source file as the function(s) tested are local to that compilation unit (ie: static).

add tests for Reorder_TrajPt_To_Internal_Order(..)

12699e4

make all tests static

2917ef7

They're not needed outside this compilation unit.

verify out array is of sufficient length

35c8f85

Need space for at least the number of joint positions in the trajectory point.

gavanderhoorn added 5 commits May 14, 2024 16:46

clearer debug output

1464eca

only check configured axes

c1f2c0e

Instead of the maximum possible number always.

log doubles, not ints

1cc7db7

Work-around doesn't seem needed anymore.

emit warning about JointTolerances for unknown joints

c52dfa3

A debug log warning only right now. We could consider making it a fatal error and actually return it.

failure to parse tolerances fails traj execution

c552d58

This is a temporary implementation, as it would be better to check the goal_tolerance field can be parsed when validating a new goal.

gavanderhoorn force-pushed the issue_233_outside_goal_tolerance branch from 64c6524 to c552d58 Compare May 14, 2024 14:51

Add Tests_ActionServer_FJT to project

c97a623

ted-miller reviewed May 17, 2024

View reviewed changes

src/ActionServer_FJT.c Outdated Show resolved Hide resolved

ted-miller reviewed May 17, 2024

View reviewed changes

src/ActionServer_FJT.c Outdated Show resolved Hide resolved

gavanderhoorn and others added 2 commits May 20, 2024 16:37

Update src/ActionServer_FJT.c

b73ad5d

Co-authored-by: Ted Miller <ted.miller@motoman.com>

Update src/ActionServer_FJT.c

2cb6115

Co-authored-by: Ted Miller <ted.miller@motoman.com>

ted-miller approved these changes May 21, 2024

View reviewed changes

ted-miller merged commit cc03e31 into Yaskawa-Global:main Jun 11, 2024
6 checks passed

gavanderhoorn mentioned this pull request Jun 18, 2024

persistent "Final position was outside tolerance" #233

Open

gavanderhoorn added this to the 0.1.3 milestone Jul 10, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Issue 233-outside goal tolerance #241

Issue 233-outside goal tolerance #241

yai-rosejo commented Apr 16, 2024

gavanderhoorn commented Apr 18, 2024 •

edited

Loading

yai-rosejo commented Apr 18, 2024

gavanderhoorn commented Apr 19, 2024

yai-rosejo commented Apr 19, 2024

gavanderhoorn commented Apr 19, 2024 •

edited

Loading

gavanderhoorn commented Apr 22, 2024

gavanderhoorn commented Apr 22, 2024

ted-miller commented Apr 22, 2024

yai-rosejo commented Apr 22, 2024

gavanderhoorn commented Apr 26, 2024

ted-miller commented Apr 26, 2024

gavanderhoorn commented Apr 26, 2024

gavanderhoorn commented May 1, 2024

yai-rosejo commented May 6, 2024

gavanderhoorn commented May 14, 2024

ted-miller commented May 14, 2024

ted-miller May 17, 2024

gavanderhoorn May 20, 2024

ted-miller May 17, 2024

gavanderhoorn May 20, 2024

ted-miller commented May 17, 2024

ted-miller left a comment

Issue 233-outside goal tolerance #241

Issue 233-outside goal tolerance #241

Conversation

yai-rosejo commented Apr 16, 2024

gavanderhoorn commented Apr 18, 2024 • edited Loading

yai-rosejo commented Apr 18, 2024

gavanderhoorn commented Apr 19, 2024

yai-rosejo commented Apr 19, 2024

gavanderhoorn commented Apr 19, 2024 • edited Loading

gavanderhoorn commented Apr 22, 2024

gavanderhoorn commented Apr 22, 2024

ted-miller commented Apr 22, 2024

yai-rosejo commented Apr 22, 2024

gavanderhoorn commented Apr 26, 2024

ted-miller commented Apr 26, 2024

gavanderhoorn commented Apr 26, 2024

gavanderhoorn commented May 1, 2024

yai-rosejo commented May 6, 2024

gavanderhoorn commented May 14, 2024

ted-miller commented May 14, 2024

ted-miller May 17, 2024

Choose a reason for hiding this comment

gavanderhoorn May 20, 2024

Choose a reason for hiding this comment

ted-miller May 17, 2024

Choose a reason for hiding this comment

gavanderhoorn May 20, 2024

Choose a reason for hiding this comment

ted-miller commented May 17, 2024

ted-miller left a comment

Choose a reason for hiding this comment

gavanderhoorn commented Apr 18, 2024 •

edited

Loading

gavanderhoorn commented Apr 19, 2024 •

edited

Loading