Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add a getter to be able to access the trajectory generated by a PathfindingCommand(holonomic #532

Closed
Ramtech59Sensei opened this issue Dec 30, 2023 · 10 comments
Labels
enhancement New feature or request

Comments

@Ramtech59Sensei
Copy link

pathfindingCommand has its currentTrajectory as private (I like it because nothing can change it ) .
Creating a getter to access the trajectory generated before the command is executing will be helpful visualization on initial test and during competition.
Your wiki show the path generated and you visualized it in the image but I cant find a way to access it , One possible way can be to access it in PathFinding.getCurrentPath() but that gives me the latest path calculated and it may not be the correct trajectory .

@Ramtech59Sensei Ramtech59Sensei added the enhancement New feature or request label Dec 30, 2023
@mjansen4857
Copy link
Owner

@Ramtech59Sensei
Copy link
Author

Is that when the Path is running , how can access is before it run ?
Let say I want to generate the path but not run it, still I can get the trajectory ?

@Ramtech59Sensei
Copy link
Author

I am able to see the poses with the link you send when I run autonomous but not before I run it

@mjansen4857
Copy link
Owner

There is no path generated until it runs

@Ramtech59Sensei
Copy link
Author

Ramtech59Sensei commented Dec 30, 2023

Is the same way for AutoBuilder .followPathWithEvents?
I know we have the path point but can we access the Trajectory to be display on the field2d before run auto?
let say the drivers pick an auto path and I want to show what the robot will do before Auto start
if they pick the wrong auto , they can see and correct the error before they FMS start match

@Ramtech59Sensei
Copy link
Author

I see there is a getTrajectory from Path object , I may be able to use it

@mjansen4857
Copy link
Owner

You don't even need that. Just get all the PathPoints from the path with the getAllPathPoints() method. Then create a list of poses from each PathPoint's position to put on a Field2d.

@Ramtech59Sensei
Copy link
Author

Ramtech59Sensei commented Dec 30, 2023

OK , thanks
I was already doing a conversion function from PathPlannerTrajctory.State to WPI Trajectory.State because I realize the trajectories are not compatible any more
I will use the function you say

@Ramtech59Sensei
Copy link
Author

you can close this one If you want.
I test the conversion function from state to state, create a trajectory and it work, also should work creating a list of pose2d

@Ramtech59Sensei
Copy link
Author

Ramtech59Sensei commented Dec 30, 2023

this is what I did if anyone need it

import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.path.PathPlannerTrajectory;
import edu.wpi.first.math.trajectory.Trajectory;

PathPlannerPath path;
Trajectory pathTrajectory;
path = PathPlannerPath.fromPathFile("Example Path");
PathPlannerTrajectory pathPlanerTrajectory = path.getTrajectory(
                                                          new ChassisSpeeds(0.0,0.0,0.0),
                                                          path.getPreviewStartingHolonomicPose().getRotation());
pathTrajectory= getStatesToTrajectory(pathPlanerTrajectory.getStates());
    

  public Trajectory getStatesToTrajectory(List<PathPlannerTrajectory.State> states){
    List<Trajectory.State> trajStates = new ArrayList<>();
    Trajectory traj = new Trajectory ();
    if (!states.isEmpty(){
      for(PathPlannerTrajectory.State state : states){
        Trajectory.State trajState = new Trajectory.State(
          state.timeSeconds,
          state.velocityMps,
          state.accelerationMpsSq,
          new Pose2d(state.positionMeters,state.heading),
          state.curvatureRadPerMeter);
            trajStates.add(trajState);
      }
      traj = new Trajectory(trajStates);
    }
    return traj;
  }

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
enhancement New feature or request
Projects
None yet
Development

No branches or pull requests

2 participants