Skip to content

Commit 8e06eb2

Browse files
liqynliqyn
committed
changes
1 parent 5c205e1 commit 8e06eb2

File tree

3 files changed

+207
-3
lines changed

3 files changed

+207
-3
lines changed

‎src/main/java/com/github/mittyrobotics/pathfollowing/Parametric.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -452,7 +452,7 @@ public double findClosestPointOnSpline(Point2D point, int steps, int iterations)
452452
//if distance is less than previous min, update distance and t
453453
double cur_d = getDistanceAtT(cur_t, point);
454454

455-
if(cur_d < cur_min.getX()) {
455+
if(cur_d < cur_min.getX() && cur_t >= 0 && cur_t <= 1) {
456456
cur_min = new Vector2D(cur_d, cur_t);
457457
}
458458
}
@@ -514,7 +514,7 @@ public double getTFromLength(double length) {
514514

515515
//Newton's method: length remaining length divided by derivative
516516
if(derivativeMagnitude > 0.0) {
517-
t -= (getGaussianQuadratureLength(t, 11) - length) / derivativeMagnitude;
517+
t -= (getGaussianQuadratureLength(t, 17) - length) / derivativeMagnitude;
518518
//Clamp to [0, 1]
519519
t = Math.min(1, Math.max(t, 0));
520520
}

‎src/main/java/com/github/mittyrobotics/pathfollowing/QuinticHermiteSplineGroup.java‎

Lines changed: 199 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,199 @@
1+
package com.github.mittyrobotics.pathfollowing;
2+
3+
import java.util.ArrayList;
4+
5+
public class QuinticHermiteSplineGroup extends Parametric {
6+
7+
//list of splines
8+
ArrayList<QuinticHermiteSpline> splines = new ArrayList<>();
9+
10+
/**
11+
* Creates a new spline group with an initial {@link QuinticHermiteSpline}
12+
* @param initialSpline {@link QuinticHermiteSpline} to initialize with
13+
*/
14+
public QuinticHermiteSplineGroup(QuinticHermiteSpline initialSpline) {
15+
length = initialSpline.getLength();
16+
splines.add(initialSpline);
17+
}
18+
19+
/**
20+
* Creates a new empty spline group
21+
*/
22+
public QuinticHermiteSplineGroup() {
23+
length = 0;
24+
}
25+
26+
/**
27+
* Adds a {@link QuinticHermiteSpline} to the list of splines
28+
* @param spline {@link QuinticHermiteSpline} to add
29+
*/
30+
public void addSpline(QuinticHermiteSpline spline) {
31+
length += spline.getLength();
32+
splines.add(spline);
33+
}
34+
35+
/**
36+
* Adds a {@link QuinticHermiteSpline} to the list of splines at the given index
37+
* @param index index to add the spline at
38+
* @param spline {@link QuinticHermiteSpline} to add
39+
*/
40+
public void addSpline(int index, QuinticHermiteSpline spline) {
41+
length += spline.getLength();
42+
splines.add(index, spline);
43+
}
44+
45+
/**
46+
* Get the {@link QuinticHermiteSpline} at the index of the list
47+
* @param index index to get spline at
48+
* @return {@link QuinticHermiteSpline} at the index of the list
49+
*/
50+
public QuinticHermiteSpline getSpline(int index) {
51+
return splines.get(index);
52+
}
53+
54+
/**
55+
* Removes a {@link QuinticHermiteSpline} from the list
56+
* @param spline {@link QuinticHermiteSpline} to remove
57+
*/
58+
public void removeSpline(QuinticHermiteSpline spline) {
59+
if(splines.contains(spline)) {
60+
length -= spline.getLength();
61+
splines.remove(spline);
62+
}
63+
}
64+
65+
/**
66+
* Removes the {@link QuinticHermiteSpline} at the given index
67+
* @param index index to remove the {@link QuinticHermiteSpline}
68+
*/
69+
public void removeSpline(int index) {
70+
length -= splines.get(index).getLength();
71+
splines.remove(index);
72+
}
73+
74+
/**
75+
* Gets the list of spline
76+
* @return an {@link ArrayList} of {@link QuinticHermiteSpline}s
77+
*/
78+
public ArrayList<QuinticHermiteSpline> getSplines() {
79+
return splines;
80+
}
81+
82+
/**
83+
* Returns the index of {@link QuinticHermiteSpline} associated with the given t
84+
* @param t t value to get spline of
85+
* @return the index of {@link QuinticHermiteSpline} associated with the given t
86+
*/
87+
public int getSplineFromT(double t) {
88+
if(t < 0) return 0;
89+
if(t >= 1) return splines.size() - 1;
90+
return (int) (t * splines.size());
91+
}
92+
93+
/**
94+
* Returns t for the spline given the overall t and the index of the spline
95+
* @param t overall t parameter
96+
* @param splineIndex index of the spline in array
97+
* @return t for the spline given the overall t and the index of the spline
98+
*/
99+
public double getSplineTFromT(double t, int splineIndex) {
100+
if(t < 0) return t * splines.size();
101+
if(t >= 1) return 1 + (t-1) * splines.size();
102+
return (t - (double) splineIndex / splines.size()) * splines.size();
103+
}
104+
105+
/**
106+
* Returns the {@link Point2D} at t
107+
* @param t t to get associated {@link Point2D}
108+
* @return {@link Point2D} at t
109+
*/
110+
@Override
111+
public Point2D getPoint(double t) {
112+
int index = getSplineFromT(t);
113+
return splines.get(index).getPoint(getSplineTFromT(t, index));
114+
}
115+
116+
/**
117+
* Returns the {@link Angle} at t
118+
* @param t t to get associated {@link Angle}
119+
* @return {@link Angle} at t
120+
*/
121+
@Override
122+
public Angle getAngle(double t) {
123+
int index = getSplineFromT(t);
124+
return splines.get(index).getAngle(getSplineTFromT(t, index));
125+
}
126+
127+
/**
128+
* Returns the {@link Pose2D} at t
129+
* @param t t to get associated {@link Pose2D}
130+
* @return {@link Pose2D} at t
131+
*/
132+
@Override
133+
public Pose2D getPose(double t) {
134+
int index = getSplineFromT(t);
135+
return splines.get(index).getPose(getSplineTFromT(t, index));
136+
}
137+
138+
/**
139+
* Returns the nth derivative as a {@link Point2D} at t
140+
* @param t t to get associated nth derivative
141+
* @param n degree of the derivative
142+
* @return the nth derivative as a {@link Point2D} at t
143+
*/
144+
@Override
145+
public Point2D getDerivative(double t, int n) {
146+
int index = getSplineFromT(t);
147+
return splines.get(index).getDerivative(getSplineTFromT(t, index), n);
148+
}
149+
150+
@Override
151+
public double findClosestPointOnSpline(Point2D point, int steps, int iterations) {
152+
Vector2D cur_min = new Vector2D(Double.POSITIVE_INFINITY, 0);
153+
double i = 0;
154+
for(QuinticHermiteSpline spline : splines) {
155+
double t = spline.findClosestPointOnSpline(point, steps, iterations);
156+
if(spline.getPoint(t).distance(point) < cur_min.getX()) {
157+
cur_min = new Vector2D(spline.getPoint(t).distance(point), i + t / splines.size());
158+
}
159+
i += 1. / splines.size();
160+
}
161+
return cur_min.getY();
162+
163+
}
164+
165+
// /**
166+
// * Returns the Gaussian quadrature length of the parametric from a start to end t parameter
167+
// * @param start t parameter to start length calculation
168+
// * @param end t parameter to end length calculation
169+
// * @param steps number of steps (degree) of quadrature
170+
// * @return the Gaussian quadrature length of the parametric from a start to end t parameter
171+
// */
172+
// public double getGaussianQuadratureLength(double start, double end, int steps) {
173+
//
174+
// }
175+
176+
@Override
177+
public double getTFromLength(double length) {
178+
double t = 0;
179+
double l = 0;
180+
for(QuinticHermiteSpline spline : splines) {
181+
if(spline.getLength() + l <= length) {
182+
l += spline.getLength();
183+
t += 1. / splines.size();
184+
} else {
185+
t += spline.getTFromLength(length - l) / splines.size();
186+
break;
187+
}
188+
}
189+
return t;
190+
}
191+
192+
public int getIndexOfSplineFromPoint(Point2D point, int newtonsSteps) {
193+
return getSplineFromT(findClosestPointOnSpline(point, newtonsSteps, 5));
194+
}
195+
196+
public double getIndexOfSplineFromPointI(Point2D point, int newtonsSteps) {
197+
return findClosestPointOnSpline(point, newtonsSteps, 5);
198+
}
199+
}

‎src/main/java/com/github/mittyrobotics/visualiser/PurePursuitPathVisualizer.java‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ public class PurePursuitPathVisualizer {
3636
private final ArrayList<Double> curvatures = new ArrayList<>();
3737
private final ArrayList<Double> linearVelocities = new ArrayList<>();
3838
private final ArrayList<Parametric> parametrics = new ArrayList<>();
39+
private final ArrayList<Point2D> closests = new ArrayList<>();
3940

4041
private int cur_pos_index = 0;
4142
private JFrame frame, adjustFrame;
@@ -165,7 +166,9 @@ public void draw(Graphics2D g) {
165166

166167
g.setStroke(new BasicStroke(3));
167168

168-
g.fillOval(convertXToPixels(pos.getX())-5, convertYToPixels(pos.getY())-5, 10, 10);
169+
// g.fillOval(convertXToPixels(pos.getX())-5, convertYToPixels(pos.getY())-5, 10, 10);
170+
g.fillOval(convertXToPixels(closests.get(cur_pos_index).x)-5, convertYToPixels(closests.get(cur_pos_index).y)-5, 10, 10);
171+
169172

170173
for(int i = 0; i < cur_pos_index; i++) {
171174
Pose2D pose1 = robotPoses.get(i);
@@ -692,6 +695,7 @@ public void simulate(Pose2D startPosition, double END_TIME) {
692695
angularVelocities.add(path.getAngularVelocityAtPoint(0, path.getStartVelocity()));
693696
linearVelocities.add(path.getStartVelocity());
694697
parametrics.add(path.getParametric());
698+
closests.add(path.getParametric().getPoint(0));
695699

696700
//http://rossum.sourceforge.net/papers/DiffSteer/
697701

@@ -729,6 +733,7 @@ public void simulate(Pose2D startPosition, double END_TIME) {
729733
angularVelocities.add(dds.getAngularVelocity());
730734
linearVelocities.add(dds.getLinearVelocity());
731735
parametrics.add(path.getParametric());
736+
closests.add(path.getParametric().getPoint(path.getParametric().findClosestPointOnSpline(robotPosition.getPosition(), NEWTONS_STEPS, 5)));
732737

733738
if(path.isFinished(robotPosition, END_THRESHOLD)) {
734739
break;

0 commit comments

Comments
 (0)