flexibility to include or exclude continuous joints from trajectory caching

[robintomar1]

Is your feature request related to a problem? Please describe.

If we have a wheeled base with an arm, the wheels are usually marked as continuous joints. In such a scenario whenever we store a trajectory using trajectory cashing the wheels positions are stored as well. This creates problem during fetching a matching trajectory. As i might be on the same position but my wheel joints can still report different values causing failure in matching. The problem will exist for any system specially when we test them in simulation as even a caster's position will create a mismatch.

Describe the solution you'd like

A simple boolean may be to enable disable inclusion of continuous joints

Describe alternatives you've considered

I have not considered any alternative as this seems most straightforward way to handle the issue.

Additional context

No response

[robintomar1]

Update : I have made changes to the following 4 files :

1. moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp
2. moveit_ros/trajectory_cache/src/features/get_cartesian_path_request_features.cpp
3. moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp
4. moveit_ros/trajectory_cache/src/utils/utils.cpp b/moveit_ros/trajectory_cache/src/utils/utils.cpp

appendRobotStateJointStateAsFetchQueryWithTolerance( ) has been copied and reimplemented with additional logic to skip continuous joints from query as appendRobotStateJointStateAsFetchQueryWithToleranceSkipContinuous( )

and similarly appendRobotStateJointStateAsInsertMetadata() is copied and implemented ad appendRobotStateJointStateAsInsertMetadataSkipContinuous() to skip pushing continuous joints data to database.

and then made changes in motion_plan_request_features.cpp to call these functions instead of the original ones.

This setup works for me but is definately not the elegant solution. If this seems like a good thing to add I can put in some time to get it done in better way.

[sea-bass]

@methylDragon developed this feature, so curious what he thinks!

[methylDragon]

Yeah, this makes sense!

Though since this is a more specific case, I would recommend implementing it as a new FeaturesInterface implementation and pass the new feature (alongside the other non-robot state default ones) instead of the default ones when fetching. This will always be a correct approach.

Alternatively if we collectively think it's general enough, to save on some code copying, we could bubble it down as a bool/allowlist/denylist parameter for:

• the robot state features
• the default features for all existing cache insert policy default features
• some other place

If we go with the latter bool bubbling option (which will be a possibly correct, more efficient approach), let me know and I'll look into the specific places to inject. I'm far away from a laptop atm so it'll take some time.

PS: @robintomar1 @sea-bass there's still a pending tutorial PR that might help (;

[robintomar1]

@sea-bass In my opinion having an arm on wheeled base is quite common So we should add it as a boolean. Implementing it as a new featureInterface will not give us much benefit. @methylDragon I can look into how to inject this. Some pointers would be helpful to how to go about it.

[methylDragon]

@sea-bass In my opinion having an arm on wheeled base is quite common So we should add it as a boolean. Implementing it as a new featureInterface will not give us much benefit. @methylDragon I can look into how to inject this. Some pointers would be helpful to how to go about it.

Still away from a laptop. Best to do it as a (regex??) allowlist and denylist on fetch (loosening the key match constraints). Because there might be continuous joints that are not part of the mobile base in the general case, and we want to be able to handle those.

I'm a little bit more neutral on whether we should also filter out the joints that get added to the cache. The only thing that would do is save space and maybe add a footgun if people accidentally fetch a cached trajectory with a wider set of joints.

[robintomar1]

I'm a little bit more neutral on whether we should also filter out the joints that get added to the cache. The only thing that would do is save space and maybe add a footgun if people accidentally fetch a cached trajectory with a wider set of joints.

That makes sense actually, adding them to database should be fine because if we filter them out then we loose the flexibility to fetch with a wider set even if we want to. Basically If we keep them then we can use other featureInterface on the same database as I understand it.

[methylDragon]

I'm a little bit more neutral on whether we should also filter out the joints that get added to the cache. The only thing that would do is save space and maybe add a footgun if people accidentally fetch a cached trajectory with a wider set of joints.

That makes sense actually, adding them to database should be fine because if we filter them out then we loose the flexibility to fetch with a wider set even if we want to. Basically If we keep them then we can use other featureInterface on the same database as I understand it.

Apologies, I thought about it a little harder and I realized, you probably want to do the joint filtering (allowlist and denylist) on the put side as well.

This is because all joints under consideration will be used for cache pruning. If you don't, then you will clutter your cache space because your mobile base's joints will cause cache misses, which will prevent cache pruning from happening.

---

A more sustainable (but slightly more complicated) implementation would be to allow match tolerances to be configured on a per-joint basis instead of via allow/denylisting. That can also allow you to tune the pruning logic accordingly.

PS: @sea-bass , adding the joint filtering will add some extra overhead for cache puts and fetches which could affect people who want very performant cache operations. If that is an issue let me know, and I will recommend adding a new path (i.e., adding a new feature and cache insertion policy) instead of augmenting the current one

[sea-bass]

Hmm... I'm winding down my involvement in this project, but happy to share high level opinions.

1. Definitely cannot exclude all continuous joints: there are several arms with actual continuous joints, such as the Kinova Gen3 series which are currently featured in the tutorials.
2. Per-joint filtering seems fine, but perhaps even better (and consistent with MoveIt design IMO) is operating on specific joint groups. It means you have to define the groups properly in the SRDF, but I'm sure anyway this would need to be done for motion planning to work. Hopefully doing it this way means you can save on some of the predicted performance drops in doing things per-joint?
3. Even if performance is sacrificed, I think supporting a subset of joints in a robot model will cover many use cases beyond single arm, whether it's arm+base, dual arm, etc. So it seems worthwhile.

Ultimately, I defer to you @methylDragon as the developer of the package!

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.