Executing robot actions while trajectory following

Technical Technical Discussion
1

Hey all, we are using some form of trajectory following this year (most likely path planner Introducing PathPlanner 2.0 ) and we are trying to build our 1\2\4\5 ball auto. With these tools the path planning is easy. Integration into your robot workflow seems to be a little more difficult.

As it stands today we are time-stamping the data and choosing to intake or shoot at set timestamps (after all trajectory following is timestamp-based anyway)

But this seems fishy, and annoying to adjust if we change the speed \ acceleration of the trajectory.

Has anyone found a better approach?

1 Like
2

Off the top of my head:

  • Using a command group, run the trajectory in a command parallel to the series of other commands you want to execute. Add a timeout to each of the other commands so they start or stop at the specified times.

Does anyone know if a trajectory can end with a non-zero velocity? That might provide alternative solutions as well.

3 Likes
3

Yeah that’s what we are doing today. Running our actions parallel to the path execution code.

But all those timeouts would need to be adjusted if the trajectory was sped up or slowed down.

4

Could you monitor the encoders and gyro values during the path and use those to trigger an event?

1 Like
5

I believe that would happen regardless of what method you use if you used time stamping.

I was hoping to find a way to start/end a (WPILIB) trajectory at a non-zero speed. That way, you could make the command waypoint dependent. I don’t currently see a way to do that however.

6

Yes definitely, and this is a great idea… I’m afraid of the plethora of possibilities that I miss the cargo or something gets stuck in our indexer.

7

This is why I was hoping the currently popular trajectory following solutions would give us an “AT WAYPOINT” variable

1 Like
8

You could try computing the distance between a desired waypoint/Pose2d location and your current odometer. If the distance is below a certain value, trigger your “at waypoint” code.

You would probably want a “NEAR WAYPOINT” variable (within some tolerance) from the trajectory library.

EDIT: Untested, but something like this:

private boolean isNearWaypoint(Pose2d waypoint, double within) {
  Pose2d youAreHere = m_drivetrain.getPose();
  double distance = youAreHere.getTranslation().getDistance(waypoint.getTranslation());
  return (distance <= within);
}
5 Likes
9

I think you’re on top something! Thanks for spelling this out, perhaps others have been saying this as well, but what clicked for me is doing this by distance traveled from waypoint to way point!

3 Likes
10

This could be used very nicely in conjunction to the WaitUntilCommand. It would lead to the code staying pretty clean overall.

image
image944Ă—476 27.1 KB

3 Likes
11

Good idea. I was hoping there was a way to insert a lambda/callback at each pose to kick off a command sequence.

12

Just be careful about the “nearWaypoint” test. If I understand it correctly, the path following does not need to actually go through the waypoints. So, you risk having the robot drive past the point at a distance larger than you threshold, and it will never trigger.

A safer, but distinctly more complicated, test would be look for getting closer to the waypoint and then further away. Once it is definitely getting farther from the waypoint, you know to start the command.

OTOH, maybe the path is more reliable and does not need to be that complicated. Your call.

1 Like
13

You can bind listeners to state changes on arbitrary boolean streams with Trigger.

1 Like
14

After some deliberation, we decided to use @veg 's response as a basis for our “path listener”. Although, we may never actually hit the waypoint, the distance between the two waypoints should be really close to reality (we’re not using distance traveled–strictly as the crow flies distance)

This is a good backup plan if our initial plan doesn’t work.

15

I’m reading through the code… trying to figure out how I query the current state of the trajectory to do that… :slight_smile: Any hints would be appreciated.

16 
new Trigger(drive::atFooWaypoint).whenActive(new FooCommand());
1 Like
17

Where drive::atFooWaypoint is querying the SwerveDriveOdometry to check for the Waypoint?

18

Or some other similar thing, yeah. The specific implementation doesn’t matter much.

1 Like
19

Basically, creating “hot spots” on the field that trigger command sequences during the trajectory. Like it!

20

You might want to do something to make sure you don’t accidentally leave an active listener on during teleop, though.

2 Likes