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

Error: X and Y Components of Rotation2D are zero when following short on the fly curved path #984

Closed
WarrenReynolds opened this issue Jan 8, 2025 · 3 comments · Fixed by #985
Labels
bug Something isn't working PathPlannerLib Changes to PathPlannerLib
Milestone

Comments

@WarrenReynolds
Copy link
Contributor

Describe the bug
Running the on-the-fly path following code as per PathPlanner Sample Code on short curved path crashes code with X and Y Components of Rotation2D are zero error

To Reproduce
Steps to reproduce the behavior:
Hard code some offsets in the following piece of code in the robotContainer.cpp

 // Add a button to SmartDashboard that will create and follow an on-the-fly
  // path This example will simply move the robot 2m in the +X field direction
  m_onTheFly = frc2::cmd::RunOnce([this]() {
                 frc::Pose2d currentPose = this->m_drive.getPose();

                 // The rotation component in these poses represents the
                 // direction of travel
                 frc::Pose2d startPos =
                     frc::Pose2d(currentPose.Translation(), frc::Rotation2d());
                 frc::Pose2d endPos = frc::Pose2d(
                     currentPose.Translation() + frc::Translation2d(0.5_m, -0.4_m),
                     frc::Rotation2d());

                 std::vector<Waypoint> waypoints =
                     PathPlannerPath::waypointsFromPoses({startPos, endPos});
                 // Paths must be used as shared pointers
                 auto path = std::make_shared<PathPlannerPath>(
                     waypoints,
                     PathConstraints(4.0_mps, 4.0_mps_sq, 360_deg_per_s,
                                     540_deg_per_s_sq),
                     std::nullopt,  // Ideal starting state can be nullopt for
                                    // on-the-fly paths
                     GoalEndState(0_mps, currentPose.Rotation()));

                 // Prevent this path from being flipped on the red alliance,
                 // since the given positions are already correct
                 path->preventFlipping = true;

                 this->m_followOnTheFly =
                     AutoBuilder::followPath(path).Unwrap();
                 this->m_followOnTheFly->Schedule();
               }).Unwrap();
  frc::SmartDashboard::PutData("On-the-fly path", m_onTheFly.get());
}

If the end point is x+0.5,y+0 (ie Straight straight ahead 0.5 metres), it works fine.
If you change the end point to x+0.5,y-0.5 (ie Move ahead 0.5m and to the right 0.5m) it drives the path and the robot stays enabled, but the command never finishes, so you never regain control of the robot. Also smart dashboard locks up and the riolog has repeats of the X and Y Components of Rotation2D are zero error with the culprit being the same line each time
at pathplanner::PathPlannerTrajectory::generateStates(std::vector<pathplanner::PathPlannerTrajectoryState,..........

On some occasions the robot will continue to keep moving or rotating slowly after it finishes the S Path. You have to disable to stop the robot moving. If you re-enable, it will continue moving like it was before you disabled it. Restarting robot code is the only way to regain control.

I tried different y offsets and -0.3m works OK but -0.4 produces the above problems.

Expected behavior
Expected it work.

Screenshots
Copy of the top section of the Rio Log when it crashes

********** Robot program startup complete **********
[phoenix] Library initialization is complete.

[phoenix-diagnostics] Server 2025.1.0 (Jan 3 2025, 16:33:21) running on port: 1250
7433: -------Teleop just started--------------
Error: x and y components of Rotation2d are zero
at wpi::GetStackTrace[abi:cxx11](int) + 0x20 [0xb5f7374c]
at pathplanner::PathPlannerTrajectory::generateStates(std::vector<pathplanner::PathPlannerTrajectoryState, std::allocator<pathplanner::PathPlannerTrajectoryState> >&, std::shared_ptr<pathplanner::PathPlannerPath>, frc::Rotation2d const&, pathplanner::RobotConfig const&) + 0xbac [0xddb28]
at pathplanner::PathPlannerTrajectory::PathPlannerTrajectory(std::shared_ptr<pathplanner::PathPlannerPath>, frc::ChassisSpeeds const&, frc::Rotation2d const&, pathplanner::RobotConfig const&) + 0x94 [0xde99c]
at pathplanner::PathPlannerPath::generateTrajectory(frc::ChassisSpeeds, frc::Rotation2d, pathplanner::RobotConfig const&) + 0x220 [0xaa498]
at pathplanner::FollowPathCommand::Initialize() + 0xe74 [0xa866c]
at frc2::CommandScheduler::Schedule(frc2::Command*) + 0x434 [0xb6f08290]
at ) [0x5a888] +
at frc2::CommandScheduler::Schedule(frc2::Command*) + 0x434 [0xb6f08290]
at std::_Function_handler<void (nt::BooleanSubscriber&), frc::SendableBuilderImpl::AddPropertyImpl<nt::BooleanTopic, std::function<bool ()>, std::function<void (bool)> >(nt::BooleanTopic, std::function<bool ()>, std::function<void (bool)>)::{lambda(auto:1&)#2}>::_M_invoke(std::_Any_data const&, nt::BooleanSubscriber&) + 0x50 [0xb67a8988]
at frc::SendableBuilderImpl::PropertyImpl<nt::BooleanTopic>::Update(bool, long long) + 0x44 [0xb67a855c]
at frc::SendableBuilderImpl::Update() + 0x3c [0xb67a46c4]
at wpi::SendableRegistry::Update(unsigned int) + 0x58 [0xb5f6a8b4]
at frc::SmartDashboard::UpdateValues() + 0x38 [0xb6856a64]
at frc::IterativeRobotBase::LoopFunc() + 0x134 [0xb682c1c0]
at frc::TimedRobot::StartCompetition() + 0xfc [0xb6845298]
at void frc::impl::RunRobot<Robot>(wpi::priority_mutex&, Robot**) + 0x54 [0x55d84]
at int frc::StartRobot<Robot>() + 0x2c0 [0x573f8]
at __libc_start_main + 0x10c [0xb4624624]
Error at ReportErrorV: Error: x and y components of Rotation2d are zero
at wpi::GetStackTrace[abi:cxx11](int) + 0x20 [0xb5f7374c]

Versions: (please complete the following information):

  • OS: Windows 11
  • GUI Version: 2025.1.1
  • PPLib Version: 2025.1.1
  • PPLib Language: C++

Additional context
The 7433: -------Teleop just started-------------- line at the top of Rio Log is just a fmt::print in our code.
Our swerve code and odometry all works as expected in Teleop and for straight move forward 0.5m it moves to with 15mm. This can be repeated over and over and you can reverse back to where you started and run more move forwards+0.5 and it never fails. It just breaks if you move to the side as well.

@WarrenReynolds WarrenReynolds added the bug Something isn't working label Jan 8, 2025
@mjansen4857 mjansen4857 added the PathPlannerLib Changes to PathPlannerLib label Jan 8, 2025
@mjansen4857 mjansen4857 added this to the 2025.2 milestone Jan 8, 2025
@mjansen4857 mjansen4857 linked a pull request Jan 8, 2025 that will close this issue
@WarrenReynolds
Copy link
Contributor Author

@mjansen4857, I get that you don't want to create a new release for every merged bug fix, but do I have to wait for another versioned release to be created to use the code that corrects this issue?

I don't really know how the whole marvin/ vendor dependence thing works under the hood, but can I modify a local file in the project to point to somewhere else so that the PPLib from the latest build is accessed?

Or, if I want to do this before a new release is created, I need to manually download the PPLib.zip, extract it locally somewhere into my project, remove the vendor dependency from VS code so it doesn't build using the none bug fixed library code.

Then when a new release comes out with the bug fix incorporated, I can just delete the local copy of PPLib, re-install the vender dependency and continue on?

@mjansen4857
Copy link
Owner

This error shouldn't actually cause a crash, it's just a console print. So you can ignore it for now. 2025.2 will be out soon, probably tomorrow.

@WarrenReynolds
Copy link
Contributor Author

For whatever reason, when this "components are zero" error happens, it prevents the on the fly command from finishing and it locks up the smart dashboard so you can't cancel the on the fly command or do anything else. The only way to get control back is to redeploy code or restart robot code.

No dramas, I can just wait for the next release if it's coming out that soon. My mind is melting at the moment trying to figure out the defer command thing. I can't get my head around how defer prevents these lines

                 this->m_followOnTheFly =
                     AutoBuilder::followPath(path).Unwrap();
                 this->m_followOnTheFly->Schedule();

from requiring the drive subsystem when m_followOnTheFly is scheduled. Wont the scheduling of the command returned by Autobuilder::followPath(path) have a drive subsystem requirement inside it?
And hence when this command executes in pathplanner as part of the "named commands" it will cause an interrupt on the path following part of path planner?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working PathPlannerLib Changes to PathPlannerLib
Projects
None yet
Development

Successfully merging a pull request may close this issue.

2 participants