Robot motion profiler/path planner for use in FRC Java programming
License: MIT License
Third year Engineering Science student at UofT, Robotics Major (2T5).
Software Lead of the SAE Aero Design competition team at UTAT UAS (University of Toronto Aerospace Team, Unmanned Aerial Systems Division).
Ex-Software Lead for FRC Team Arctos 6135 from 2018-2020.
I've also made minor contributions to open source projects such as wpilibsuite/shuffleboard and naco-siren/vscode-gradle-language.
The number of arguments in the constructors of Followers are ridiculously high. For example, one constructor of DynamicTankDriveFollower has a whopping 14 arguments:
https://github.com/Arctos6135/RobotPathfinder/blob/3ab8a3ad5168152c4afad6c37336fad5fc3966dc/src/main/java/com/arctos6135/robotpathfinder/follower/DynamicTankDriveFollower.java#L216-L218
A simple class like TrajectoryParams should be implemented that combines some or all of these parameters into one object, to make the constructors seem cleaner, such as a Gains class to group all the PID gains into 1.
testBasicTrajectoryGenerationException has failed, resulting in a broken build!
Gradle build scan
Travis CI build
Unfortunately, since this failure occurred in a Travis build, there doesn't seem to be a way to recover the log files for TestHelper to replicate the failure... Perhaps add something in TestLogger that prints to stdout?
Implement an algorithm that numerically integrates the wheel movements to get the final robot position.
To reproduce:
{
"maxVelocity": 100.0,
"maxAcceleration": 4.0,
"basePlateWidth": 5.0,
"alpha": 4.0,
"segmentCount": 1000,
"tankDrive": false,
"pathType": "quinticHermite",
"waypoints": [
{
"x": 0.0,
"y": 0.0,
"heading": 0.0
},
{
"x": 6.0,
"y": 0.0,
"heading": 2.0943951023931953
},
{
"velocity": 5.0,
"x": -6.0,
"y": 8.0,
"heading": -1.0471975511965976
},
{
"velocity": -2.0,
"x": 0.0,
"y": 0.0,
"heading": 0.0
},
{
"x": 9.0,
"y": 9.0,
"heading": 0.15707963267948966
}
]
}Fix documentation broken by all the new refactoring done on the dev/main branch.
Fix inconsistent use of the word "distance" vs the word "position" in the API.
Using this code to repeatedly run TankDriveTrajectoryTest, the test usually fails after running less than 100 times:
int i = 0;
JUnitCore c = new JUnitCore();
Result r;
while((r = c.run(TankDriveTrajectoryTest.class)).wasSuccessful()) {
System.out.println(i++);
}
for(Failure f : r.getFailures()) {
System.out.println(f.toString());
f.getException().printStackTrace();
}Stack trace:
java.lang.IllegalStateException: This object has already been freed
at com.arctos6135.robotpathfinder.core.trajectory.TankDriveTrajectory.totalTime(Native Method)
at com.arctos6135.robotpathfinder.tests.core.trajectory.TankDriveTrajectoryTest.testTankDriveTrajectoryMultipleMirroring(TankDriveTrajectoryTest.java:222)
at jdk.internal.reflect.GeneratedMethodAccessor4.invoke(Unknown Source)
at java.base/jdk.internal.reflect.DelegatingMethodAccessorImpl.invoke(DelegatingMethodAccessorImpl.java:43)
at java.base/java.lang.reflect.Method.invoke(Method.java:566)
at org.junit.runners.model.FrameworkMethod$1.runReflectiveCall(FrameworkMethod.java:50)
at org.junit.internal.runners.model.ReflectiveCallable.run(ReflectiveCallable.java:12)
at org.junit.runners.model.FrameworkMethod.invokeExplosively(FrameworkMethod.java:47)
at org.junit.internal.runners.statements.InvokeMethod.evaluate(InvokeMethod.java:17)
at org.junit.rules.TestWatcher$1.evaluate(TestWatcher.java:55)
at org.junit.rules.RunRules.evaluate(RunRules.java:20)
at org.junit.runners.ParentRunner.runLeaf(ParentRunner.java:325)
at org.junit.runners.BlockJUnit4ClassRunner.runChild(BlockJUnit4ClassRunner.java:78)
at org.junit.runners.BlockJUnit4ClassRunner.runChild(BlockJUnit4ClassRunner.java:57)
at org.junit.runners.ParentRunner$3.run(ParentRunner.java:290)
at org.junit.runners.ParentRunner$1.schedule(ParentRunner.java:71)
at org.junit.runners.ParentRunner.runChildren(ParentRunner.java:288)
at org.junit.runners.ParentRunner.access$000(ParentRunner.java:58)
at org.junit.runners.ParentRunner$2.evaluate(ParentRunner.java:268)
at org.junit.runners.ParentRunner.run(ParentRunner.java:363)
at org.junit.runners.Suite.runChild(Suite.java:128)
at org.junit.runners.Suite.runChild(Suite.java:27)
at org.junit.runners.ParentRunner$3.run(ParentRunner.java:290)
at org.junit.runners.ParentRunner$1.schedule(ParentRunner.java:71)
at org.junit.runners.ParentRunner.runChildren(ParentRunner.java:288)
at org.junit.runners.ParentRunner.access$000(ParentRunner.java:58)
at org.junit.runners.ParentRunner$2.evaluate(ParentRunner.java:268)
at org.junit.runners.ParentRunner.run(ParentRunner.java:363)
at org.junit.runner.JUnitCore.run(JUnitCore.java:137)
at org.junit.runner.JUnitCore.run(JUnitCore.java:115)
at org.junit.runner.JUnitCore.run(JUnitCore.java:105)
at org.junit.runner.JUnitCore.run(JUnitCore.java:94)
at DebugTests.main(DebugTests.java:97)
This seems to be caused by a bug in the GlobalLifeCycleManager, since if the resource disposal threads are turned off, this problem doesn't seem to occur.
Add followers that can regenerate simple motion profiles on-the-fly so that they're more accurate.
The order of Javadoc tags is sometimes inconsistent throughout the project. The order of tags should always follow the standard. Use this checkstyle rule to check for Javadoc tag order.
Based on #39, implement complex dynamic trajectories that can re-generate paths and trajectories on-the-fly.
The acceleration of the outside wheel at a turn in TankDriveTrajectory breaks the acceleration limit.
There should be some simple unit tests for all methods of Grapher. The output doesn't need to be verified (since it's graphical), but there should at least be tests to make sure no exceptions are thrown, etc.
Log:
D:\Arctos 6135\Projects\RobotPathfinder>gradlew build
Starting a Gradle Daemon, 2 incompatible and 1 stopped Daemons could not be reused, use --status for details
> Configure project :
Detected architeture: amd64
Detected OS: Windows
Detected architeture: amd64
Detected OS: Windows
INFO: Could not find files for the given pattern(s).
> Task :copyLibWindowsx8664Release
1 file(s) copied.
> Task :test FAILED
com.arctos6135.robotpathfinder.tests.BasicTest > testPathIllegalStateException STANDARD_OUT
Trying to find dynamic library on Java library path...
com.arctos6135.robotpathfinder.tests.BasicTest > testPathIllegalStateException STANDARD_ERROR
Warning: RobotPathfinder dynamic library not found in library path. Searching working directory...
com.arctos6135.robotpathfinder.tests.BasicTest > testPathIllegalStateException STANDARD_OUT
Attempting to load D:\Arctos 6135\Projects\RobotPathfinder\RobotPathfinder.dll
Library loaded successfully.
com.arctos6135.robotpathfinder.tests.BasicTest > testPathIllegalStateException PASSED
com.arctos6135.robotpathfinder.tests.BasicTest > testPathAllMethods PASSED
com.arctos6135.robotpathfinder.tests.BasicTest > testBasicTrajectoryAllMethods PASSED
com.arctos6135.robotpathfinder.tests.BasicTest > testTankDriveTrajectoryIllegalStateException PASSED
com.arctos6135.robotpathfinder.tests.BasicTest > testBasicTrajectoryIllegalStateException PASSED
com.arctos6135.robotpathfinder.tests.BasicTest > testTankDriveTrajectoryAllMethods PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalRotationTankDriveProfileBasicReversed STANDARD_OUT
[INFO] maxV: 577.6985548834648
[INFO] maxA: 316.43276766640673
[INFO] angle: -0.7126924452684626
[INFO] baseWidth: 790.8902997727864
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalRotationTankDriveProfileBasicReversed PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalBasicProfileBasicReversed STANDARD_OUT [INFO] maxV: 759.8154291360925
[INFO] maxA: 810.9247415300326
[INFO] distance: -478.30158769854336
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalBasicProfileBasicReversed PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalRotationTankDriveProfileAdvanced STANDARD_OUT
[INFO] maxV: 134.2927487684219
[INFO] maxA: 654.4407868449796
[INFO] angle: 0.5400194780178694
[INFO] baseWidth: 364.2173222241877
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalRotationTankDriveProfileAdvanced PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalTankDriveProfileAdvanced STANDARD_OUT
[INFO] maxV: 272.32302602722814
[INFO] maxA: 61.46617940081644
[INFO] distance: 325.7206322129882
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalTankDriveProfileAdvanced PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalRotationTankDriveProfileBasic STANDARD_OUT
[INFO] maxV: 658.2146417463606
[INFO] maxA: 780.4243509113413
[INFO] angle: 0.5048547423573325
[INFO] baseWidth: 871.7674792877663
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalRotationTankDriveProfileBasic PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalTankDriveProfileAdvancedReversed STANDARD_OUT
[INFO] maxV: 191.0572200298123
[INFO] maxA: 168.88589623083072
[INFO] distance: -631.8965032677423
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalTankDriveProfileAdvancedReversed PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalBasicProfileAdvancedReversed STANDARD_OUT
[INFO] maxV: 408.2328349502934
[INFO] maxA: 736.7973506382554
[INFO] distance: -615.6248861477911
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalBasicProfileAdvancedReversed PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalRotationTankDriveProfileAdvancedReversed STANDARD_OUT
[INFO] maxV: 319.040776066411
[INFO] maxA: 228.40220740492413
[INFO] angle: -2.262337865956286
[INFO] baseWidth: 397.09227322385334
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalRotationTankDriveProfileAdvancedReversed PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalTankDriveProfileBasicReversed STANDARD_OUT
[INFO] maxV: 78.01730764001636
[INFO] maxA: 722.8117752637526
[INFO] distance: -216.50118401032537
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalTankDriveProfileBasicReversed PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalTankDriveProfileBasic STANDARD_OUT
[INFO] maxV: 832.0823182280303
[INFO] maxA: 645.1620742633091
[INFO] distance: 113.02884971426718
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalTankDriveProfileBasic PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalBasicProfileAdvanced STANDARD_OUT
[INFO] maxV: 133.14181312807892
[INFO] maxA: 690.128086830791
[INFO] distance: 901.306544099477
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalBasicProfileAdvanced PASSED
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalBasicProfileBasic STANDARD_OUT
[INFO] maxV: 668.3828895829103
[INFO] maxA: 607.8192211117573
[INFO] distance: 581.9504980773568
com.arctos6135.robotpathfinder.tests.FollowableMotionProfileTest > testTrapezoidalBasicProfileBasic PASSED
com.arctos6135.robotpathfinder.tests.FollowerTest > testTankDriveFollowerBasic PASSED
com.arctos6135.robotpathfinder.tests.FollowerTest > testTankDriveFollowerState PASSED
com.arctos6135.robotpathfinder.tests.MotionProfileTest > testTrapezoidalMotionProfileBasic STANDARD_OUT
[INFO] maxV: 314.92868636044426
[INFO] maxA: 881.5725473129869
[INFO] distance: 37.394185352983776
com.arctos6135.robotpathfinder.tests.MotionProfileTest > testTrapezoidalMotionProfileBasic PASSED
com.arctos6135.robotpathfinder.tests.MotionProfileTest > testTrapezoidalMotionProfileBasicReversed STANDARD_OUT
[INFO] maxV: 669.5363231181776
[INFO] maxA: 557.1171758103889
[INFO] distance: -249.9877122042522
com.arctos6135.robotpathfinder.tests.MotionProfileTest > testTrapezoidalMotionProfileBasicReversed PASSED
com.arctos6135.robotpathfinder.tests.MotionProfileTest > testTrapezoidalMotionProfileAdvancedReversed STANDARD_OUT
[INFO] maxV: 85.48278607323012
[INFO] maxA: 786.7552382177757
[INFO] distance: -32.83271606412042
[thread 2548 also had an error]
#
# A fatal error has been detected by the Java Runtime Environment:
#
# EXCEPTION_ACCESS_VIOLATION (0xc0000005) at pc=0x00007ffe732a6b30, pid=13588, tid=14268
#
# JRE version: Java(TM) SE Runtime Environment (11.0.1+13) (build 11.0.1+13-LTS)
# Java VM: Java HotSpot(TM) 64-Bit Server VM (11.0.1+13-LTS, mixed mode, tiered, compressed oops, g1 gc, windows-amd64)
# Problematic frame:
# C
com.arctos6135.robotpathfinder.tests.MotionProfileTest > testTrapezoidalMotionProfileAdvancedReversed PASSED
com.arctos6135.robotpathfinder.tests.MotionProfileTest > testTrapezoidalMotionProfileAdvanced STANDARD_OUT
[INFO] maxV: 662.7875417074015
[INFO] maxA: 622.7446966367266
[INFO] distance: 498.6961490461723
[RobotPathfinder.dll+0x6b30]
#
# No core dump will be written. Minidumps are not enabled by default on client versions of Windows
#
# An error report file with more information is saved as:
# D:\Arctos 6135\Projects\RobotPathfinder\hs_err_pid13588.log
com.arctos6135.robotpathfinder.tests.MotionProfileTest > testTrapezoidalMotionProfileAdvanced PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryGeneratorTest > testGenerateRotationTank PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryGeneratorTest > testGenerateStraightTank PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryGeneratorTest > testGenerateStraightBasic PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testTankDriveTrajectoryMirrorLeftRight PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testBasicTrajectoryMirrorLeftRight PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testBasicTrajectoryMultipleMirroring PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testVelocityLimitTank PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testTankDriveTrajectoryMultipleMirroring PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testTankDriveTrajectoryGenerationException PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testVelocityAndAccelerationLimitBasic PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testBeginningAndEndWaypointEx PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testTankDriveTrajectoryRetrace PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testTankDriveTrajectoryMirrorFrontBack PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testBasicTrajectoryMirrorFrontBack PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testBasicTrajectoryRetrace PASSED
com.arctos6135.robotpathfinder.tests.TrajectoryTest > testBasicTrajectoryGenerationException PASSED
#
# If you would like to submit a bug report, please visit:
# http://bugreport.java.com/bugreport/crash.jsp
# The crash happened outside the Java Virtual Machine in native code.
# See problematic frame for where to report the bug.
#
[error occurred during error reporting (), id 0xc0000005, EXCEPTION_ACCESS_VIOLATION (0xc0000005) at pc=0x00007ffe732aebd1]
FAILURE: Build failed with an exception.
* What went wrong:
Execution failed for task ':test'.
> Process 'Gradle Test Executor 1' finished with non-zero exit value 1
This problem might be caused by incorrect test process configuration.
Please refer to the test execution section in the user guide at https://docs.gradle.org/5.0/userguide/java_testing.html#sec:test_execution
* Try:
Run with --stacktrace option to get the stack trace. Run with --info or --debug option to get more log output. Run with --scan to get full insights.
* Get more help at https://help.gradle.org
BUILD FAILED in 43s
16 actionable tasks: 9 executed, 7 up-to-date
Devise and implement an algorithm that can re-generate a smooth path, preferably similar to the original path, if given the current robot location and the goal location.
Because of the update() methods, a DynamicFollowable may be modified when it is followed. This should be stated in the documentation. All DynamicFollowables should also provide a copy() method to produce an exact copy of itself, so that the user is able to use the same DynamicFollowable multiple times.
Currently, only MotionProfileTest and FollowableMotionProfileTest tests use randomized inputs. Other tests use the same numbers every time. All the old unit tests should be updated to use randomized inputs to maintain consistency and increase effectiveness.
They're cool so why not.
Devise an algorithm that takes individual wheel movements at points in time and calculate the robot's path from the data in order to know the robot's position.
Gradle did not include JMathPlot jars in the Trajectory Visualizer, so it is missing dependencies and thus cannot generate graphs.
It might be beneficial to provide classes that allow the trajectory following control loop to run independently of the WPILib Command-based framework and its 50Hz frequency constraint. This also means that if for some reason another command took too long to execute, the control loop would not be slowed down.
Add complex trajectories (TankDriveTrajectorys and BasicTrajectorys) that can be re-generated/updated on-the-fly to improve accuracy.
When BezierPath.integrateWheelLens(0) is called on a new object, it returns 1.0 instead of the expected 0.
Calling BezierPath.resetWheelIntegration() before integration fixes the problem, however if this method is called within the constructor it still does not work. Currently an if statement is added in the code to automatically reset.
If a path has a very sharp bend, it may cause the wheels on one side to have to go in reverse to accommodate for the turn:
However, in this situation, the trajectory generation fails. Possibly related to how path length integration is always increasing but velocity can be negative.
Add an optimized trapezoidal or s-curve motion profile that doesn't use numerical integration and its corresponding follower.
When using dynamic motion profile re-generation with TrapezoidalTankDriveProfiles, an IllegalArgumentException is thrown. Full stack trace:
[112.38] ERROR 1 Unhandled exception: java.lang.IllegalArgumentException: Time out of range (0.786478 ? [113.037009, 2.430556])!
com.arctos6135.robotpathfinder.motionprofile.TrapezoidalMotionProfile.position(TrapezoidalMotionProfile.java:169)
at com.arctos6135.robotpathfinder.motionprofile.TrapezoidalMotionProfile.position(TrapezoidalMotionProfile.java:169)
at com.arctos6135.robotpathfinder.motionprofile.DualMotionProfile.leftPosition(DualMotionProfile.java:68)
at com.arctos6135.robotpathfinder.motionprofile.followable.TankDriveFollowableMotionProfile.get(TankDriveFollowableMotionProfile.java:40)
at com.arctos6135.robotpathfinder.motionprofile.followable.TankDriveFollowableMotionProfile.get(TankDriveFollowableMotionProfile.java:21)
at com.arctos6135.robotpathfinder.follower.DynamicTankDriveFollower._run(DynamicTankDriveFollower.java:482)
at com.arctos6135.robotpathfinder.follower.Follower.run(Follower.java:248)
at com.arctos6135.robotpathfinder.follower.DynamicFollower.run(DynamicFollower.java:74)
at frc.robot.commands.FollowTrajectory.execute(FollowTrajectory.java:175)
at frc.robot.commands.DriveDistance.execute(DriveDistance.java:46)
at edu.wpi.first.wpilibj.command.Command.run(Command.java:292)
at edu.wpi.first.wpilibj.command.CommandGroup._execute(CommandGroup.java:234)
at edu.wpi.first.wpilibj.command.Command.run(Command.java:291)
at edu.wpi.first.wpilibj.command.Scheduler.run(Scheduler.java:224)
at frc.robot.Robot.teleopPeriodic(Robot.java:427)
at edu.wpi.first.wpilibj.IterativeRobotBase.loopFunc(IterativeRobotBase.java:240)
at edu.wpi.first.wpilibj.TimedRobot.startCompetition(TimedRobot.java:81)
at edu.wpi.first.wpilibj.RobotBase.startRobot(RobotBase.java:263)
at frc.robot.Main.main(Main.java:27)
Many files are indented with spaces or have mixed indents. Fix with (fish shell):
for a in (find src -type f)
unexpand -t 4 --first-only $a > $a"-tabs"
mv $a"-tabs" $a
endBug here ->
https://github.com/Arctos6135/RobotPathfinder/blob/96df103055bef121836756849ea90551851c4b03/src/main/cpp/include/math/mat.h#L46
should test if j == m instead of i == m since j is the counter variable.
Test RobotPathfinder with different compile flags (e.g. -O2 vs -O3) to determine what works the best.
rpf::rangle() and rpf::langle() could use much better algorithms than the ones currently used.
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
Django
The Web framework for perfectionists with deadlines.
Laravel
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
Microsoft
Open source projects and samples from Microsoft.
Google
Google ❤️ Open Source for everyone.
Alibaba
Alibaba Open Source for everyone
D3
Data-Driven Documents codes.
Tencent
China tencent open source team.