Solving velocity deviation in Pilz

[vivekcdavid]

The solution for solving the deviation as described in #2909.

The issue is that the Pilz LIN planner deviates from target linear speed when rotation is involved in a move. The issue seems to arise from two factors within Pilz, both related to KDL::Path_line.

1. While generating the path using KDL::Path_line a parameter called eqradius is used. The use of Eqradius prevents very fast angular motions but as a side effect the motion deviated from the specified linear speed. * solution* : set eqradius to zero if the motion should only consider the specified linear TCP speed

2. Even after setting eqradius to zero we observed a small deviation in the time take for motion, this was pin-pointed to the use of trapezoidal velocity profile. solution : By using a rectangular velocity profile this issue seems to be solved.

It would be nice if these changes can be used if the use-case requires that the time for motion has to be accurate. Now I have permanently changed the code but maybe we could force Pilz to use rectangular profile when acceleration limit is not specified as suggested by @sea-bass ?

[mergify[bot]]

Please target the main branch for development, we will backport the changes to humble for you if approved and if they don't break API.

[sea-bass]

Please target the main branch for development, we will backport the changes to humble for you if approved and if they don't break API.

Thanks @vivekcdavid! As this comment says, all new changes should go through to the main branch and backported to humble. Consider rebasing your changes on main and retargeting that branch here.

I'll still leave a few comments for now in case you want to go at those too.

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[mergify[bot]]

This pull request is in conflict. Could you fix it @vivekcdavid?

[mergify[bot]]

Please target the main branch for development, we will backport the changes to humble for you if approved and if they don't break API.

[gaspatxo]

hey @sea-bass,

Me and @vivekcdavid have been thinking what the best way forwards is.

We first considered adding the interpretetion of acceleration=0 on a trajectory profile implies the usage of the rectiliniar profile (as if acceleration=∞). But adding a zero case can easily introduce math bugs; this is somethin apparently move_group already avoids, by setting accleration to 1 should the value be given as 0.

Our next and current idea is adding a new flag variable in the MotionPlanRequest after filling it out with constructMotionPlanRequest. Then we would interpret that in the trajectory_planner_lin.cpp to use a rectangular profile.

Please give us your thoughts in the idea, what are we missing, what would be better for us to implement.

[sea-bass]

@gaspatxo I might suggest instead of modifying the request, to make a boolean parameter to either use trapezoidal or rectangular velocity profiles. Since this is specific to one planner type, it would bloat the planning request definition to add this there.

If you (or users in general) need to mix and match both profiles, you can launch 2 separate Pilz planner instances with the different parameters.

What do you think?

[gaspatxo]

Where would we add this parameter? we are currently making use of pilz planner over the move_group_interface, modifying that function would modify it for all users which Im guessing it is not a good idea.

Are you suggesting we stop using the move_group_interface and directly call the pilz_planner?

[sea-bass]

Where would we add this parameter?

For example, the cartesian limits are currently defined as parameters here: https://github.com/moveit/moveit2/blob/b47e302d5652c0f9ee06bffad749c1793690bb26/moveit_planners/pilz_industrial_motion_planner/src/cartesian_limits_parameters.yaml#L1

And are read in here: https://github.com/moveit/moveit2/blob/b47e302d5652c0f9ee06bffad749c1793690bb26/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp#L138

It is kind of annoying that these parameters only refer to the Cartesian limits, since it feels out of place to add what we've been talking about there.

But maybe this could come with a slight rename of these parameters, or you could add a completely new parameter definition file?

Are you suggesting we stop using the move_group_interface and directly call the pilz_planner?

Not at all! These would be configuration time parameters that would be set at launch time. So basically they are server-side parameters, but when you use move group interface you're still calling out to that server to plan.

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.