Skip to content

Commit 4e02cf0

Browse files
jlbabilinojlbabilino
committed
Upgraded to new trajectory optimization library
1 parent 57e533a commit 4e02cf0

14 files changed

+96
-54
lines changed

‎app/build.gradle.kts

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,13 @@ repositories {
1919
maven {
2020
url = uri("https://jitpack.io")
2121
}
22+
maven {
23+
url = uri("https://maven.pkg.github.com/jlbabilino/HelixTrajectory")
24+
credentials {
25+
username = "jlbabilino"
26+
password = "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"
27+
}
28+
}
2229
}
2330

2431
dependencies {
@@ -31,7 +38,7 @@ dependencies {
3138
implementation("systems.uom:systems-quantity:2.1")
3239
// implementation "org.djunits:djunits:4.01.07"
3340

34-
implementation(files("../libs/jar/helixtrajectoryj.jar"))
41+
implementation("org.team2363:helixtrajectory:0.0.0-pre4")
3542
}
3643

3744
application {
@@ -74,7 +81,7 @@ tasks.register("jpackage") {
7481
"--input", jarDir,
7582
"--java-options", javaOptions,
7683
"--main-jar", jarName,
77-
// "--main-class", "org.team2363.helixnavigator.Main",
84+
"--main-class", "org.team2363.helixnavigator.Main",
7885
"--dest", dest,
7986
"--name", project.name,
8087
"--type", "dmg",

‎app/src/main/java/org/team2363/helixnavigator/document/HPath.java

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -11,20 +11,20 @@
1111
import org.team2363.helixnavigator.document.waypoint.HInitialGuessWaypoint;
1212
import org.team2363.helixnavigator.document.waypoint.HSoftWaypoint;
1313
import org.team2363.helixnavigator.document.waypoint.HWaypoint;
14+
import org.team2363.helixtrajectory.HolonomicPath;
15+
import org.team2363.helixtrajectory.HolonomicWaypoint;
1416
import org.team2363.helixtrajectory.InitialGuessPoint;
15-
import org.team2363.helixtrajectory.Path;
16-
import org.team2363.helixtrajectory.Waypoint;
1717

1818
import com.jlbabilino.json.DeserializedJSONConstructor;
1919
import com.jlbabilino.json.DeserializedJSONObjectValue;
2020
import com.jlbabilino.json.DeserializedJSONTarget;
2121
import com.jlbabilino.json.InvalidJSONTranslationConfiguration;
2222
import com.jlbabilino.json.JSONDeserializable;
23+
import com.jlbabilino.json.JSONEntry.JSONType;
2324
import com.jlbabilino.json.JSONSerializable;
2425
import com.jlbabilino.json.JSONSerializer;
2526
import com.jlbabilino.json.JSONSerializerException;
2627
import com.jlbabilino.json.SerializedJSONObjectValue;
27-
import com.jlbabilino.json.JSONEntry.JSONType;
2828

2929
import javafx.beans.property.ReadOnlyBooleanProperty;
3030
import javafx.beans.property.ReadOnlyBooleanWrapper;
@@ -205,8 +205,8 @@ public final HTrajectory getTrajectory() {
205205
return trajectory.get();
206206
}
207207

208-
public Path toPath() {
209-
List<Waypoint> htWaypoints = new ArrayList<>();
208+
public HolonomicPath toPath() {
209+
List<HolonomicWaypoint> htWaypoints = new ArrayList<>();
210210
int i = 0;
211211
while (waypoints.get(i).isInitialGuess()) {
212212
i++;
@@ -219,22 +219,21 @@ public Path toPath() {
219219
initialGuessPoints.add(((HInitialGuessWaypoint) waypoints.get(i)).toInitialGuessPoint());
220220
i++;
221221
}
222-
InitialGuessPoint[] initialGuessPointsArray = initialGuessPoints.toArray(new InitialGuessPoint[0]);
223222
switch (waypoints.get(waypointIndex).getWaypointType()) {
224223
case SOFT:
225-
htWaypoints.add(((HSoftWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPointsArray));
224+
htWaypoints.add(((HSoftWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints));
226225
break;
227226
case HARD:
228-
htWaypoints.add(((HHardWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPointsArray));
227+
htWaypoints.add(((HHardWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints));
229228
break;
230229
case CUSTOM:
231-
htWaypoints.add(((HCustomWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPointsArray));
230+
htWaypoints.add(((HCustomWaypoint) waypoints.get(waypointIndex)).toWaypoint(initialGuessPoints));
232231
break;
233232
default:
234233
break;
235234
}
236235
}
237-
return new Path(htWaypoints.toArray(new Waypoint[0]));
236+
return new HolonomicPath(htWaypoints);
238237
}
239238

240239
@Override

‎app/src/main/java/org/team2363/helixnavigator/document/HRobotConfiguration.java

Lines changed: 11 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,20 @@
11
package org.team2363.helixnavigator.document;
22

3+
import java.util.List;
4+
35
import org.team2363.helixnavigator.document.obstacle.HObstacle;
46
import org.team2363.helixnavigator.global.Standards.DefaultRobotConfiguration;
57
import org.team2363.helixtrajectory.Obstacle;
6-
import org.team2363.helixtrajectory.SwerveDrive;
8+
import org.team2363.helixtrajectory.SwerveDrivetrain;
9+
import org.team2363.helixtrajectory.SwerveModule;
710

811
import com.jlbabilino.json.DeserializedJSONConstructor;
912
import com.jlbabilino.json.DeserializedJSONObjectValue;
1013
import com.jlbabilino.json.DeserializedJSONTarget;
1114
import com.jlbabilino.json.JSONDeserializable;
15+
import com.jlbabilino.json.JSONEntry.JSONType;
1216
import com.jlbabilino.json.JSONSerializable;
1317
import com.jlbabilino.json.SerializedJSONObjectValue;
14-
import com.jlbabilino.json.JSONEntry.JSONType;
1518

1619
import javafx.beans.property.DoubleProperty;
1720
import javafx.beans.property.IntegerProperty;
@@ -212,9 +215,11 @@ public void importConfiguration(HRobotConfiguration otherConfiguration) {
212215
setBumpers(otherConfiguration.getBumpers());
213216
}
214217

215-
public SwerveDrive toDrive() {
216-
return new SwerveDrive(getWheelHorizontalDistance(), getWheelVerticalDistance(),
217-
getMass(), getMomentOfInertia(),
218-
getMotorMaxAngularSpeed(), getMotorMaxTorque(), getWheelRadius(), getBumpers());
218+
public SwerveDrivetrain toDrive() {
219+
SwerveModule module0 = new SwerveModule(+getWheelHorizontalDistance(), +getWheelVerticalDistance(), getWheelRadius(), getMotorMaxAngularSpeed(), getMotorMaxTorque());
220+
SwerveModule module1 = new SwerveModule(+getWheelHorizontalDistance(), -getWheelVerticalDistance(), getWheelRadius(), getMotorMaxAngularSpeed(), getMotorMaxTorque());
221+
SwerveModule module2 = new SwerveModule(-getWheelHorizontalDistance(), +getWheelVerticalDistance(), getWheelRadius(), getMotorMaxAngularSpeed(), getMotorMaxTorque());
222+
SwerveModule module3 = new SwerveModule(-getWheelHorizontalDistance(), -getWheelVerticalDistance(), getWheelRadius(), getMotorMaxAngularSpeed(), getMotorMaxTorque());
223+
return new SwerveDrivetrain(getMass(), getMomentOfInertia(), List.of(module0, module1, module2, module3), getBumpers());
219224
}
220225
}

‎app/src/main/java/org/team2363/helixnavigator/document/HTrajectory.java

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,9 @@
55
import java.util.List;
66

77
import org.team2363.helixnavigator.document.waypoint.HHardWaypoint;
8-
import org.team2363.helixtrajectory.Trajectory;
8+
import org.team2363.helixtrajectory.HolonomicTrajectory;
9+
import org.team2363.helixtrajectory.HolonomicTrajectorySample;
10+
import org.team2363.helixtrajectory.HolonomicTrajectorySegment;
911

1012
import com.jlbabilino.json.DeserializedJSONConstructor;
1113
import com.jlbabilino.json.DeserializedJSONEntry;
@@ -101,10 +103,14 @@ public final double getTimestamp() {
101103
return timestamp.get();
102104
}
103105

104-
public static HTrajectory fromTrajectory(Trajectory trajectory) {
106+
public static HTrajectory fromTrajectory(HolonomicTrajectory trajectory) {
105107
List<HTrajectorySample> hSamples = new ArrayList<>();
106-
for (int i = 0; i < trajectory.length(); i++) {
107-
hSamples.add(HTrajectorySample.fromTrajectorySample(trajectory.get(i)));
108+
double timestamp = 0.0;
109+
for (HolonomicTrajectorySegment segment : trajectory.holonomicSegments) {
110+
for (HolonomicTrajectorySample sample : segment.holonomicSamples) {
111+
timestamp += sample.intervalDuration;
112+
hSamples.add(HTrajectorySample.fromTrajectorySample(sample, timestamp));
113+
}
108114
}
109115
return new HTrajectory(hSamples);
110116
}

‎app/src/main/java/org/team2363/helixnavigator/document/HTrajectorySample.java

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,13 @@
11
package org.team2363.helixnavigator.document;
22

3-
import org.team2363.helixtrajectory.TrajectorySample;
3+
import org.team2363.helixtrajectory.HolonomicTrajectorySample;
44

55
import com.jlbabilino.json.DeserializedJSONConstructor;
66
import com.jlbabilino.json.DeserializedJSONObjectValue;
77
import com.jlbabilino.json.JSONDeserializable;
8+
import com.jlbabilino.json.JSONEntry.JSONType;
89
import com.jlbabilino.json.JSONSerializable;
910
import com.jlbabilino.json.SerializedJSONObjectValue;
10-
import com.jlbabilino.json.JSONEntry.JSONType;
1111

1212
@JSONSerializable(JSONType.OBJECT)
1313
@JSONDeserializable({JSONType.OBJECT})
@@ -58,8 +58,8 @@ public static HTrajectorySample interpolate(HTrajectorySample a, HTrajectorySamp
5858
return new HTrajectorySample(ts, x, y, heading, vx, vy, omega);
5959
}
6060

61-
public static HTrajectorySample fromTrajectorySample(TrajectorySample sample) {
62-
return new HTrajectorySample(sample.ts, sample.x, sample.y, sample.heading,
63-
sample.vx, sample.vy, sample.omega);
61+
public static HTrajectorySample fromTrajectorySample(HolonomicTrajectorySample sample, double timestamp) {
62+
return new HTrajectorySample(timestamp, sample.x, sample.y, sample.heading,
63+
sample.velocityX, sample.velocityY, sample.angularVelocity);
6464
}
6565
}

‎app/src/main/java/org/team2363/helixnavigator/document/obstacle/HCircleObstacle.java

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package org.team2363.helixnavigator.document.obstacle;
22

3+
import java.util.List;
4+
35
import org.team2363.helixtrajectory.Obstacle;
46
import org.team2363.helixtrajectory.ObstaclePoint;
57

@@ -25,7 +27,7 @@ public HCircleObstacle() {
2527

2628
@Override
2729
public Obstacle toObstacle() {
28-
return new Obstacle(getSafetyDistance() + getRadius(), new ObstaclePoint(getCenterX(), getCenterY()));
30+
return new Obstacle(getSafetyDistance() + getRadius(), true, List.of(new ObstaclePoint(getCenterX(), getCenterY())));
2931
}
3032

3133
@Override

‎app/src/main/java/org/team2363/helixnavigator/document/obstacle/HObstacle.java

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package org.team2363.helixnavigator.document.obstacle;
22

3+
import java.util.List;
4+
35
import org.team2363.helixnavigator.document.HPathElement;
46
import org.team2363.helixnavigator.global.Standards.DefaultRobotConfiguration;
57
import org.team2363.helixtrajectory.Obstacle;
@@ -89,6 +91,6 @@ public static Obstacle defaultBumpers() {
8991
ObstaclePoint point2 = new ObstaclePoint(-DefaultRobotConfiguration.BUMPER_LENGTH / 2, +DefaultRobotConfiguration.BUMPER_WIDTH / 2);
9092
ObstaclePoint point3 = new ObstaclePoint(-DefaultRobotConfiguration.BUMPER_LENGTH / 2, -DefaultRobotConfiguration.BUMPER_WIDTH / 2);
9193
ObstaclePoint point4 = new ObstaclePoint(+DefaultRobotConfiguration.BUMPER_LENGTH / 2, -DefaultRobotConfiguration.BUMPER_WIDTH / 2);
92-
return new Obstacle(0, point1, point2, point3, point4);
94+
return new Obstacle(0, true, List.of(point1, point2, point3, point4));
9395
}
9496
}

‎app/src/main/java/org/team2363/helixnavigator/document/obstacle/HPolygonObstacle.java

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package org.team2363.helixnavigator.document.obstacle;
22

3+
import java.util.ArrayList;
34
import java.util.List;
45

56
import org.team2363.helixtrajectory.Obstacle;
@@ -38,11 +39,11 @@ public HPolygonObstacle(@DeserializedJSONObjectValue(key = "points") List<? exte
3839

3940
@Override
4041
public Obstacle toObstacle() {
41-
ObstaclePoint[] obstaclePoints = new ObstaclePoint[points.size()];
42+
List<ObstaclePoint> obstaclePoints = new ArrayList<>(points.size());
4243
for (int i = 0; i < points.size(); i++) {
43-
obstaclePoints[i] = points.get(i).toObstaclePoint();
44+
obstaclePoints.add(points.get(i).toObstaclePoint());
4445
}
45-
return new Obstacle(getSafetyDistance(), obstaclePoints);
46+
return new Obstacle(getSafetyDistance(), true, obstaclePoints);
4647
}
4748

4849
@Override

‎app/src/main/java/org/team2363/helixnavigator/document/obstacle/HRectangleObstacle.java

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package org.team2363.helixnavigator.document.obstacle;
22

3+
import java.util.List;
4+
35
import org.team2363.helixtrajectory.Obstacle;
46
import org.team2363.helixtrajectory.ObstaclePoint;
57

@@ -38,11 +40,11 @@ public Obstacle toObstacle() {
3840
getCenterY() + diagonal * Math.sin(getRotateAngle() + angles[1]),
3941
getCenterY() + diagonal * Math.sin(getRotateAngle() + angles[2]),
4042
getCenterY() + diagonal * Math.sin(getRotateAngle() + angles[3])};
41-
ObstaclePoint[] obstaclePoints = {new ObstaclePoint(x[0], y[0]),
43+
List<ObstaclePoint> obstaclePoints = List.of(new ObstaclePoint(x[0], y[0]),
4244
new ObstaclePoint(x[1], y[1]),
4345
new ObstaclePoint(x[2], y[2]),
44-
new ObstaclePoint(x[3], y[3])};
45-
return new Obstacle(getSafetyDistance(), obstaclePoints);
46+
new ObstaclePoint(x[3], y[3]));
47+
return new Obstacle(getSafetyDistance(), true, obstaclePoints);
4648
}
4749

4850
@Override

‎app/src/main/java/org/team2363/helixnavigator/document/waypoint/HCustomWaypoint.java

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,9 @@
11
package org.team2363.helixnavigator.document.waypoint;
22

3+
import java.util.List;
4+
5+
import org.team2363.helixtrajectory.HolonomicWaypoint;
36
import org.team2363.helixtrajectory.InitialGuessPoint;
4-
import org.team2363.helixtrajectory.Waypoint;
57

68
import com.jlbabilino.json.DeserializedJSONConstructor;
79
import com.jlbabilino.json.DeserializedJSONObjectValue;
@@ -182,9 +184,9 @@ public final boolean isAngularVelocityConstrained() {
182184
return angularVelocityConstrained.get();
183185
}
184186

185-
public Waypoint toWaypoint(InitialGuessPoint[] initialGuessPoints) {
186-
return new Waypoint(getX(), getY(), getHeading(), getVelocityX(), getVelocityY(), getAngularVelocity(),
187+
public HolonomicWaypoint toWaypoint(List<InitialGuessPoint> initialGuessPoints) {
188+
return new HolonomicWaypoint(getX(), getY(), getHeading(), getVelocityX(), getVelocityY(), getAngularVelocity(),
187189
isXConstrained(), isYConstrained(), isHeadingConstrained(), isVelocityXConstrained(),
188-
isVelocityYConstrained(), isVelocityMagnitudeConstrained(), isAngularVelocityConstrained(), initialGuessPoints);
190+
isVelocityYConstrained(), isVelocityMagnitudeConstrained(), isAngularVelocityConstrained(), 100, initialGuessPoints, List.of());
189191
}
190192
}

0 commit comments

Comments
 (0)