├── manifest.mf ├── .gitignore ├── README.md ├── src └── com │ └── team254 │ └── lib │ ├── trajectory │ ├── io │ │ ├── IPathSerializer.java │ │ ├── IPathDeserializer.java │ │ ├── TextFileSerializer.java │ │ ├── JavaStringSerializer.java │ │ ├── JavaSerializer.java │ │ └── TextFileDeserializer.java │ ├── Path.java │ ├── WaypointSequence.java │ ├── TrajectoryFollower.java │ ├── Trajectory.java │ ├── PathGenerator.java │ ├── Main.java │ ├── Spline.java │ └── TrajectoryGenerator.java │ └── util │ └── ChezyMath.java ├── nbproject ├── genfiles.properties ├── project.xml ├── project.properties └── build-impl.xml └── test └── com └── team254 └── lib └── trajectory ├── TrajectoryLibTestSuite.java ├── TrajectoryGeneratorTest.java ├── io └── SerializationDeserializationTest.java ├── SplineTest.java └── PathGeneratorTest.java /manifest.mf: -------------------------------------------------------------------------------- 1 | Manifest-Version: 1.0 2 | X-COMMENT: Main-Class will be added automatically by build 3 | 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.class 2 | 3 | # Package Files # 4 | *.jar 5 | *.war 6 | *.ear 7 | /nbproject/private/ 8 | /build/ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | TrajectoryLib 2 | ============= 3 | 4 | This code is used to generate Trajectories for the FRC254 robot to follow during Autonomous mode. Trajectories are calculated offboard on a laptop then deployed to the robot with the main robot JAR. 5 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/io/IPathSerializer.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.io; 2 | 3 | import com.team254.lib.trajectory.Path; 4 | 5 | /** 6 | * Interface for methods that serialize a Path or Trajectory. 7 | * 8 | * @author Jared341 9 | */ 10 | public interface IPathSerializer { 11 | 12 | public String serialize(Path path); 13 | } 14 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/io/IPathDeserializer.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.io; 2 | 3 | import com.team254.lib.trajectory.Path; 4 | 5 | /** 6 | * Interface for methods that deserializes a Path or Trajectory. 7 | * 8 | * @author Jared341 9 | */ 10 | public interface IPathDeserializer { 11 | 12 | public Path deserialize(String serialized); 13 | } 14 | -------------------------------------------------------------------------------- /nbproject/genfiles.properties: -------------------------------------------------------------------------------- 1 | build.xml.data.CRC32=572cf416 2 | build.xml.script.CRC32=bf71286b 3 | build.xml.stylesheet.CRC32=8064a381@1.68.1.46 4 | # This file is used by a NetBeans-based IDE to track changes in generated files such as build-impl.xml. 5 | # Do not edit this file. You may delete it but then the IDE will never regenerate such files for you. 6 | nbproject/build-impl.xml.data.CRC32=572cf416 7 | nbproject/build-impl.xml.script.CRC32=fe6576db 8 | nbproject/build-impl.xml.stylesheet.CRC32=5a01deb7@1.68.1.46 9 | -------------------------------------------------------------------------------- /nbproject/project.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | org.netbeans.modules.java.j2seproject 4 | 5 | 6 | TrajectoryLib 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /test/com/team254/lib/trajectory/TrajectoryLibTestSuite.java: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this license header, choose License Headers in Project Properties. 3 | * To change this template file, choose Tools | Templates 4 | * and open the template in the editor. 5 | */ 6 | package com.team254.lib.trajectory; 7 | 8 | import org.junit.After; 9 | import org.junit.AfterClass; 10 | import org.junit.Before; 11 | import org.junit.BeforeClass; 12 | import org.junit.runner.RunWith; 13 | import org.junit.runners.Suite; 14 | 15 | /** 16 | * Test suite. 17 | * 18 | * @author Jared341 19 | */ 20 | @RunWith(Suite.class) 21 | @Suite.SuiteClasses({ 22 | com.team254.lib.trajectory.TrajectoryGeneratorTest.class, 23 | com.team254.lib.trajectory.SplineTest.class, 24 | com.team254.lib.trajectory.PathGeneratorTest.class, 25 | com.team254.lib.trajectory.io.SerializationDeserializationTest.class}) 26 | public class TrajectoryLibTestSuite { 27 | 28 | @BeforeClass 29 | public static void setUpClass() throws Exception { 30 | } 31 | 32 | @AfterClass 33 | public static void tearDownClass() throws Exception { 34 | } 35 | 36 | @Before 37 | public void setUp() throws Exception { 38 | } 39 | 40 | @After 41 | public void tearDown() throws Exception { 42 | } 43 | 44 | } 45 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/Path.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.trajectory.Trajectory.Segment; 4 | 5 | /** 6 | * Base class for an autonomous path. 7 | * 8 | * @author Jared341 9 | */ 10 | public class Path { 11 | protected Trajectory.Pair go_left_pair_; 12 | protected String name_; 13 | protected boolean go_left_; 14 | 15 | public Path(String name, Trajectory.Pair go_left_pair) { 16 | name_ = name; 17 | go_left_pair_ = go_left_pair; 18 | go_left_ = true; 19 | } 20 | 21 | public Path() { 22 | 23 | } 24 | 25 | public String getName() { return name_; } 26 | 27 | public void goLeft() { 28 | go_left_ = true; 29 | go_left_pair_.left.setInvertedY(false); 30 | go_left_pair_.right.setInvertedY(false); 31 | } 32 | 33 | public void goRight() { 34 | go_left_ = false; 35 | go_left_pair_.left.setInvertedY(true); 36 | go_left_pair_.right.setInvertedY(true); 37 | } 38 | 39 | public Trajectory getLeftWheelTrajectory() { 40 | return (go_left_ ? go_left_pair_.left : go_left_pair_.right); 41 | } 42 | 43 | public Trajectory getRightWheelTrajectory() { 44 | return (go_left_ ? go_left_pair_.right : go_left_pair_.left); 45 | } 46 | 47 | public Trajectory.Pair getPair() { 48 | return go_left_pair_; 49 | } 50 | 51 | public double getEndHeading() { 52 | int numSegments = getLeftWheelTrajectory().getNumSegments(); 53 | Trajectory.Segment lastSegment = getLeftWheelTrajectory().getSegment(numSegments - 1); 54 | return lastSegment.heading; 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/io/TextFileSerializer.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.io; 2 | 3 | import com.team254.lib.trajectory.Trajectory; 4 | import com.team254.lib.trajectory.Trajectory.Segment; 5 | import com.team254.lib.trajectory.Path; 6 | 7 | /** 8 | * Serializes a Path to a simple space and CR separated text file. 9 | * 10 | * @author Jared341 11 | */ 12 | public class TextFileSerializer implements IPathSerializer { 13 | 14 | /** 15 | * Format: 16 | * PathName 17 | * NumSegments 18 | * LeftSegment1 19 | * ... 20 | * LeftSegmentN 21 | * RightSegment1 22 | * ... 23 | * RightSegmentN 24 | * 25 | * Each segment is in the format: 26 | * pos vel acc jerk heading dt x y 27 | * 28 | * @param path The path to serialize. 29 | * @return A string representation. 30 | */ 31 | public String serialize(Path path) { 32 | String content = path.getName() + "\n"; 33 | path.goLeft(); 34 | content += path.getLeftWheelTrajectory().getNumSegments() + "\n"; 35 | content += serializeTrajectory(path.getLeftWheelTrajectory()); 36 | content += serializeTrajectory(path.getRightWheelTrajectory()); 37 | return content; 38 | } 39 | 40 | private String serializeTrajectory(Trajectory trajectory) { 41 | String content = ""; 42 | for (int i = 0; i < trajectory.getNumSegments(); ++i) { 43 | Segment segment = trajectory.getSegment(i); 44 | content += String.format( 45 | "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", 46 | segment.pos, segment.vel, segment.acc, segment.jerk, 47 | segment.heading, segment.dt, segment.x, segment.y); 48 | } 49 | return content; 50 | } 51 | 52 | } 53 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/io/JavaStringSerializer.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.io; 2 | 3 | import com.team254.lib.trajectory.Path; 4 | import java.util.StringTokenizer; 5 | 6 | /** 7 | * Save to a Java class with a static string, because J2ME has problems with 8 | * large arrays. 9 | * 10 | * @author Jared341 11 | */ 12 | public class JavaStringSerializer implements IPathSerializer { 13 | 14 | public String serialize(Path path) { 15 | String contents = "package com.team254.frc2014.paths;\n\n"; 16 | contents += "import com.team254.lib.trajectory.Trajectory;\n"; 17 | contents += "import com.team254.lib.trajectory.io.TextFileDeserializer;\n"; 18 | contents += "import com.team254.path.Path;\n\n"; 19 | contents += "public class " + path.getName() + " extends Path {\n"; 20 | 21 | TextFileSerializer serializer = new TextFileSerializer(); 22 | String serialized = serializer.serialize(path); 23 | 24 | // J2ME can't parse multi line string literals. 25 | StringTokenizer tokenizer = new StringTokenizer(serialized, "\n"); 26 | contents += " private static final String kSerialized = \"" + 27 | tokenizer.nextToken() + "\\n\"\n"; 28 | while (tokenizer.hasMoreTokens()) { 29 | contents += " + \"" + tokenizer.nextToken() + "\\n\""; 30 | if (tokenizer.hasMoreTokens()) { 31 | contents += "\n"; 32 | } else { 33 | contents += ";\n\n"; 34 | } 35 | } 36 | 37 | contents += " public " + path.getName() + "() {\n"; 38 | contents += " TextFileDeserializer d = new TextFileDeserializer();\n"; 39 | contents += " Path p = d.deserialize(kSerialized);\n"; 40 | contents += " this.name_ = p.getName();\n"; 41 | contents += " this.go_left_pair_ = p.getPair();\n"; 42 | contents += " }\n\n"; 43 | 44 | contents += "}\n"; 45 | return contents; 46 | } 47 | 48 | } 49 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/WaypointSequence.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.util.ChezyMath; 4 | 5 | /** 6 | * A WaypointSequence is a sequence of Waypoints. #whatdidyouexpect 7 | * 8 | * @author Art Kalb 9 | * @author Stephen Pinkerton 10 | * @author Jared341 11 | */ 12 | public class WaypointSequence { 13 | 14 | public static class Waypoint { 15 | 16 | public Waypoint(double x, double y, double theta) { 17 | this.x = x; 18 | this.y = y; 19 | this.theta = theta; 20 | } 21 | 22 | public Waypoint(Waypoint tocopy) { 23 | this.x = tocopy.x; 24 | this.y = tocopy.y; 25 | this.theta = tocopy.theta; 26 | } 27 | 28 | public double x; 29 | public double y; 30 | public double theta; 31 | } 32 | 33 | Waypoint[] waypoints_; 34 | int num_waypoints_; 35 | 36 | public WaypointSequence(int max_size) { 37 | waypoints_ = new Waypoint[max_size]; 38 | } 39 | 40 | public void addWaypoint(Waypoint w) { 41 | if (num_waypoints_ < waypoints_.length) { 42 | waypoints_[num_waypoints_] = w; 43 | ++num_waypoints_; 44 | } 45 | } 46 | 47 | public int getNumWaypoints() { 48 | return num_waypoints_; 49 | } 50 | 51 | public Waypoint getWaypoint(int index) { 52 | if (index >= 0 && index < getNumWaypoints()) { 53 | return waypoints_[index]; 54 | } else { 55 | return null; 56 | } 57 | } 58 | 59 | public WaypointSequence invertY() { 60 | WaypointSequence inverted = new WaypointSequence(waypoints_.length); 61 | inverted.num_waypoints_ = num_waypoints_; 62 | for (int i = 0; i < num_waypoints_; ++i) { 63 | inverted.waypoints_[i] = waypoints_[i]; 64 | inverted.waypoints_[i].y *= -1; 65 | inverted.waypoints_[i].theta = ChezyMath.boundAngle0to2PiRadians( 66 | 2*Math.PI - inverted.waypoints_[i].theta); 67 | } 68 | 69 | return inverted; 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/TrajectoryFollower.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | /** 4 | * PID + Feedforward controller for following a Trajectory. 5 | * 6 | * @author Jared341 7 | */ 8 | public class TrajectoryFollower { 9 | 10 | private double kp_; 11 | private double ki_; // Not currently used, but might be in the future. 12 | private double kd_; 13 | private double kv_; 14 | private double ka_; 15 | private double last_error_; 16 | private double current_heading = 0; 17 | private int current_segment; 18 | private Trajectory profile_; 19 | 20 | public TrajectoryFollower() { 21 | 22 | } 23 | 24 | public void configure(double kp, double ki, double kd, double kv, double ka) { 25 | kp_ = kp; 26 | ki_ = ki; 27 | kd_ = kd; 28 | kv_ = kv; 29 | ka_ = ka; 30 | } 31 | 32 | public void reset() { 33 | last_error_ = 0.0; 34 | current_segment = 0; 35 | } 36 | 37 | public void setTrajectory(Trajectory profile) { 38 | profile_ = profile; 39 | } 40 | 41 | public double calculate(double distance_so_far) { 42 | if (current_segment < profile_.getNumSegments()) { 43 | Trajectory.Segment segment = profile_.getSegment(current_segment); 44 | double error = segment.pos - distance_so_far; 45 | double output = kp_ * error + kd_ * ((error - last_error_) 46 | / segment.dt - segment.vel) + (kv_ * segment.vel 47 | + ka_ * segment.acc); 48 | 49 | last_error_ = error; 50 | current_heading = segment.heading; 51 | current_segment++; 52 | //System.out.println("so far: " + distance_so_far + "; output: " + output); 53 | return output; 54 | } else { 55 | return 0; 56 | } 57 | } 58 | 59 | public double getHeading() { 60 | return current_heading; 61 | } 62 | 63 | public boolean isFinishedTrajectory() { 64 | return current_segment >= profile_.getNumSegments(); 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/io/JavaSerializer.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.io; 2 | 3 | import com.team254.lib.trajectory.Trajectory; 4 | import com.team254.lib.trajectory.Path; 5 | 6 | /** 7 | * Serialize a path to a Java file that can be compiled into a J2ME project. 8 | * 9 | * @author Jared341 10 | */ 11 | public class JavaSerializer implements IPathSerializer { 12 | 13 | /** 14 | * Generate a Java source code file from a Path 15 | * 16 | * For example output, see the unit test. 17 | * 18 | * @param path The path to serialize. 19 | * @return A complete Java file as a string. 20 | */ 21 | public String serialize(Path path) { 22 | String contents = "package com.team254.frc2014.paths;\n\n"; 23 | contents += "import com.team254.lib.trajectory.Trajectory;\n"; 24 | contents += "import com.team254.path.Path;\n\n"; 25 | contents += "public class " + path.getName() + " extends Path {\n"; 26 | path.goLeft(); 27 | contents += serializeTrajectory("kLeftWheel", 28 | path.getLeftWheelTrajectory()); 29 | contents += serializeTrajectory("kRightWheel", 30 | path.getRightWheelTrajectory()); 31 | 32 | contents += " public " + path.getName() + "() {\n"; 33 | contents += " this.name_ = \"" + path.getName() + "\";\n"; 34 | contents += " this.go_left_pair_ = new Trajectory.Pair(kLeftWheel, kRightWheel);\n"; 35 | contents += " }\n\n"; 36 | 37 | contents += "}\n"; 38 | return contents; 39 | } 40 | 41 | private String serializeTrajectory(String name, Trajectory traj) { 42 | String contents = 43 | " private final Trajectory " + name + " = new Trajectory( new Trajectory.Segment[] {\n"; 44 | for (int i = 0; i < traj.getNumSegments(); ++i) { 45 | Trajectory.Segment seg = traj.getSegment(i); 46 | contents += " new Trajectory.Segment(" 47 | + seg.pos + ", " + seg.vel + ", " + seg.acc + ", " 48 | + seg.jerk + ", " + seg.heading + ", " + seg.dt + ", " 49 | + seg.x + ", " + seg.y + "),\n"; 50 | } 51 | contents += " });\n\n"; 52 | return contents; 53 | } 54 | 55 | } 56 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/io/TextFileDeserializer.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory.io; 2 | 3 | import com.team254.lib.trajectory.Path; 4 | import java.util.StringTokenizer; 5 | import com.team254.lib.trajectory.Trajectory; 6 | 7 | /** 8 | * 9 | * @author Jared341 10 | */ 11 | public class TextFileDeserializer implements IPathDeserializer { 12 | 13 | public Path deserialize(String serialized) { 14 | StringTokenizer tokenizer = new StringTokenizer(serialized, "\n"); 15 | System.out.println("Parsing path string..."); 16 | System.out.println("String has " + serialized.length() + " chars"); 17 | System.out.println("Found " + tokenizer.countTokens() + " tokens"); 18 | 19 | String name = tokenizer.nextToken(); 20 | int num_elements = Integer.parseInt(tokenizer.nextToken()); 21 | 22 | Trajectory left = new Trajectory(num_elements); 23 | for (int i = 0; i < num_elements; ++i) { 24 | Trajectory.Segment segment = new Trajectory.Segment(); 25 | StringTokenizer line_tokenizer = new StringTokenizer( 26 | tokenizer.nextToken(), " "); 27 | 28 | segment.pos = Double.parseDouble(line_tokenizer.nextToken()); 29 | segment.vel = Double.parseDouble(line_tokenizer.nextToken()); 30 | segment.acc = Double.parseDouble(line_tokenizer.nextToken()); 31 | segment.jerk = Double.parseDouble(line_tokenizer.nextToken()); 32 | segment.heading = Double.parseDouble(line_tokenizer.nextToken()); 33 | segment.dt = Double.parseDouble(line_tokenizer.nextToken()); 34 | segment.x = Double.parseDouble(line_tokenizer.nextToken()); 35 | segment.y = Double.parseDouble(line_tokenizer.nextToken()); 36 | 37 | left.setSegment(i, segment); 38 | } 39 | Trajectory right = new Trajectory(num_elements); 40 | for (int i = 0; i < num_elements; ++i) { 41 | Trajectory.Segment segment = new Trajectory.Segment(); 42 | StringTokenizer line_tokenizer = new StringTokenizer( 43 | tokenizer.nextToken(), " "); 44 | 45 | segment.pos = Double.parseDouble(line_tokenizer.nextToken()); 46 | segment.vel = Double.parseDouble(line_tokenizer.nextToken()); 47 | segment.acc = Double.parseDouble(line_tokenizer.nextToken()); 48 | segment.jerk = Double.parseDouble(line_tokenizer.nextToken()); 49 | segment.heading = Double.parseDouble(line_tokenizer.nextToken()); 50 | segment.dt = Double.parseDouble(line_tokenizer.nextToken()); 51 | segment.x = Double.parseDouble(line_tokenizer.nextToken()); 52 | segment.y = Double.parseDouble(line_tokenizer.nextToken()); 53 | 54 | right.setSegment(i, segment); 55 | } 56 | 57 | System.out.println("...finished parsing path from string."); 58 | return new Path(name, new Trajectory.Pair(left, right)); 59 | } 60 | 61 | } 62 | -------------------------------------------------------------------------------- /nbproject/project.properties: -------------------------------------------------------------------------------- 1 | annotation.processing.enabled=true 2 | annotation.processing.enabled.in.editor=false 3 | annotation.processing.processors.list= 4 | annotation.processing.run.all.processors=true 5 | annotation.processing.source.output=${build.generated.sources.dir}/ap-source-output 6 | application.title=TrajectoryLib 7 | application.vendor=jarussell 8 | build.classes.dir=${build.dir}/classes 9 | build.classes.excludes=**/*.java,**/*.form 10 | # This directory is removed when the project is cleaned: 11 | build.dir=build 12 | build.generated.dir=${build.dir}/generated 13 | build.generated.sources.dir=${build.dir}/generated-sources 14 | # Only compile against the classpath explicitly listed here: 15 | build.sysclasspath=ignore 16 | build.test.classes.dir=${build.dir}/test/classes 17 | build.test.results.dir=${build.dir}/test/results 18 | # Uncomment to specify the preferred debugger connection transport: 19 | #debug.transport=dt_socket 20 | debug.classpath=\ 21 | ${run.classpath} 22 | debug.test.classpath=\ 23 | ${run.test.classpath} 24 | # Files in build.classes.dir which should be excluded from distribution jar 25 | dist.archive.excludes= 26 | # This directory is removed when the project is cleaned: 27 | dist.dir=dist 28 | dist.jar=${dist.dir}/TrajectoryLib.jar 29 | dist.javadoc.dir=${dist.dir}/javadoc 30 | endorsed.classpath= 31 | excludes= 32 | includes=** 33 | jar.archive.disabled=${jnlp.enabled} 34 | jar.compress=false 35 | jar.index=${jnlp.enabled} 36 | javac.classpath= 37 | # Space-separated list of extra javac options 38 | javac.compilerargs= 39 | javac.deprecation=false 40 | javac.processorpath=\ 41 | ${javac.classpath} 42 | javac.source=1.7 43 | javac.target=1.7 44 | javac.test.classpath=\ 45 | ${javac.classpath}:\ 46 | ${build.classes.dir}:\ 47 | ${libs.junit_4.classpath} 48 | javac.test.processorpath=\ 49 | ${javac.test.classpath} 50 | javadoc.additionalparam= 51 | javadoc.author=false 52 | javadoc.encoding=${source.encoding} 53 | javadoc.noindex=false 54 | javadoc.nonavbar=false 55 | javadoc.notree=false 56 | javadoc.private=false 57 | javadoc.splitindex=true 58 | javadoc.use=true 59 | javadoc.version=false 60 | javadoc.windowtitle= 61 | jnlp.codebase.type=no.codebase 62 | jnlp.descriptor=application 63 | jnlp.enabled=false 64 | jnlp.mixed.code=default 65 | jnlp.offline-allowed=false 66 | jnlp.signed=false 67 | jnlp.signing= 68 | jnlp.signing.alias= 69 | jnlp.signing.keystore= 70 | main.class=com.team254.lib.trajectory.Main 71 | # Optional override of default Codebase manifest attribute, use to prevent RIAs from being repurposed 72 | manifest.custom.codebase= 73 | # Optional override of default Permissions manifest attribute (supported values: sandbox, all-permissions) 74 | manifest.custom.permissions= 75 | manifest.file=manifest.mf 76 | meta.inf.dir=${src.dir}/META-INF 77 | mkdist.disabled=false 78 | platform.active=default_platform 79 | run.classpath=\ 80 | ${javac.classpath}:\ 81 | ${build.classes.dir} 82 | # Space-separated list of JVM arguments used when running the project. 83 | # You may also define separate properties like run-sys-prop.name=value instead of -Dname=value. 84 | # To set system properties for unit tests define test-sys-prop.name=value: 85 | run.jvmargs= 86 | run.test.classpath=\ 87 | ${javac.test.classpath}:\ 88 | ${build.test.classes.dir} 89 | source.encoding=UTF-8 90 | src.dir=src 91 | test.src.dir=test 92 | -------------------------------------------------------------------------------- /test/com/team254/lib/trajectory/TrajectoryGeneratorTest.java: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this license header, choose License Headers in Project Properties. 3 | * To change this template file, choose Tools | Templates 4 | * and open the template in the editor. 5 | */ 6 | package com.team254.lib.trajectory; 7 | 8 | import com.team254.lib.trajectory.Trajectory; 9 | import com.team254.lib.trajectory.TrajectoryGenerator; 10 | import junit.framework.Assert; 11 | import org.junit.After; 12 | import org.junit.AfterClass; 13 | import org.junit.Before; 14 | import org.junit.BeforeClass; 15 | import org.junit.Test; 16 | 17 | /** 18 | * 19 | * @author jarussell 20 | */ 21 | public class TrajectoryGeneratorTest { 22 | 23 | static void test(double start_vel, double goal_vel, double goal_distance, 24 | TrajectoryGenerator.Strategy strategy) { 25 | TrajectoryGenerator.Config config = new TrajectoryGenerator.Config(); 26 | config.dt = .01; 27 | config.max_acc = 250.0; 28 | config.max_jerk = 1250.0; 29 | config.max_vel = 100.0; 30 | 31 | Trajectory traj = TrajectoryGenerator.generate( 32 | config, 33 | strategy, 34 | start_vel, 35 | -75.0, 36 | goal_distance, 37 | goal_vel, 38 | 75.0); 39 | 40 | System.out.print(traj.toString()); 41 | 42 | Trajectory.Segment last = traj.getSegment(traj.getNumSegments() - 1); 43 | Assert.assertFalse(Math.abs(last.pos - goal_distance) > 1.0); 44 | Assert.assertFalse(Math.abs(last.vel - goal_vel) > 1.0); 45 | Assert.assertFalse(Math.abs(last.heading - 75.0) > 1.0); 46 | } 47 | 48 | public TrajectoryGeneratorTest() { 49 | } 50 | 51 | @BeforeClass 52 | public static void setUpClass() { 53 | } 54 | 55 | @AfterClass 56 | public static void tearDownClass() { 57 | } 58 | 59 | @Before 60 | public void setUp() { 61 | } 62 | 63 | @After 64 | public void tearDown() { 65 | } 66 | 67 | // Zero velocity endpoints 68 | @Test 69 | public void testP2PStep() { 70 | test(100, 100, 120, TrajectoryGenerator.StepStrategy); 71 | } 72 | 73 | @Test 74 | public void testP2PShortStep() { 75 | test(100, 100, 30, TrajectoryGenerator.StepStrategy); 76 | } 77 | 78 | @Test 79 | public void testP2PTrapezoid() { 80 | test(0, 0, 120, TrajectoryGenerator.TrapezoidalStrategy); 81 | } 82 | 83 | @Test 84 | public void testP2PShortTrapezoid() { 85 | test(0, 0, 30, TrajectoryGenerator.TrapezoidalStrategy); 86 | } 87 | 88 | @Test 89 | public void testP2PSCurves() { 90 | test(0, 0, 120, TrajectoryGenerator.SCurvesStrategy); 91 | } 92 | 93 | @Test 94 | public void testP2PShortSCurves() { 95 | test(0, 0, 30, TrajectoryGenerator.SCurvesStrategy); 96 | } 97 | 98 | // Non-zero velocity endpoints 99 | @Test 100 | public void testRampUp() { 101 | test(0, 100, 120, TrajectoryGenerator.TrapezoidalStrategy); 102 | } 103 | 104 | @Test 105 | public void testSlowRampUp() { 106 | test(0, 50, 120, TrajectoryGenerator.TrapezoidalStrategy); 107 | } 108 | 109 | @Test 110 | public void testRampDown() { 111 | test(100, 0, 120, TrajectoryGenerator.TrapezoidalStrategy); 112 | } 113 | 114 | @Test 115 | public void testSlowRampDown() { 116 | test(50, 0, 120, TrajectoryGenerator.TrapezoidalStrategy); 117 | } 118 | 119 | @Test 120 | public void testRampUpDown() { 121 | test(50, 50, 120, TrajectoryGenerator.TrapezoidalStrategy); 122 | } 123 | 124 | @Test 125 | public void testConstantVelTrapezoid() { 126 | test(100, 100, 120, TrajectoryGenerator.TrapezoidalStrategy); 127 | } 128 | } 129 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/Trajectory.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.util.ChezyMath; 4 | 5 | /** 6 | * Implementation of a Trajectory using arrays as the underlying storage 7 | * mechanism. 8 | * 9 | * @author Jared341 10 | */ 11 | public class Trajectory { 12 | 13 | public static class Pair { 14 | public Pair(Trajectory left, Trajectory right) { 15 | this.left = left; 16 | this.right = right; 17 | } 18 | 19 | public Trajectory left; 20 | public Trajectory right; 21 | } 22 | 23 | public static class Segment { 24 | 25 | public double pos, vel, acc, jerk, heading, dt, x, y; 26 | 27 | public Segment() { 28 | } 29 | 30 | public Segment(double pos, double vel, double acc, double jerk, 31 | double heading, double dt, double x, double y) { 32 | this.pos = pos; 33 | this.vel = vel; 34 | this.acc = acc; 35 | this.jerk = jerk; 36 | this.heading = heading; 37 | this.dt = dt; 38 | this.x = x; 39 | this.y = y; 40 | } 41 | 42 | public Segment(Segment to_copy) { 43 | pos = to_copy.pos; 44 | vel = to_copy.vel; 45 | acc = to_copy.acc; 46 | jerk = to_copy.jerk; 47 | heading = to_copy.heading; 48 | dt = to_copy.dt; 49 | x = to_copy.x; 50 | y = to_copy.y; 51 | } 52 | 53 | public String toString() { 54 | return "pos: " + pos + "; vel: " + vel + "; acc: " + acc + "; jerk: " 55 | + jerk + "; heading: " + heading; 56 | } 57 | } 58 | 59 | Segment[] segments_ = null; 60 | boolean inverted_y_ = false; 61 | 62 | public Trajectory(int length) { 63 | segments_ = new Segment[length]; 64 | for (int i = 0; i < length; ++i) { 65 | segments_[i] = new Segment(); 66 | } 67 | } 68 | 69 | public Trajectory(Segment[] segments) { 70 | segments_ = segments; 71 | } 72 | 73 | public void setInvertedY(boolean inverted) { 74 | inverted_y_ = inverted; 75 | } 76 | 77 | public int getNumSegments() { 78 | return segments_.length; 79 | } 80 | 81 | public Segment getSegment(int index) { 82 | if (index < getNumSegments()) { 83 | if (!inverted_y_) { 84 | return segments_[index]; 85 | } else { 86 | Segment segment = new Segment(segments_[index]); 87 | segment.y *= -1.0; 88 | segment.heading *= -1.0; 89 | return segment; 90 | } 91 | } else { 92 | return new Segment(); 93 | } 94 | } 95 | 96 | public void setSegment(int index, Segment segment) { 97 | if (index < getNumSegments()) { 98 | segments_[index] = segment; 99 | } 100 | } 101 | 102 | public void scale(double scaling_factor) { 103 | for (int i = 0; i < getNumSegments(); ++i) { 104 | segments_[i].pos *= scaling_factor; 105 | segments_[i].vel *= scaling_factor; 106 | segments_[i].acc *= scaling_factor; 107 | segments_[i].jerk *= scaling_factor; 108 | } 109 | } 110 | 111 | public void append(Trajectory to_append) { 112 | Segment[] temp = new Segment[getNumSegments() 113 | + to_append.getNumSegments()]; 114 | 115 | for (int i = 0; i < getNumSegments(); ++i) { 116 | temp[i] = new Segment(segments_[i]); 117 | } 118 | for (int i = 0; i < to_append.getNumSegments(); ++i) { 119 | temp[i + getNumSegments()] = new Segment(to_append.getSegment(i)); 120 | } 121 | 122 | this.segments_ = temp; 123 | } 124 | 125 | public Trajectory copy() { 126 | Trajectory cloned 127 | = new Trajectory(getNumSegments()); 128 | cloned.segments_ = copySegments(this.segments_); 129 | return cloned; 130 | } 131 | 132 | private Segment[] copySegments(Segment[] tocopy) { 133 | Segment[] copied = new Segment[tocopy.length]; 134 | for (int i = 0; i < tocopy.length; ++i) { 135 | copied[i] = new Segment(tocopy[i]); 136 | } 137 | return copied; 138 | } 139 | 140 | public String toString() { 141 | String str = "Segment\tPos\tVel\tAcc\tJerk\tHeading\n"; 142 | for (int i = 0; i < getNumSegments(); ++i) { 143 | Trajectory.Segment segment = getSegment(i); 144 | str += i + "\t"; 145 | str += segment.pos + "\t"; 146 | str += segment.vel + "\t"; 147 | str += segment.acc + "\t"; 148 | str += segment.jerk + "\t"; 149 | str += segment.heading + "\t"; 150 | str += "\n"; 151 | } 152 | 153 | return str; 154 | } 155 | 156 | public String toStringProfile() { 157 | return toString(); 158 | } 159 | 160 | public String toStringEuclidean() { 161 | String str = "Segment\tx\ty\tHeading\n"; 162 | for (int i = 0; i < getNumSegments(); ++i) { 163 | Trajectory.Segment segment = getSegment(i); 164 | str += i + "\t"; 165 | str += segment.x + "\t"; 166 | str += segment.y + "\t"; 167 | str += segment.heading + "\t"; 168 | str += "\n"; 169 | } 170 | 171 | return str; 172 | } 173 | } 174 | -------------------------------------------------------------------------------- /src/com/team254/lib/util/ChezyMath.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.util; 2 | 3 | /** 4 | * This class holds a bunch of static methods and variables needed for 5 | * mathematics 6 | */ 7 | public class ChezyMath { 8 | // constants 9 | 10 | static final double sq2p1 = 2.414213562373095048802e0; 11 | static final double sq2m1 = .414213562373095048802e0; 12 | static final double p4 = .161536412982230228262e2; 13 | static final double p3 = .26842548195503973794141e3; 14 | static final double p2 = .11530293515404850115428136e4; 15 | static final double p1 = .178040631643319697105464587e4; 16 | static final double p0 = .89678597403663861959987488e3; 17 | static final double q4 = .5895697050844462222791e2; 18 | static final double q3 = .536265374031215315104235e3; 19 | static final double q2 = .16667838148816337184521798e4; 20 | static final double q1 = .207933497444540981287275926e4; 21 | static final double q0 = .89678597403663861962481162e3; 22 | static final double PIO2 = 1.5707963267948966135E0; 23 | static final double nan = (0.0 / 0.0); 24 | // reduce 25 | 26 | private static double mxatan(double arg) { 27 | double argsq, value; 28 | 29 | argsq = arg * arg; 30 | value = ((((p4 * argsq + p3) * argsq + p2) * argsq + p1) * argsq + p0); 31 | value = value / (((((argsq + q4) * argsq + q3) * argsq + q2) * argsq + q1) * argsq + q0); 32 | return value * arg; 33 | } 34 | 35 | // reduce 36 | private static double msatan(double arg) { 37 | if (arg < sq2m1) { 38 | return mxatan(arg); 39 | } 40 | if (arg > sq2p1) { 41 | return PIO2 - mxatan(1 / arg); 42 | } 43 | return PIO2 / 2 + mxatan((arg - 1) / (arg + 1)); 44 | } 45 | 46 | // implementation of atan 47 | public static double atan(double arg) { 48 | if (arg > 0) { 49 | return msatan(arg); 50 | } 51 | return -msatan(-arg); 52 | } 53 | 54 | // implementation of atan2 55 | public static double atan2(double arg1, double arg2) { 56 | if (arg1 + arg2 == arg1) { 57 | if (arg1 >= 0) { 58 | return PIO2; 59 | } 60 | return -PIO2; 61 | } 62 | arg1 = atan(arg1 / arg2); 63 | if (arg2 < 0) { 64 | if (arg1 <= 0) { 65 | return arg1 + Math.PI; 66 | } 67 | return arg1 - Math.PI; 68 | } 69 | return arg1; 70 | 71 | } 72 | 73 | // implementation of asin 74 | public static double asin(double arg) { 75 | double temp; 76 | int sign; 77 | 78 | sign = 0; 79 | if (arg < 0) { 80 | arg = -arg; 81 | sign++; 82 | } 83 | if (arg > 1) { 84 | return nan; 85 | } 86 | temp = Math.sqrt(1 - arg * arg); 87 | if (arg > 0.7) { 88 | temp = PIO2 - atan(temp / arg); 89 | } else { 90 | temp = atan(arg / temp); 91 | } 92 | if (sign > 0) { 93 | temp = -temp; 94 | } 95 | return temp; 96 | } 97 | 98 | // implementation of acos 99 | public static double acos(double arg) { 100 | if (arg > 1 || arg < -1) { 101 | return nan; 102 | } 103 | return PIO2 - asin(arg); 104 | } 105 | 106 | /** 107 | * Get the difference in angle between two angles. 108 | * 109 | * @param from The first angle 110 | * @param to The second angle 111 | * @return The change in angle from the first argument necessary to line up 112 | * with the second. Always between -Pi and Pi 113 | */ 114 | public static double getDifferenceInAngleRadians(double from, double to) { 115 | return boundAngleNegPiToPiRadians(to - from); 116 | } 117 | 118 | /** 119 | * Get the difference in angle between two angles. 120 | * 121 | * @param from The first angle 122 | * @param to The second angle 123 | * @return The change in angle from the first argument necessary to line up 124 | * with the second. Always between -180 and 180 125 | */ 126 | public static double getDifferenceInAngleDegrees(double from, double to) { 127 | return boundAngleNeg180to180Degrees(to - from); 128 | } 129 | 130 | public static double boundAngle0to360Degrees(double angle) { 131 | // Naive algorithm 132 | while (angle >= 360.0) { 133 | angle -= 360.0; 134 | } 135 | while (angle < 0.0) { 136 | angle += 360.0; 137 | } 138 | return angle; 139 | } 140 | 141 | public static double boundAngleNeg180to180Degrees(double angle) { 142 | // Naive algorithm 143 | while (angle >= 180.0) { 144 | angle -= 360.0; 145 | } 146 | while (angle < -180.0) { 147 | angle += 360.0; 148 | } 149 | return angle; 150 | } 151 | 152 | public static double boundAngle0to2PiRadians(double angle) { 153 | // Naive algorithm 154 | while (angle >= 2.0 * Math.PI) { 155 | angle -= 2.0 * Math.PI; 156 | } 157 | while (angle < 0.0) { 158 | angle += 2.0 * Math.PI; 159 | } 160 | return angle; 161 | } 162 | 163 | public static double boundAngleNegPiToPiRadians(double angle) { 164 | // Naive algorithm 165 | while (angle >= Math.PI) { 166 | angle -= 2.0 * Math.PI; 167 | } 168 | while (angle < -Math.PI) { 169 | angle += 2.0 * Math.PI; 170 | } 171 | return angle; 172 | } 173 | 174 | public ChezyMath() { 175 | } 176 | } 177 | -------------------------------------------------------------------------------- /test/com/team254/lib/trajectory/io/SerializationDeserializationTest.java: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this license header, choose License Headers in Project Properties. 3 | * To change this template file, choose Tools | Templates 4 | * and open the template in the editor. 5 | */ 6 | package com.team254.lib.trajectory.io; 7 | 8 | import com.team254.lib.trajectory.WaypointSequence; 9 | import com.team254.lib.trajectory.WaypointSequence.Waypoint; 10 | import com.team254.lib.trajectory.PathGenerator; 11 | import com.team254.lib.trajectory.Trajectory; 12 | import com.team254.lib.trajectory.TrajectoryGenerator; 13 | import com.team254.lib.trajectory.io.JavaSerializer; 14 | import com.team254.lib.trajectory.Path; 15 | import junit.framework.Assert; 16 | import org.junit.After; 17 | import org.junit.AfterClass; 18 | import org.junit.Before; 19 | import org.junit.BeforeClass; 20 | import org.junit.Test; 21 | import static org.junit.Assert.*; 22 | 23 | /** 24 | * 25 | * @author jarussell 26 | */ 27 | public class SerializationDeserializationTest { 28 | 29 | public SerializationDeserializationTest() { 30 | } 31 | 32 | @BeforeClass 33 | public static void setUpClass() { 34 | } 35 | 36 | @AfterClass 37 | public static void tearDownClass() { 38 | } 39 | 40 | @Before 41 | public void setUp() { 42 | } 43 | 44 | @After 45 | public void tearDown() { 46 | } 47 | 48 | @Test 49 | public void testJavaSerialization() { 50 | WaypointSequence p = new WaypointSequence(10); 51 | p.addWaypoint(new Waypoint(0, 0, 0)); 52 | p.addWaypoint(new Waypoint(10, 0, 0)); 53 | p.addWaypoint(new Waypoint(20, 20, Math.PI / 4)); 54 | 55 | TrajectoryGenerator.Config config = new TrajectoryGenerator.Config(); 56 | config.dt = .01; 57 | config.max_acc = 1000.0; 58 | config.max_jerk = 5000.0; 59 | config.max_vel = 100.0; 60 | 61 | Path path = PathGenerator.makePath(p, config, 25.0, "TestPath"); 62 | 63 | JavaSerializer js = new JavaSerializer(); 64 | String serialized = js.serialize(path); 65 | System.out.print(serialized); 66 | 67 | //Assert.assertEquals(serialized, kGoldenOutput); 68 | } 69 | 70 | public boolean almostEqual(double a, double b) { 71 | return Math.abs(a-b) < 1E-6; 72 | } 73 | 74 | public void checkSegmentsEqual(Trajectory.Segment a, Trajectory.Segment b) { 75 | Assert.assertTrue(almostEqual(a.acc, b.acc)); 76 | Assert.assertTrue(almostEqual(a.jerk, b.jerk)); 77 | Assert.assertTrue(almostEqual(a.vel, b.vel)); 78 | Assert.assertTrue(almostEqual(a.pos, b.pos)); 79 | Assert.assertTrue(almostEqual(a.heading, b.heading)); 80 | Assert.assertTrue(almostEqual(a.dt, b.dt)); 81 | Assert.assertTrue(almostEqual(a.x, b.x)); 82 | Assert.assertTrue(almostEqual(a.y, b.y)); 83 | } 84 | 85 | @Test 86 | public void testTextFileSerialization() { 87 | WaypointSequence p = new WaypointSequence(10); 88 | p.addWaypoint(new Waypoint(0, 0, 0)); 89 | p.addWaypoint(new Waypoint(10, 0, 0)); 90 | p.addWaypoint(new Waypoint(20, 20, Math.PI / 4)); 91 | 92 | TrajectoryGenerator.Config config = new TrajectoryGenerator.Config(); 93 | config.dt = .01; 94 | config.max_acc = 1000.0; 95 | config.max_jerk = 5000.0; 96 | config.max_vel = 100.0; 97 | 98 | Path path = PathGenerator.makePath(p, config, 25.0, "TestPath"); 99 | 100 | TextFileSerializer tf = new TextFileSerializer(); 101 | String serialized = tf.serialize(path); 102 | System.out.print(serialized); 103 | 104 | TextFileDeserializer tfd = new TextFileDeserializer(); 105 | Path deserialized = tfd.deserialize(serialized); 106 | 107 | Assert.assertEquals("TestPath", deserialized.getName()); 108 | Assert.assertEquals(deserialized.getLeftWheelTrajectory().getNumSegments(), 109 | path.getLeftWheelTrajectory().getNumSegments()); 110 | Assert.assertEquals(deserialized.getRightWheelTrajectory().getNumSegments(), 111 | path.getRightWheelTrajectory().getNumSegments()); 112 | 113 | // Check segments as well 114 | for (int i = 0; i < path.getLeftWheelTrajectory().getNumSegments(); ++i) { 115 | System.out.println("Checking segment " + i); 116 | Trajectory.Segment left_serialized = 117 | path.getLeftWheelTrajectory().getSegment(i); 118 | Trajectory.Segment right_serialized = 119 | path.getRightWheelTrajectory().getSegment(i); 120 | Trajectory.Segment left_deserialized = 121 | deserialized.getLeftWheelTrajectory().getSegment(i); 122 | Trajectory.Segment right_deserialized = 123 | deserialized.getRightWheelTrajectory().getSegment(i); 124 | checkSegmentsEqual(left_serialized, left_deserialized); 125 | checkSegmentsEqual(right_serialized, right_deserialized); 126 | } 127 | } 128 | 129 | @Test 130 | public void testJavaStringSerializerDeserializer() { 131 | WaypointSequence p = new WaypointSequence(10); 132 | p.addWaypoint(new Waypoint(0, 0, 0)); 133 | p.addWaypoint(new Waypoint(10, 0, 0)); 134 | p.addWaypoint(new Waypoint(20, 20, Math.PI / 4)); 135 | 136 | TrajectoryGenerator.Config config = new TrajectoryGenerator.Config(); 137 | config.dt = .01; 138 | config.max_acc = 1000.0; 139 | config.max_jerk = 5000.0; 140 | config.max_vel = 100.0; 141 | 142 | Path path = PathGenerator.makePath(p, config, 25.0, "TestPath"); 143 | 144 | JavaStringSerializer tf = new JavaStringSerializer(); 145 | String serialized = tf.serialize(path); 146 | System.out.print(serialized); 147 | } 148 | } 149 | -------------------------------------------------------------------------------- /test/com/team254/lib/trajectory/SplineTest.java: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this license header, choose License Headers in Project Properties. 3 | * To change this template file, choose Tools | Templates 4 | * and open the template in the editor. 5 | */ 6 | package com.team254.lib.trajectory; 7 | 8 | import com.team254.lib.util.ChezyMath; 9 | import org.junit.After; 10 | import org.junit.AfterClass; 11 | import org.junit.Assert; 12 | import org.junit.Before; 13 | import org.junit.BeforeClass; 14 | import org.junit.Test; 15 | 16 | /** 17 | * Unit test for Spline class. 18 | * 19 | * @author Art Kalb 20 | * @author Jared341 21 | */ 22 | public class SplineTest { 23 | 24 | public boolean almostEqual(double x, double y) { 25 | return Math.abs(x - y) < 1E-6; 26 | } 27 | 28 | public void test(double x0, double y0, double theta0, double x1, double y1, 29 | double theta1, boolean is_straight) { 30 | Spline.Type[] types = {Spline.CubicHermite, Spline.QuinticHermite}; 31 | for (Spline.Type type : types) { 32 | Spline s = new Spline(); 33 | Assert.assertTrue(Spline.reticulateSplines(x0, y0, theta0, x1, y1, theta1, 34 | s, type)); 35 | System.out.println(s.toString()); 36 | 37 | for (double t = 0; t <= 1.0; t += .05) { 38 | System.out.println("" + t + ", " + s.valueAt(t) + ", " + s.angleAt(t)); 39 | } 40 | 41 | Assert.assertTrue(almostEqual(s.valueAt(0), y0)); 42 | Assert.assertTrue(almostEqual(s.valueAt(1), y1)); 43 | 44 | Assert.assertTrue(almostEqual( 45 | ChezyMath.boundAngleNegPiToPiRadians(s.angleAt(0)), 46 | ChezyMath.boundAngleNegPiToPiRadians(theta0))); 47 | Assert.assertTrue(almostEqual( 48 | ChezyMath.boundAngleNegPiToPiRadians(s.angleAt(1)), 49 | ChezyMath.boundAngleNegPiToPiRadians(theta1))); 50 | 51 | if (is_straight) { 52 | double expected = Math.sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0)); 53 | System.out.println("Expected length=" + expected + "; actual=" 54 | + s.calculateLength()); 55 | Assert.assertTrue(almostEqual(s.calculateLength(), expected)); 56 | } 57 | 58 | if (type == Spline.QuinticHermite) { 59 | // Second derivatives should be 0 60 | Assert.assertTrue(almostEqual(0, s.angleChangeAt(0))); 61 | Assert.assertTrue(almostEqual(0, s.angleChangeAt(1))); 62 | 63 | System.out.println("From (" + x0 + "," + y0 + ") to (" + x1 + "," + y1 64 | + ")"); 65 | for (double t = 0; t <= 1.04; t += .05) { 66 | System.out.println(t + "\t" + s.angleAt(t)); 67 | } 68 | } 69 | 70 | // Test arc length estimates 71 | double length = s.calculateLength(); 72 | System.out.println("Arc Length: " + length); 73 | double x_last = x0; 74 | double y_last = y0; 75 | for (int i = 1; i <= 100; ++i) { 76 | double percentage = s.getPercentageForDistance(i * length / 100); 77 | double[] xy = s.getXandY(percentage); 78 | double measured = Math.sqrt((xy[0] - x_last) * (xy[0] - x_last) 79 | + (xy[1] - y_last) * (xy[1] - y_last)); 80 | System.out.println(i + "\t" + percentage + "\t" + length / 100 + "\t" 81 | + measured); 82 | x_last = xy[0]; 83 | y_last = xy[1]; 84 | Assert.assertTrue(almostEqual(measured / 1E3, length / 100 / 1E3)); 85 | } 86 | } 87 | } 88 | 89 | public void expectFailure(double x0, double y0, double theta0, double x1, 90 | double y1, double theta1) { 91 | Spline s = new Spline(); 92 | Assert.assertFalse(Spline.reticulateSplines(x0, y0, theta0, x1, y1, theta1, 93 | s, Spline.CubicHermite)); 94 | } 95 | 96 | public SplineTest() { 97 | } 98 | 99 | @BeforeClass 100 | public static void setUpClass() { 101 | } 102 | 103 | @AfterClass 104 | public static void tearDownClass() { 105 | } 106 | 107 | @Before 108 | public void setUp() { 109 | } 110 | 111 | @After 112 | public void tearDown() { 113 | } 114 | 115 | @Test 116 | public void testUnitLines() { 117 | test(0, 0, 0, 1, 0, 0, true); 118 | test(0, 0, Math.PI / 2, 0, 1, Math.PI / 2, true); 119 | test(0, 0, Math.PI, -1, 0, Math.PI, true); 120 | test(0, 0, -Math.PI / 2, 0, -1, -Math.PI / 2, true); 121 | test(0, 0, Math.PI / 4, Math.sqrt(2) / 2, Math.sqrt(2) / 2, Math.PI / 4, true); 122 | } 123 | 124 | @Test 125 | public void testNonUnitLines() { 126 | test(0, 0, 0, 5, 0, 0, true); 127 | test(0, 0, Math.PI / 4, Math.sqrt(2), Math.sqrt(2), Math.PI / 4, true); 128 | } 129 | 130 | @Test 131 | public void testTranslatedUnitLines() { 132 | test(1, 1, 0, 2, 1, 0, true); 133 | test(1, 1, Math.PI / 4, Math.sqrt(2) / 2 + 1, Math.sqrt(2) / 2 + 1, Math.PI / 4, 134 | true); 135 | } 136 | 137 | @Test 138 | public void testTranslatedNonUnitLines() { 139 | test(1, 1, 0, 6, 1, 0, true); 140 | test(1, 1, Math.PI / 4, Math.sqrt(2) + 1, Math.sqrt(2) + 1, Math.PI / 4, 141 | true); 142 | } 143 | 144 | @Test 145 | public void testUnitStep() { 146 | test(0, 0, 0, Math.sqrt(2) / 2, Math.sqrt(2) / 2, 0, false); 147 | } 148 | 149 | @Test 150 | public void testNonUnitStep() { 151 | test(0, 0, 0, 10, 20, 0, false); 152 | } 153 | 154 | @Test 155 | public void testTranslatedStep() { 156 | test(0, 5, 0, 10, 20, 0, false); 157 | } 158 | 159 | @Test 160 | public void testRotatedStep() { 161 | test(0, 0, Math.PI / 4, 0, 1, Math.PI / 4, false); 162 | } 163 | 164 | @Test 165 | public void testInvalidInput() { 166 | expectFailure(0, 0, 0, 0, 0, 0); 167 | expectFailure(0, 0, -Math.PI / 2, 1, 0, 0); 168 | } 169 | 170 | @Test 171 | public void testProblematicSCurve() { 172 | test(0, 0, 0, 3, 4, Math.PI / 4, false); 173 | } 174 | 175 | @Test 176 | public void testRegressionQuintic1() { 177 | test(0, 0, 0, 10, 5, 0, false); 178 | test(10, 5, 0, 30, -5, 0, false); 179 | test(30, -5, 0, 40, 0, 0, false); 180 | } 181 | } 182 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/PathGenerator.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | /** 4 | * Generate a smooth Trajectory from a Path. 5 | * 6 | * @author Art Kalb 7 | * @author Stephen Pinkerton 8 | * @author Jared341 9 | */ 10 | public class PathGenerator { 11 | /** 12 | * Generate a path for autonomous driving. 13 | * 14 | * @param waypoints The waypoints to drive to (FOR THE "GO LEFT" CASE!!!!) 15 | * @param config Trajectory config. 16 | * @param wheelbase_width Wheelbase separation; units must be consistent with 17 | * config and waypoints. 18 | * @param name The name of the new path. THIS MUST BE A VALID JAVA CLASS NAME 19 | * @return The path. 20 | */ 21 | public static Path makePath(WaypointSequence waypoints, 22 | TrajectoryGenerator.Config config, double wheelbase_width, 23 | String name) { 24 | return new Path(name, 25 | generateLeftAndRightFromSeq(waypoints, config, wheelbase_width)); 26 | } 27 | 28 | static Trajectory.Pair generateLeftAndRightFromSeq(WaypointSequence path, 29 | TrajectoryGenerator.Config config, double wheelbase_width) { 30 | return makeLeftAndRightTrajectories(generateFromPath(path, config), 31 | wheelbase_width); 32 | } 33 | 34 | static Trajectory generateFromPath(WaypointSequence path, 35 | TrajectoryGenerator.Config config) { 36 | if (path.getNumWaypoints() < 2) { 37 | return null; 38 | } 39 | 40 | // Compute the total length of the path by creating splines for each pair 41 | // of waypoints. 42 | Spline[] splines = new Spline[path.getNumWaypoints() - 1]; 43 | double[] spline_lengths = new double[splines.length]; 44 | double total_distance = 0; 45 | for (int i = 0; i < splines.length; ++i) { 46 | splines[i] = new Spline(); 47 | if (!Spline.reticulateSplines(path.getWaypoint(i), 48 | path.getWaypoint(i + 1), splines[i], Spline.QuinticHermite)) { 49 | return null; 50 | } 51 | spline_lengths[i] = splines[i].calculateLength(); 52 | total_distance += spline_lengths[i]; 53 | } 54 | 55 | // Generate a smooth trajectory over the total distance. 56 | Trajectory traj = TrajectoryGenerator.generate(config, 57 | TrajectoryGenerator.SCurvesStrategy, 0.0, path.getWaypoint(0).theta, 58 | total_distance, 0.0, path.getWaypoint(0).theta); 59 | 60 | // Assign headings based on the splines. 61 | int cur_spline = 0; 62 | double cur_spline_start_pos = 0; 63 | double length_of_splines_finished = 0; 64 | for (int i = 0; i < traj.getNumSegments(); ++i) { 65 | double cur_pos = traj.getSegment(i).pos; 66 | 67 | boolean found_spline = false; 68 | while (!found_spline) { 69 | double cur_pos_relative = cur_pos - cur_spline_start_pos; 70 | if (cur_pos_relative <= spline_lengths[cur_spline]) { 71 | double percentage = splines[cur_spline].getPercentageForDistance( 72 | cur_pos_relative); 73 | traj.getSegment(i).heading = splines[cur_spline].angleAt(percentage); 74 | double[] coords = splines[cur_spline].getXandY(percentage); 75 | traj.getSegment(i).x = coords[0]; 76 | traj.getSegment(i).y = coords[1]; 77 | found_spline = true; 78 | } else if (cur_spline < splines.length - 1) { 79 | length_of_splines_finished += spline_lengths[cur_spline]; 80 | cur_spline_start_pos = length_of_splines_finished; 81 | ++cur_spline; 82 | } else { 83 | traj.getSegment(i).heading = splines[splines.length - 1].angleAt(1.0); 84 | double[] coords = splines[splines.length - 1].getXandY(1.0); 85 | traj.getSegment(i).x = coords[0]; 86 | traj.getSegment(i).y = coords[1]; 87 | found_spline = true; 88 | } 89 | } 90 | } 91 | 92 | return traj; 93 | } 94 | 95 | /** 96 | * Generate left and right wheel trajectories from a reference. 97 | * 98 | * @param input The reference trajectory. 99 | * @param wheelbase_width The center-to-center distance between the left and 100 | * right sides. 101 | * @return [0] is left, [1] is right 102 | */ 103 | static Trajectory.Pair makeLeftAndRightTrajectories(Trajectory input, 104 | double wheelbase_width) { 105 | Trajectory[] output = new Trajectory[2]; 106 | output[0] = input.copy(); 107 | output[1] = input.copy(); 108 | Trajectory left = output[0]; 109 | Trajectory right = output[1]; 110 | 111 | for (int i = 0; i < input.getNumSegments(); ++i) { 112 | Trajectory.Segment current = input.getSegment(i); 113 | double cos_angle = Math.cos(current.heading); 114 | double sin_angle = Math.sin(current.heading); 115 | 116 | Trajectory.Segment s_left = left.getSegment(i); 117 | s_left.x = current.x - wheelbase_width / 2 * sin_angle; 118 | s_left.y = current.y + wheelbase_width / 2 * cos_angle; 119 | if (i > 0) { 120 | // Get distance between current and last segment 121 | double dist = Math.sqrt((s_left.x - left.getSegment(i - 1).x) 122 | * (s_left.x - left.getSegment(i - 1).x) 123 | + (s_left.y - left.getSegment(i - 1).y) 124 | * (s_left.y - left.getSegment(i - 1).y)); 125 | s_left.pos = left.getSegment(i - 1).pos + dist; 126 | s_left.vel = dist / s_left.dt; 127 | s_left.acc = (s_left.vel - left.getSegment(i - 1).vel) / s_left.dt; 128 | s_left.jerk = (s_left.acc - left.getSegment(i - 1).acc) / s_left.dt; 129 | } 130 | 131 | Trajectory.Segment s_right = right.getSegment(i); 132 | s_right.x = current.x + wheelbase_width / 2 * sin_angle; 133 | s_right.y = current.y - wheelbase_width / 2 * cos_angle; 134 | if (i > 0) { 135 | // Get distance between current and last segment 136 | double dist = Math.sqrt((s_right.x - right.getSegment(i - 1).x) 137 | * (s_right.x - right.getSegment(i - 1).x) 138 | + (s_right.y - right.getSegment(i - 1).y) 139 | * (s_right.y - right.getSegment(i - 1).y)); 140 | s_right.pos = right.getSegment(i - 1).pos + dist; 141 | s_right.vel = dist / s_right.dt; 142 | s_right.acc = (s_right.vel - right.getSegment(i - 1).vel) / s_right.dt; 143 | s_right.jerk = (s_right.acc - right.getSegment(i - 1).acc) / s_right.dt; 144 | } 145 | } 146 | 147 | return new Trajectory.Pair(output[0], output[1]); 148 | } 149 | } 150 | -------------------------------------------------------------------------------- /test/com/team254/lib/trajectory/PathGeneratorTest.java: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this license header, choose License Headers in Project Properties. 3 | * To change this template file, choose Tools | Templates 4 | * and open the template in the editor. 5 | */ 6 | package com.team254.lib.trajectory; 7 | 8 | import com.team254.lib.trajectory.Trajectory.Segment; 9 | import static com.team254.lib.trajectory.TrajectoryGeneratorTest.test; 10 | import com.team254.lib.util.ChezyMath; 11 | import junit.framework.Assert; 12 | import org.junit.After; 13 | import org.junit.AfterClass; 14 | import org.junit.Before; 15 | import org.junit.BeforeClass; 16 | import org.junit.Test; 17 | import static org.junit.Assert.*; 18 | 19 | /** 20 | * 21 | * @author Art Kalb 22 | * @author Stephen Pinkerton 23 | * @author Jared341 24 | */ 25 | public class PathGeneratorTest { 26 | 27 | static double distanceToClosest(Trajectory traj, WaypointSequence.Waypoint waypoint, 28 | Trajectory.Segment closest_segment) { 29 | double closest = Double.MAX_VALUE; 30 | int closest_id = -1; 31 | for (int i = 0; i < traj.getNumSegments(); ++i) { 32 | Segment segment = traj.getSegment(i); 33 | double distance = Math.sqrt( 34 | (segment.x - waypoint.x) * (segment.x - waypoint.x) 35 | + (segment.y - waypoint.y) * (segment.y - waypoint.y)); 36 | if (distance < closest) { 37 | closest = distance; 38 | closest_segment.x = waypoint.x; 39 | closest_segment.y = waypoint.y; 40 | closest_segment.heading = segment.heading; 41 | closest_id = i; 42 | } 43 | } 44 | System.out.println("Closest point segment #: " + closest_id); 45 | System.out.println("Closest point distance: " + closest); 46 | System.out.println("Closest point heading difference: " 47 | + ChezyMath.getDifferenceInAngleRadians(closest_segment.heading, 48 | waypoint.theta)); 49 | return closest; 50 | } 51 | 52 | static void test(WaypointSequence path) { 53 | TrajectoryGenerator.Config config = new TrajectoryGenerator.Config(); 54 | config.dt = .01; 55 | config.max_acc = 250.0; 56 | config.max_jerk = 1250.0; 57 | config.max_vel = 100.0; 58 | Trajectory traj = PathGenerator.generateFromPath(path, config); 59 | 60 | System.out.print(traj.toStringProfile()); 61 | System.out.print(traj.toStringEuclidean()); 62 | System.out.println("Final distance=" 63 | + traj.getSegment(traj.getNumSegments() - 1).pos); 64 | 65 | // The trajectory should be close (allowing for loss of precision) to each 66 | // desired waypoint. 67 | for (int i = 0; i < path.getNumWaypoints(); ++i) { 68 | WaypointSequence.Waypoint waypoint = path.getWaypoint(i); 69 | Segment closest = new Segment(); 70 | Assert.assertTrue(1 > distanceToClosest(traj, waypoint, closest)); 71 | double heading_diff = Math.abs(ChezyMath.getDifferenceInAngleRadians( 72 | closest.heading, waypoint.theta)); 73 | System.out.println("Heading diff: " + heading_diff); 74 | Assert.assertTrue(heading_diff < 1E-2); 75 | } 76 | 77 | Trajectory.Pair output = PathGenerator.makeLeftAndRightTrajectories(traj, 78 | 20.0); 79 | 80 | System.out.println("LEFT PROFILE:"); 81 | System.out.println(output.left.toStringProfile()); 82 | System.out.println(output.left.toStringEuclidean()); 83 | System.out.println("RIGHT PROFILE:"); 84 | System.out.println(output.right.toStringProfile()); 85 | System.out.println(output.right.toStringEuclidean()); 86 | 87 | // At all points, the distance from left to right should equal the wheelbase 88 | // width and the angle of the line between them should be 90 degrees off the 89 | // heading. 90 | for (int i = 0; i < traj.getNumSegments(); ++i) { 91 | Segment left = output.left.getSegment(i); 92 | Segment right = output.right.getSegment(i); 93 | Assert.assertTrue(Math.abs(Math.sqrt((left.x - right.x) * (left.x - right.x) 94 | + (left.y - right.y) * (left.y - right.y)) - 20.0) < 1E-3); 95 | double angle_left_to_right = Math.atan2(left.y - right.y, left.x - right.x); 96 | Assert.assertTrue(Math.abs( 97 | ChezyMath.getDifferenceInAngleRadians(angle_left_to_right, 98 | traj.getSegment(i).heading + Math.PI / 2)) < 1E-3); 99 | } 100 | } 101 | 102 | public PathGeneratorTest() { 103 | } 104 | 105 | @BeforeClass 106 | public static void setUpClass() { 107 | } 108 | 109 | @AfterClass 110 | public static void tearDownClass() { 111 | } 112 | 113 | @Before 114 | public void setUp() { 115 | } 116 | 117 | @After 118 | public void tearDown() { 119 | } 120 | 121 | @Test 122 | public void testSimplePath() { 123 | WaypointSequence p = new WaypointSequence(10); 124 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 125 | p.addWaypoint(new WaypointSequence.Waypoint(100, 0, 0)); 126 | p.addWaypoint(new WaypointSequence.Waypoint(150, 50, Math.PI / 4)); 127 | test(p); 128 | } 129 | 130 | @Test 131 | public void testSCurveLikePath() { 132 | WaypointSequence p = new WaypointSequence(10); 133 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 134 | p.addWaypoint(new WaypointSequence.Waypoint(10 * 12, 0, 0)); 135 | test(p); 136 | p.addWaypoint(new WaypointSequence.Waypoint(15 * 12, 5 * 12, Math.PI / 4)); 137 | test(p); 138 | p.addWaypoint(new WaypointSequence.Waypoint(20 * 12, 10 * 12, Math.PI / 4)); 139 | test(p); 140 | p.addWaypoint(new WaypointSequence.Waypoint(30 * 12, 10 * 12, 0)); 141 | test(p); 142 | } 143 | 144 | @Test 145 | public void testZigZag() { 146 | WaypointSequence p = new WaypointSequence(10); 147 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 148 | p.addWaypoint(new WaypointSequence.Waypoint(10, 5, 0)); 149 | p.addWaypoint(new WaypointSequence.Waypoint(30, -5, 0)); 150 | p.addWaypoint(new WaypointSequence.Waypoint(40, 0, 0)); 151 | test(p); 152 | } 153 | 154 | @Test 155 | public void testZigZagWithHeadings() { 156 | WaypointSequence p = new WaypointSequence(10); 157 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 158 | p.addWaypoint(new WaypointSequence.Waypoint(5, 2.5, Math.PI / 5)); 159 | p.addWaypoint(new WaypointSequence.Waypoint(25, -2.5, -Math.PI / 5)); 160 | p.addWaypoint(new WaypointSequence.Waypoint(40, 0, 0)); 161 | test(p); 162 | } 163 | 164 | @Test 165 | public void testRealishAutoMode() { 166 | WaypointSequence p = new WaypointSequence(10); 167 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 168 | p.addWaypoint(new WaypointSequence.Waypoint(5 * 12, 0, 0)); 169 | p.addWaypoint(new WaypointSequence.Waypoint(16 * 12, 12 * 12, 0)); 170 | p.addWaypoint(new WaypointSequence.Waypoint(18 * 12, 12 * 12, 0)); 171 | test(p); 172 | } 173 | 174 | @Test 175 | public void testDiscontinuity() { 176 | WaypointSequence p = new WaypointSequence(10); 177 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 178 | p.addWaypoint(new WaypointSequence.Waypoint(60, 0, 0)); 179 | p.addWaypoint(new WaypointSequence.Waypoint(200, 100, 0)); 180 | test(p); 181 | } 182 | } 183 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/Main.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.trajectory.io.JavaSerializer; 4 | import com.team254.lib.trajectory.io.JavaStringSerializer; 5 | import com.team254.lib.trajectory.io.TextFileSerializer; 6 | import java.io.BufferedWriter; 7 | import java.io.File; 8 | import java.io.FileWriter; 9 | import java.io.IOException; 10 | 11 | /** 12 | * 13 | * @author Jared341 14 | */ 15 | public class Main { 16 | public static String joinPath(String path1, String path2) 17 | { 18 | File file1 = new File(path1); 19 | File file2 = new File(file1, path2); 20 | return file2.getPath(); 21 | } 22 | 23 | private static boolean writeFile(String path, String data) { 24 | try { 25 | File file = new File(path); 26 | 27 | // if file doesnt exists, then create it 28 | if (!file.exists()) { 29 | file.createNewFile(); 30 | } 31 | 32 | FileWriter fw = new FileWriter(file.getAbsoluteFile()); 33 | BufferedWriter bw = new BufferedWriter(fw); 34 | bw.write(data); 35 | bw.close(); 36 | } catch (IOException e) { 37 | return false; 38 | } 39 | 40 | return true; 41 | } 42 | 43 | public static void main(String[] args) { 44 | String directory = "../FRC-2014/paths"; 45 | if (args.length >= 1) { 46 | directory = args[0]; 47 | } 48 | 49 | TrajectoryGenerator.Config config = new TrajectoryGenerator.Config(); 50 | config.dt = .01; 51 | config.max_acc = 10.0; 52 | config.max_jerk = 60.0; 53 | config.max_vel = 15.0; 54 | 55 | final double kWheelbaseWidth = 25.5/12; 56 | { 57 | config.dt = .01; 58 | config.max_acc = 8.0; 59 | config.max_jerk = 50.0; 60 | config.max_vel = 10.0; 61 | // Path name must be a valid Java class name. 62 | final String path_name = "InsideLanePathFar"; 63 | 64 | // Description of this auto mode path. 65 | // Remember that this is for the GO LEFT CASE! 66 | WaypointSequence p = new WaypointSequence(10); 67 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 68 | p.addWaypoint(new WaypointSequence.Waypoint(7.0, 0, 0)); 69 | p.addWaypoint(new WaypointSequence.Waypoint(14.0, 1.0, Math.PI / 12.0)); 70 | 71 | Path path = PathGenerator.makePath(p, config, 72 | kWheelbaseWidth, path_name); 73 | 74 | // Outputs to the directory supplied as the first argument. 75 | TextFileSerializer js = new TextFileSerializer(); 76 | String serialized = js.serialize(path); 77 | //System.out.print(serialized); 78 | String fullpath = joinPath(directory, path_name + ".txt"); 79 | if (!writeFile(fullpath, serialized)) { 80 | System.err.println(fullpath + " could not be written!!!!1"); 81 | System.exit(1); 82 | } else { 83 | System.out.println("Wrote " + fullpath); 84 | } 85 | } 86 | 87 | { 88 | config.dt = .01; 89 | config.max_acc = 8.0; 90 | config.max_jerk = 50.0; 91 | config.max_vel = 10.0; 92 | // Path name must be a valid Java class name. 93 | final String path_name = "CenterLanePathFar"; 94 | 95 | // Description of this auto mode path. 96 | // Remember that this is for the GO LEFT CASE! 97 | WaypointSequence p = new WaypointSequence(10); 98 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 99 | p.addWaypoint(new WaypointSequence.Waypoint(7.0, 0, 0)); 100 | p.addWaypoint(new WaypointSequence.Waypoint(14.0, 5.0, Math.PI / 12.0)); 101 | 102 | Path path = PathGenerator.makePath(p, config, 103 | kWheelbaseWidth, path_name); 104 | 105 | // Outputs to the directory supplied as the first argument. 106 | TextFileSerializer js = new TextFileSerializer(); 107 | String serialized = js.serialize(path); 108 | //System.out.print(serialized); 109 | String fullpath = joinPath(directory, path_name + ".txt"); 110 | if (!writeFile(fullpath, serialized)) { 111 | System.err.println(fullpath + " could not be written!!!!1"); 112 | System.exit(1); 113 | } else { 114 | System.out.println("Wrote " + fullpath); 115 | } 116 | } 117 | 118 | 119 | { 120 | config.dt = .01; 121 | config.max_acc = 8.5; 122 | config.max_jerk = 50.0; 123 | config.max_vel = 12.0; 124 | // Path name must be a valid Java class name. 125 | final String path_name = "InsideLanePathClose"; 126 | 127 | // Description of this auto mode path. 128 | // Remember that this is for the GO LEFT CASE! 129 | WaypointSequence p = new WaypointSequence(10); 130 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 131 | p.addWaypoint(new WaypointSequence.Waypoint(7.0, 0, 0)); 132 | p.addWaypoint(new WaypointSequence.Waypoint(15.0, 3, Math.PI / 12.0)); 133 | 134 | Path path = PathGenerator.makePath(p, config, 135 | kWheelbaseWidth, path_name); 136 | 137 | // Outputs to the directory supplied as the first argument. 138 | TextFileSerializer js = new TextFileSerializer(); 139 | String serialized = js.serialize(path); 140 | //System.out.print(serialized); 141 | String fullpath = joinPath(directory, path_name + ".txt"); 142 | if (!writeFile(fullpath, serialized)) { 143 | System.err.println(fullpath + " could not be written!!!!1"); 144 | System.exit(1); 145 | } else { 146 | System.out.println("Wrote " + fullpath); 147 | } 148 | } 149 | 150 | { 151 | config.dt = .01; 152 | config.max_acc = 9.0; 153 | config.max_jerk = 50.0; 154 | config.max_vel = 11.75; 155 | // Path name must be a valid Java class name. 156 | final String path_name = "StraightAheadPath"; 157 | 158 | // Description of this auto mode path. 159 | // Remember that this is for the GO LEFT CASE! 160 | WaypointSequence p = new WaypointSequence(10); 161 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 162 | p.addWaypoint(new WaypointSequence.Waypoint(14, 0, 0)); 163 | 164 | Path path = PathGenerator.makePath(p, config, 165 | kWheelbaseWidth, path_name); 166 | 167 | // Outputs to the directory supplied as the first argument. 168 | TextFileSerializer js = new TextFileSerializer(); 169 | String serialized = js.serialize(path); 170 | //System.out.print(serialized); 171 | String fullpath = joinPath(directory, path_name + ".txt"); 172 | if (!writeFile(fullpath, serialized)) { 173 | System.err.println(fullpath + " could not be written!!!!1"); 174 | System.exit(1); 175 | } else { 176 | System.out.println("Wrote " + fullpath); 177 | } 178 | } 179 | 180 | { 181 | // Path name must be a valid Java class name. 182 | config.dt = .01; 183 | config.max_acc = 7.0; 184 | config.max_jerk = 50.0; 185 | config.max_vel = 10.0; 186 | final String path_name = "WallLanePath"; 187 | 188 | // Description of this auto mode path. 189 | // Remember that this is for the GO LEFT CASE! 190 | WaypointSequence p = new WaypointSequence(10); 191 | p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0)); 192 | p.addWaypoint(new WaypointSequence.Waypoint(2.5, 0, 0)); 193 | p.addWaypoint(new WaypointSequence.Waypoint(10.5, 8, Math.PI/12.0)); 194 | p.addWaypoint(new WaypointSequence.Waypoint(13.75, 9.5, 0.0/* * Math.PI/18.0*/)); 195 | 196 | 197 | Path path = PathGenerator.makePath(p, config, 198 | kWheelbaseWidth, path_name); 199 | 200 | // Outputs to the directory supplied as the first argument. 201 | TextFileSerializer js = new TextFileSerializer(); 202 | String serialized = js.serialize(path); 203 | //System.out.print(serialized); 204 | String fullpath = joinPath(directory, path_name + ".txt"); 205 | if (!writeFile(fullpath, serialized)) { 206 | System.err.println(fullpath + " could not be written!!!!1"); 207 | System.exit(1); 208 | } else { 209 | System.out.println("Wrote " + fullpath); 210 | } 211 | } 212 | } 213 | } 214 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/Spline.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | import com.team254.lib.util.ChezyMath; 4 | 5 | /** 6 | * Do cubic spline interpolation between points. 7 | * 8 | * @author Art Kalb 9 | * @author Jared341 10 | */ 11 | public class Spline { 12 | 13 | public static class Type { 14 | 15 | private final String value_; 16 | 17 | private Type(String value) { 18 | this.value_ = value; 19 | } 20 | 21 | public String toString() { 22 | return value_; 23 | } 24 | } 25 | 26 | // Cubic spline where positions and first derivatives (angle) constraints will 27 | // be met but second derivatives may be discontinuous. 28 | public static final Type CubicHermite = new Type("CubicHermite"); 29 | 30 | // Quintic spline where positions and first derivatives (angle) constraints 31 | // will be met, and all second derivatives at knots = 0. 32 | public static final Type QuinticHermite = new Type("QuinticHermite"); 33 | 34 | Type type_; 35 | double a_; // ax^5 36 | double b_; // + bx^4 37 | double c_; // + cx^3 38 | double d_; // + dx^2 39 | double e_; // + ex 40 | // f is always 0 for the spline formulation we support. 41 | 42 | // The offset from the world frame to the spline frame. 43 | // Add these to the output of the spline to obtain world coordinates. 44 | double y_offset_; 45 | double x_offset_; 46 | double knot_distance_; 47 | double theta_offset_; 48 | double arc_length_; 49 | 50 | Spline() { 51 | // All splines should be made via the static interface 52 | arc_length_ = -1; 53 | } 54 | 55 | private static boolean almostEqual(double x, double y) { 56 | return Math.abs(x - y) < 1E-6; 57 | } 58 | 59 | public static boolean reticulateSplines(WaypointSequence.Waypoint start, 60 | WaypointSequence.Waypoint goal, Spline result, Type type) { 61 | return reticulateSplines(start.x, start.y, start.theta, goal.x, goal.y, 62 | goal.theta, result, type); 63 | } 64 | 65 | public static boolean reticulateSplines(double x0, double y0, double theta0, 66 | double x1, double y1, double theta1, Spline result, Type type) { 67 | System.out.println("Reticulating splines..."); 68 | result.type_ = type; 69 | 70 | // Transform x to the origin 71 | result.x_offset_ = x0; 72 | result.y_offset_ = y0; 73 | double x1_hat = Math.sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0)); 74 | if (x1_hat == 0) { 75 | return false; 76 | } 77 | result.knot_distance_ = x1_hat; 78 | result.theta_offset_ = Math.atan2(y1 - y0, x1 - x0); 79 | double theta0_hat = ChezyMath.getDifferenceInAngleRadians( 80 | result.theta_offset_, theta0); 81 | double theta1_hat = ChezyMath.getDifferenceInAngleRadians( 82 | result.theta_offset_, theta1); 83 | // We cannot handle vertical slopes in our rotated, translated basis. 84 | // This would mean the user wants to end up 90 degrees off of the straight 85 | // line between p0 and p1. 86 | if (almostEqual(Math.abs(theta0_hat), Math.PI / 2) 87 | || almostEqual(Math.abs(theta1_hat), Math.PI / 2)) { 88 | return false; 89 | } 90 | // We also cannot handle the case that the end angle is facing towards the 91 | // start angle (total turn > 90 degrees). 92 | if (Math.abs(ChezyMath.getDifferenceInAngleRadians(theta0_hat, 93 | theta1_hat)) 94 | >= Math.PI / 2) { 95 | return false; 96 | } 97 | // Turn angles into derivatives (slopes) 98 | double yp0_hat = Math.tan(theta0_hat); 99 | double yp1_hat = Math.tan(theta1_hat); 100 | 101 | if (type == CubicHermite) { 102 | // Calculate the cubic spline coefficients 103 | result.a_ = 0; 104 | result.b_ = 0; 105 | result.c_ = (yp1_hat + yp0_hat) / (x1_hat * x1_hat); 106 | result.d_ = -(2 * yp0_hat + yp1_hat) / x1_hat; 107 | result.e_ = yp0_hat; 108 | } else if (type == QuinticHermite) { 109 | result.a_ = -(3 * (yp0_hat + yp1_hat)) / (x1_hat * x1_hat * x1_hat * x1_hat); 110 | result.b_ = (8 * yp0_hat + 7 * yp1_hat) / (x1_hat * x1_hat * x1_hat); 111 | result.c_ = -(6 * yp0_hat + 4 * yp1_hat) / (x1_hat * x1_hat); 112 | result.d_ = 0; 113 | result.e_ = yp0_hat; 114 | } 115 | 116 | return true; 117 | } 118 | 119 | public double calculateLength() { 120 | if (arc_length_ >= 0) { 121 | return arc_length_; 122 | } 123 | 124 | final int kNumSamples = 100000; 125 | double arc_length = 0; 126 | double t, dydt; 127 | double integrand, last_integrand 128 | = Math.sqrt(1 + derivativeAt(0) * derivativeAt(0)) / kNumSamples; 129 | for (int i = 1; i <= kNumSamples; ++i) { 130 | t = ((double) i) / kNumSamples; 131 | dydt = derivativeAt(t); 132 | integrand = Math.sqrt(1 + dydt * dydt) / kNumSamples; 133 | arc_length += (integrand + last_integrand) / 2; 134 | last_integrand = integrand; 135 | } 136 | arc_length_ = knot_distance_ * arc_length; 137 | return arc_length_; 138 | } 139 | 140 | public double getPercentageForDistance(double distance) { 141 | final int kNumSamples = 100000; 142 | double arc_length = 0; 143 | double t = 0; 144 | double last_arc_length = 0; 145 | double dydt; 146 | double integrand, last_integrand 147 | = Math.sqrt(1 + derivativeAt(0) * derivativeAt(0)) / kNumSamples; 148 | distance /= knot_distance_; 149 | for (int i = 1; i <= kNumSamples; ++i) { 150 | t = ((double) i) / kNumSamples; 151 | dydt = derivativeAt(t); 152 | integrand = Math.sqrt(1 + dydt * dydt) / kNumSamples; 153 | arc_length += (integrand + last_integrand) / 2; 154 | if (arc_length > distance) { 155 | break; 156 | } 157 | last_integrand = integrand; 158 | last_arc_length = arc_length; 159 | } 160 | 161 | // Interpolate between samples. 162 | double interpolated = t; 163 | if (arc_length != last_arc_length) { 164 | interpolated += ((distance - last_arc_length) 165 | / (arc_length - last_arc_length) - 1) / (double) kNumSamples; 166 | } 167 | return interpolated; 168 | } 169 | 170 | public double[] getXandY(double percentage) { 171 | double[] result = new double[2]; 172 | 173 | percentage = Math.max(Math.min(percentage, 1), 0); 174 | double x_hat = percentage * knot_distance_; 175 | double y_hat = (a_ * x_hat + b_) * x_hat * x_hat * x_hat * x_hat 176 | + c_ * x_hat * x_hat * x_hat + d_ * x_hat * x_hat + e_ * x_hat; 177 | 178 | double cos_theta = Math.cos(theta_offset_); 179 | double sin_theta = Math.sin(theta_offset_); 180 | 181 | result[0] = x_hat * cos_theta - y_hat * sin_theta + x_offset_; 182 | result[1] = x_hat * sin_theta + y_hat * cos_theta + y_offset_; 183 | return result; 184 | } 185 | 186 | public double valueAt(double percentage) { 187 | percentage = Math.max(Math.min(percentage, 1), 0); 188 | double x_hat = percentage * knot_distance_; 189 | double y_hat = (a_ * x_hat + b_) * x_hat * x_hat * x_hat * x_hat 190 | + c_ * x_hat * x_hat * x_hat + d_ * x_hat * x_hat + e_ * x_hat; 191 | 192 | double cos_theta = Math.cos(theta_offset_); 193 | double sin_theta = Math.sin(theta_offset_); 194 | 195 | double value = x_hat * sin_theta + y_hat * cos_theta + y_offset_; 196 | return value; 197 | } 198 | 199 | private double derivativeAt(double percentage) { 200 | percentage = Math.max(Math.min(percentage, 1), 0); 201 | 202 | double x_hat = percentage * knot_distance_; 203 | double yp_hat = (5 * a_ * x_hat + 4 * b_) * x_hat * x_hat * x_hat + 3 * c_ * x_hat * x_hat 204 | + 2 * d_ * x_hat + e_; 205 | 206 | return yp_hat; 207 | } 208 | 209 | private double secondDerivativeAt(double percentage) { 210 | percentage = Math.max(Math.min(percentage, 1), 0); 211 | 212 | double x_hat = percentage * knot_distance_; 213 | double ypp_hat = (20 * a_ * x_hat + 12 * b_) * x_hat * x_hat + 6 * c_ * x_hat + 2 * d_; 214 | 215 | return ypp_hat; 216 | } 217 | 218 | public double angleAt(double percentage) { 219 | double angle = ChezyMath.boundAngle0to2PiRadians( 220 | Math.atan(derivativeAt(percentage)) + theta_offset_); 221 | return angle; 222 | } 223 | 224 | public double angleChangeAt(double percentage) { 225 | return ChezyMath.boundAngleNegPiToPiRadians( 226 | Math.atan(secondDerivativeAt(percentage))); 227 | } 228 | 229 | public String toString() { 230 | return "a=" + a_ + "; b=" + b_ + "; c=" + c_ + "; d=" + d_ + "; e=" + e_; 231 | } 232 | } 233 | -------------------------------------------------------------------------------- /src/com/team254/lib/trajectory/TrajectoryGenerator.java: -------------------------------------------------------------------------------- 1 | package com.team254.lib.trajectory; 2 | 3 | /** 4 | * Factory class for creating Trajectories. 5 | * 6 | * @author Jared341 7 | */ 8 | public class TrajectoryGenerator { 9 | 10 | ///// INNER CLASSES ///// 11 | public static class Config { 12 | 13 | public double dt; 14 | public double max_vel; 15 | public double max_acc; 16 | public double max_jerk; 17 | } 18 | 19 | public static class Strategy { 20 | 21 | // J2ME Enum pattern 22 | private final String value_; 23 | 24 | private Strategy(String value) { 25 | value_ = value; 26 | } 27 | 28 | public String toString() { 29 | return value_; 30 | } 31 | } 32 | 33 | private static class IntegrationMethod { 34 | 35 | // J2ME Enum pattern 36 | private final String value_; 37 | 38 | private IntegrationMethod(String value) { 39 | value_ = value; 40 | } 41 | 42 | public String toString() { 43 | return value_; 44 | } 45 | } 46 | 47 | ///// CONSTANTS ///// 48 | // Move from the start to the goal at a constant velocity. Acceleration and 49 | // jerk limits are ignored, and start and goal vel are ignored (since the 50 | // velocity at all times will be max_vel). 51 | public static final Strategy StepStrategy = new Strategy("StepStrategy"); 52 | 53 | // Move from the start to the goal with a trapezoidal speed profile. 54 | // Jerk limits are ignored. 55 | public static final Strategy TrapezoidalStrategy 56 | = new Strategy("TrapezoidalStrategy"); 57 | 58 | // Move from the start tot he goal with a S-curve speed profile. All limits 59 | // are obeyed, but only point-to-point moves (start_vel = goal_vel = 0) are 60 | // currently supported. 61 | public static final Strategy SCurvesStrategy 62 | = new Strategy("SCurvesStrategy"); 63 | 64 | // Choose one of the above strategies based on the inputs. 65 | public static final Strategy AutomaticStrategy 66 | = new Strategy("AutomaticStrategy"); 67 | 68 | private static final IntegrationMethod RectangularIntegration 69 | = new IntegrationMethod("RectangularIntegration"); 70 | 71 | private static final IntegrationMethod TrapezoidalIntegration 72 | = new IntegrationMethod("TrapezoidalIntegration"); 73 | 74 | ///// METHODS ///// 75 | /** 76 | * Generate a trajectory from a start state to a goal state. 77 | * 78 | * Read the notes on each of the Strategies defined above, as certain 79 | * arguments are ignored for some strategies. 80 | * 81 | * @param config Definition of constraints and sampling rate (WARNING: Some 82 | * may be ignored) 83 | * @param strategy Which generator to use 84 | * @param start_vel The starting velocity (WARNING: May be ignored) 85 | * @param start_heading The starting heading 86 | * @param goal_pos The goal position 87 | * @param goal_vel The goal velocity (WARNING: May be ignored) 88 | * @param goal_heading The goal heading 89 | * @return A Trajectory that satisfies the relevant constraints and end 90 | * conditions. 91 | */ 92 | public static Trajectory generate( 93 | Config config, 94 | Strategy strategy, 95 | double start_vel, 96 | double start_heading, 97 | double goal_pos, 98 | double goal_vel, 99 | double goal_heading) { 100 | // Choose an automatic strategy. 101 | if (strategy == AutomaticStrategy) { 102 | strategy = chooseStrategy(start_vel, goal_vel, config.max_vel); 103 | } 104 | 105 | Trajectory traj; 106 | if (strategy == StepStrategy) { 107 | double impulse = (goal_pos / config.max_vel) / config.dt; 108 | 109 | // Round down, meaning we may undershoot by less than max_vel*dt. 110 | // This is due to discretization and avoids a strange final 111 | // velocity. 112 | int time = (int) (Math.floor(impulse)); 113 | traj = secondOrderFilter(1, 1, config.dt, config.max_vel, 114 | config.max_vel, impulse, time, TrapezoidalIntegration); 115 | 116 | } else if (strategy == TrapezoidalStrategy) { 117 | // How fast can we go given maximum acceleration and deceleration? 118 | double start_discount = .5 * start_vel * start_vel / config.max_acc; 119 | double end_discount = .5 * goal_vel * goal_vel / config.max_acc; 120 | 121 | double adjusted_max_vel = Math.min(config.max_vel, 122 | Math.sqrt(config.max_acc * goal_pos - start_discount 123 | - end_discount)); 124 | double t_rampup = (adjusted_max_vel - start_vel) / config.max_acc; 125 | double x_rampup = start_vel * t_rampup + .5 * config.max_acc 126 | * t_rampup * t_rampup; 127 | double t_rampdown = (adjusted_max_vel - goal_vel) / config.max_acc; 128 | double x_rampdown = adjusted_max_vel * t_rampdown - .5 129 | * config.max_acc * t_rampdown * t_rampdown; 130 | double x_cruise = goal_pos - x_rampdown - x_rampup; 131 | 132 | // The +.5 is to round to nearest 133 | int time = (int) ((t_rampup + t_rampdown + x_cruise 134 | / adjusted_max_vel) / config.dt + .5); 135 | 136 | // Compute the length of the linear filters and impulse. 137 | int f1_length = (int) Math.ceil((adjusted_max_vel 138 | / config.max_acc) / config.dt); 139 | double impulse = (goal_pos / adjusted_max_vel) / config.dt 140 | - start_vel / config.max_acc / config.dt 141 | + start_discount + end_discount; 142 | traj = secondOrderFilter(f1_length, 1, config.dt, start_vel, 143 | adjusted_max_vel, impulse, time, TrapezoidalIntegration); 144 | 145 | } else if (strategy == SCurvesStrategy) { 146 | // How fast can we go given maximum acceleration and deceleration? 147 | double adjusted_max_vel = Math.min(config.max_vel, 148 | (-config.max_acc * config.max_acc + Math.sqrt(config.max_acc 149 | * config.max_acc * config.max_acc * config.max_acc 150 | + 4 * config.max_jerk * config.max_jerk * config.max_acc 151 | * goal_pos)) / (2 * config.max_jerk)); 152 | 153 | // Compute the length of the linear filters and impulse. 154 | int f1_length = (int) Math.ceil((adjusted_max_vel 155 | / config.max_acc) / config.dt); 156 | int f2_length = (int) Math.ceil((config.max_acc 157 | / config.max_jerk) / config.dt); 158 | double impulse = (goal_pos / adjusted_max_vel) / config.dt; 159 | int time = (int) (Math.ceil(f1_length + f2_length + impulse)); 160 | traj = secondOrderFilter(f1_length, f2_length, config.dt, 0, 161 | adjusted_max_vel, impulse, time, TrapezoidalIntegration); 162 | 163 | } else { 164 | return null; 165 | } 166 | 167 | // Now assign headings by interpolating along the path. 168 | // Don't do any wrapping because we don't know units. 169 | double total_heading_change = goal_heading - start_heading; 170 | for (int i = 0; i < traj.getNumSegments(); ++i) { 171 | traj.segments_[i].heading = start_heading + total_heading_change 172 | * (traj.segments_[i].pos) 173 | / traj.segments_[traj.getNumSegments() - 1].pos; 174 | } 175 | 176 | return traj; 177 | } 178 | 179 | private static Trajectory secondOrderFilter( 180 | int f1_length, 181 | int f2_length, 182 | double dt, 183 | double start_vel, 184 | double max_vel, 185 | double total_impulse, 186 | int length, 187 | IntegrationMethod integration) { 188 | if (length <= 0) { 189 | return null; 190 | } 191 | Trajectory traj = new Trajectory(length); 192 | 193 | Trajectory.Segment last = new Trajectory.Segment(); 194 | // First segment is easy 195 | last.pos = 0; 196 | last.vel = start_vel; 197 | last.acc = 0; 198 | last.jerk = 0; 199 | last.dt = dt; 200 | 201 | // f2 is the average of the last f2_length samples from f1, so while we 202 | // can recursively compute f2's sum, we need to keep a buffer for f1. 203 | double[] f1 = new double[length]; 204 | f1[0] = (start_vel / max_vel) * f1_length; 205 | double f2; 206 | for (int i = 0; i < length; ++i) { 207 | // Apply input 208 | double input = Math.min(total_impulse, 1); 209 | if (input < 1) { 210 | // The impulse is over, so decelerate 211 | input -= 1; 212 | total_impulse = 0; 213 | } else { 214 | total_impulse -= input; 215 | } 216 | 217 | // Filter through F1 218 | double f1_last; 219 | if (i > 0) { 220 | f1_last = f1[i - 1]; 221 | } else { 222 | f1_last = f1[0]; 223 | } 224 | f1[i] = Math.max(0.0, Math.min(f1_length, f1_last + input)); 225 | 226 | f2 = 0; 227 | // Filter through F2 228 | for (int j = 0; j < f2_length; ++j) { 229 | if (i - j < 0) { 230 | break; 231 | } 232 | 233 | f2 += f1[i - j]; 234 | } 235 | f2 = f2 / f1_length; 236 | 237 | // Velocity is the normalized sum of f2 * the max velocity 238 | traj.segments_[i].vel = f2 / f2_length * max_vel; 239 | 240 | if (integration == RectangularIntegration) { 241 | traj.segments_[i].pos = traj.segments_[i].vel * dt + last.pos; 242 | } else if (integration == TrapezoidalIntegration) { 243 | traj.segments_[i].pos = (last.vel 244 | + traj.segments_[i].vel) / 2.0 * dt + last.pos; 245 | } 246 | traj.segments_[i].x = traj.segments_[i].pos; 247 | traj.segments_[i].y = 0; 248 | 249 | // Acceleration and jerk are the differences in velocity and 250 | // acceleration, respectively. 251 | traj.segments_[i].acc = (traj.segments_[i].vel - last.vel) / dt; 252 | traj.segments_[i].jerk = (traj.segments_[i].acc - last.acc) / dt; 253 | traj.segments_[i].dt = dt; 254 | 255 | last = traj.segments_[i]; 256 | } 257 | 258 | return traj; 259 | } 260 | 261 | public static Strategy chooseStrategy(double start_vel, double goal_vel, 262 | double max_vel) { 263 | Strategy strategy; 264 | if (start_vel == goal_vel && start_vel == max_vel) { 265 | strategy = StepStrategy; 266 | } else if (start_vel == goal_vel && start_vel == 0) { 267 | strategy = SCurvesStrategy; 268 | } else { 269 | strategy = TrapezoidalStrategy; 270 | } 271 | return strategy; 272 | } 273 | } 274 | -------------------------------------------------------------------------------- /nbproject/build-impl.xml: -------------------------------------------------------------------------------- 1 | 2 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | Must set src.dir 224 | Must set test.src.dir 225 | Must set build.dir 226 | Must set dist.dir 227 | Must set build.classes.dir 228 | Must set dist.javadoc.dir 229 | Must set build.test.classes.dir 230 | Must set build.test.results.dir 231 | Must set build.classes.excludes 232 | Must set dist.jar 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | Must set javac.includes 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | No tests executed. 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 572 | 573 | 574 | 575 | 576 | 577 | 578 | 579 | 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 590 | 591 | 592 | 593 | 594 | 595 | 596 | 597 | 598 | 599 | 600 | 601 | 602 | 603 | 604 | 605 | 606 | 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | 630 | 631 | 632 | 633 | 634 | 635 | 636 | 637 | 638 | 639 | 640 | 641 | 642 | 643 | 644 | 645 | 646 | 647 | 648 | 649 | 650 | 651 | 652 | 653 | 654 | 655 | 656 | 657 | 658 | 659 | 660 | 661 | 662 | 663 | 664 | 665 | 666 | 667 | 670 | 671 | 672 | 673 | 674 | 675 | 676 | 677 | 678 | 679 | 680 | 681 | 682 | 683 | 684 | 685 | 686 | 687 | 688 | 689 | 690 | 691 | 692 | 693 | 694 | 695 | 696 | 697 | 698 | 699 | 700 | 701 | 702 | 703 | 704 | 705 | 706 | 707 | 708 | 709 | 710 | 711 | 712 | Must set JVM to use for profiling in profiler.info.jvm 713 | Must set profiler agent JVM arguments in profiler.info.jvmargs.agent 714 | 715 | 718 | 719 | 720 | 721 | 722 | 723 | 724 | 725 | 726 | 727 | 728 | 729 | 730 | 731 | 732 | 733 | 734 | 735 | 736 | 737 | 738 | 739 | 740 | 741 | 742 | 743 | 744 | 745 | 746 | 747 | 748 | 749 | 750 | 751 | 752 | 753 | 754 | 755 | 756 | 757 | 758 | 759 | 760 | 761 | 762 | 763 | 764 | 765 | 766 | 767 | 768 | 769 | 770 | 771 | 772 | 773 | 774 | 775 | 776 | 777 | 778 | 779 | 780 | 781 | 782 | 783 | 784 | 785 | 786 | 787 | 788 | 789 | 790 | 791 | 792 | 793 | 794 | 795 | 796 | 797 | 798 | 799 | 800 | 801 | 802 | 803 | 804 | 805 | 806 | 807 | 808 | 809 | 810 | 811 | 812 | 813 | 814 | 815 | 816 | 817 | 818 | 819 | 820 | 821 | 822 | 823 | 824 | 825 | 826 | 827 | 828 | 829 | 830 | 831 | 832 | 833 | 834 | 835 | 836 | 837 | 838 | 839 | 840 | 841 | 842 | 843 | 844 | 845 | 846 | 847 | 848 | 849 | 850 | 851 | 852 | 853 | 854 | 855 | 856 | 857 | 858 | 859 | 860 | 861 | 862 | 863 | 864 | 865 | 866 | 867 | 868 | 869 | 870 | 871 | 872 | 873 | 874 | 875 | 876 | 881 | 882 | 883 | 884 | 885 | 886 | 887 | 888 | 889 | 890 | 891 | 892 | 893 | 894 | 895 | 896 | 897 | 898 | 899 | 900 | 901 | 902 | 903 | 904 | 905 | 906 | 907 | 908 | 909 | 910 | 911 | 912 | 913 | 914 | 915 | 916 | 917 | 918 | 919 | 920 | 921 | 922 | 923 | 924 | 925 | 926 | 927 | 928 | 929 | 930 | 931 | 932 | 933 | 934 | 935 | 936 | 937 | 938 | 939 | 940 | 941 | Must select some files in the IDE or set javac.includes 942 | 943 | 944 | 945 | 946 | 947 | 948 | 949 | 950 | 955 | 956 | 957 | 958 | 959 | 960 | 961 | 962 | 963 | 964 | 965 | 966 | 967 | 968 | 969 | 970 | 971 | 972 | 973 | 974 | 975 | 976 | 977 | 978 | 979 | 980 | 981 | 982 | 983 | 984 | 985 | 986 | 987 | 988 | 989 | 990 | 991 | To run this application from the command line without Ant, try: 992 | 993 | java -jar "${dist.jar.resolved}" 994 | 995 | 996 | 997 | 998 | 999 | 1000 | 1001 | 1002 | 1003 | 1004 | 1005 | 1006 | 1007 | 1008 | 1009 | 1010 | 1011 | 1012 | 1013 | 1014 | 1015 | 1016 | 1017 | 1018 | 1019 | 1020 | 1021 | 1022 | 1023 | 1024 | 1029 | 1030 | 1031 | 1032 | 1033 | 1034 | 1035 | 1036 | 1037 | 1038 | 1039 | 1040 | Must select one file in the IDE or set run.class 1041 | 1042 | 1043 | 1044 | Must select one file in the IDE or set run.class 1045 | 1046 | 1047 | 1052 | 1053 | 1054 | 1055 | 1056 | 1057 | 1058 | 1059 | 1060 | 1061 | 1062 | 1063 | 1064 | 1065 | 1066 | 1067 | 1068 | 1069 | 1070 | 1071 | Must select one file in the IDE or set debug.class 1072 | 1073 | 1074 | 1075 | 1076 | Must select one file in the IDE or set debug.class 1077 | 1078 | 1079 | 1080 | 1081 | Must set fix.includes 1082 | 1083 | 1084 | 1085 | 1086 | 1087 | 1088 | 1093 | 1096 | 1097 | This target only works when run from inside the NetBeans IDE. 1098 | 1099 | 1100 | 1101 | 1102 | 1103 | 1104 | 1105 | 1106 | Must select one file in the IDE or set profile.class 1107 | This target only works when run from inside the NetBeans IDE. 1108 | 1109 | 1110 | 1111 | 1112 | 1113 | 1114 | 1115 | 1116 | This target only works when run from inside the NetBeans IDE. 1117 | 1118 | 1119 | 1120 | 1121 | 1122 | 1123 | 1124 | 1125 | 1126 | 1127 | 1128 | 1129 | This target only works when run from inside the NetBeans IDE. 1130 | 1131 | 1132 | 1133 | 1134 | 1135 | 1136 | 1137 | 1138 | 1139 | 1140 | 1141 | 1142 | 1143 | 1144 | 1145 | 1146 | 1147 | 1148 | 1149 | 1150 | 1151 | 1154 | 1155 | 1156 | 1157 | 1158 | 1159 | 1160 | 1161 | 1162 | 1163 | 1164 | 1165 | 1166 | 1167 | Must select one file in the IDE or set run.class 1168 | 1169 | 1170 | 1171 | 1172 | 1173 | Must select some files in the IDE or set test.includes 1174 | 1175 | 1176 | 1177 | 1178 | Must select one file in the IDE or set run.class 1179 | 1180 | 1181 | 1182 | 1183 | Must select one file in the IDE or set applet.url 1184 | 1185 | 1186 | 1187 | 1192 | 1193 | 1194 | 1195 | 1196 | 1197 | 1198 | 1199 | 1200 | 1201 | 1202 | 1203 | 1204 | 1205 | 1206 | 1207 | 1208 | 1209 | 1210 | 1211 | 1212 | 1213 | 1214 | 1215 | 1216 | 1217 | 1218 | 1219 | 1220 | 1221 | 1222 | 1223 | 1224 | 1225 | 1226 | 1227 | 1228 | 1233 | 1234 | 1235 | 1236 | 1237 | 1238 | 1239 | 1240 | 1241 | 1242 | 1243 | 1244 | 1245 | 1246 | 1247 | 1248 | 1249 | 1250 | 1251 | 1252 | 1253 | 1254 | 1255 | 1256 | 1257 | 1258 | 1259 | Must select some files in the IDE or set javac.includes 1260 | 1261 | 1262 | 1263 | 1264 | 1265 | 1266 | 1267 | 1268 | 1269 | 1270 | 1271 | 1276 | 1277 | 1278 | 1279 | 1280 | 1281 | 1282 | 1283 | Some tests failed; see details above. 1284 | 1285 | 1286 | 1287 | 1288 | 1289 | 1290 | 1291 | 1292 | Must select some files in the IDE or set test.includes 1293 | 1294 | 1295 | 1296 | Some tests failed; see details above. 1297 | 1298 | 1299 | 1300 | Must select some files in the IDE or set test.class 1301 | Must select some method in the IDE or set test.method 1302 | 1303 | 1304 | 1305 | Some tests failed; see details above. 1306 | 1307 | 1308 | 1313 | 1314 | Must select one file in the IDE or set test.class 1315 | 1316 | 1317 | 1318 | Must select one file in the IDE or set test.class 1319 | Must select some method in the IDE or set test.method 1320 | 1321 | 1322 | 1323 | 1324 | 1325 | 1326 | 1327 | 1328 | 1329 | 1330 | 1331 | 1336 | 1337 | Must select one file in the IDE or set applet.url 1338 | 1339 | 1340 | 1341 | 1342 | 1343 | 1344 | 1349 | 1350 | Must select one file in the IDE or set applet.url 1351 | 1352 | 1353 | 1354 | 1355 | 1356 | 1357 | 1358 | 1363 | 1364 | 1365 | 1366 | 1367 | 1368 | 1369 | 1370 | 1371 | 1372 | 1373 | 1374 | 1375 | 1376 | 1377 | 1378 | 1379 | 1380 | 1381 | 1382 | 1383 | 1384 | 1385 | 1386 | 1387 | 1388 | 1389 | 1390 | 1391 | 1392 | 1393 | 1394 | 1395 | 1396 | 1397 | 1398 | 1399 | 1400 | 1401 | 1402 | 1403 | 1404 | 1405 | 1406 | 1407 | 1408 | --------------------------------------------------------------------------------