Last active
February 17, 2023 21:19
-
-
Save Sam-Belliveau/f7179ca2a62072c7d1987f882f513112 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import java.util.ArrayDeque; | |
import java.util.Queue; | |
public class ArmBFSField { | |
public interface Constraint { | |
public boolean isInvalid(double armDeg, double wristDeg); | |
public default Constraint add(Constraint next) { | |
return (a, w) -> this.isInvalid(a, w) || next.isInvalid(a, w); | |
} | |
} | |
public static final int kDegreeRange = 360; | |
// This value does not matter as much as long as its small. | |
public static final double kConstraintDistCost = (1 / 16.0); | |
public static final double kEpsilon = kConstraintDistCost / 2.0; | |
public static final int kArmNodeSpeed = 1; | |
public static final int kWristNodeSpeed = 2; | |
// Amount to downscale the field by | |
// kBinning = 1) 360 x 360 | |
// kBinning = 2) 180 x 180 | |
// kBinning = 3) 120 x 120 | |
public static final int kBinning = 3; | |
private static int normalize(int degrees) { | |
return degrees - kDegreeRange * Math.floorDiv(degrees, kDegreeRange); | |
} | |
private static double normalizeZero(double degrees) { | |
return degrees - kDegreeRange * Math.round(degrees / kDegreeRange); | |
} | |
public class Node { | |
private final boolean mValid; | |
private final int mArmDeg; | |
private final int mWristDeg; | |
private double mConstraintCost; | |
private double mSetpointCost; | |
private Node mNextNode; | |
public Node(int armDeg, int wristDeg) { | |
mArmDeg = normalize(armDeg); | |
mWristDeg = normalize(wristDeg); | |
mValid = !mConstraints.isInvalid(getArmDeg(), getWristDeg()); | |
mConstraintCost = mValid ? Double.MAX_VALUE : 0.0; | |
mSetpointCost = Double.MAX_VALUE; | |
mNextNode = this; | |
} | |
private double getDistanceCost(Node previous) { | |
final double dx = normalizeZero(this.mArmDeg - previous.mArmDeg); | |
final double dy = normalizeZero(this.mWristDeg - previous.mWristDeg); | |
return Math.hypot(dx, dy); | |
} | |
private Node makeSetpoint() { | |
if (!isValid()) | |
throw new RuntimeException("Setpoint is within constraint!"); | |
mNextNode = this; | |
mSetpointCost = 0; | |
return this; | |
} | |
private boolean expandConstraint(Node previous) { | |
if (this == previous) { | |
return false; | |
} | |
final double cost = this.getDistanceCost(previous) * kConstraintDistCost; | |
final double newConstraintCost = previous.mConstraintCost + cost; | |
if (newConstraintCost + kEpsilon < mConstraintCost) { | |
mConstraintCost = newConstraintCost; | |
return true; | |
} | |
return false; | |
} | |
private boolean expandSearch(Node previous) { | |
// do not expand searches into invalid areas | |
if (this == previous || this == previous.mNextNode || !this.isValid() || !previous.isValid()) { | |
return false; | |
} | |
// the difference in constraint cost between adjacent nodes is small | |
// so this will basically find the shortest path, but if there is an | |
// equidistant path that is further from the constraints, take that one instead, | |
final double cost = this.getDistanceCost(previous); | |
final double newSetpointCost = previous.mSetpointCost + cost; | |
final double newCost = newSetpointCost - previous.mConstraintCost; | |
final double oldCost = mSetpointCost - mNextNode.mConstraintCost; | |
if (newCost + kEpsilon < oldCost) { | |
mSetpointCost = newSetpointCost; | |
mNextNode = previous; | |
return true; | |
} | |
return false; | |
} | |
private boolean expandEscape(Node previous) { | |
// a valid node should not escape as it is valid | |
if (this == previous || this == previous.mNextNode || this.isValid()) { | |
return false; | |
} | |
// when escaping you only want to move one mech at a time | |
if (mArmDeg != previous.mArmDeg && mWristDeg != previous.mWristDeg) { | |
return false; | |
} | |
// if there is a new shorter escape path, take it | |
final double cost = this.getDistanceCost(previous); | |
final double newSetpointCost = previous.isValid() ? 0.0 : previous.mSetpointCost + cost; | |
if (newSetpointCost + kEpsilon < mSetpointCost) { | |
mSetpointCost = newSetpointCost; | |
mNextNode = previous; | |
return true; | |
} | |
return false; | |
} | |
public boolean isSetpoint() { | |
return this == next(); | |
} | |
public boolean isValid() { | |
return mValid; | |
} | |
public double getArmDeg() { | |
return normalizeZero(mArmDeg + mArmDegOffset); | |
} | |
public double getWristDeg() { | |
return normalizeZero(mWristDeg + mWristDegOffset); | |
} | |
private Node getNeighbor(int dx, int dy) { | |
return getRawNode(mArmDeg + kBinning * dx, mWristDeg + kBinning * dy); | |
} | |
public Node next() { | |
return mNextNode; | |
} | |
public Node next(int depth) { | |
Node result = this; | |
while (depth-- > 0) { | |
result = result.next(); | |
} | |
return result; | |
} | |
public Node travel(double dist) { | |
Node result = this; | |
while (dist > 0 && !result.isSetpoint()) { | |
dist -= result.getDistanceCost(result.next()); | |
result = result.next(); | |
} | |
return result; | |
} | |
} | |
private final Constraint mConstraints; | |
private final double mArmDegOffset; | |
private final double mWristDegOffset; | |
private final Node[] mNodeMap; | |
public ArmBFSField(double targetArmDeg, double targetWristDeg, Constraint constraints) { | |
mConstraints = constraints; | |
mArmDegOffset = targetArmDeg - kBinning * Math.round(targetArmDeg / kBinning); | |
mWristDegOffset = targetWristDeg - kBinning * Math.round(targetWristDeg / kBinning); | |
mNodeMap = new Node[getIndex(kDegreeRange - 1, kDegreeRange - 1) + 1]; | |
Queue<Node> openSet = new ArrayDeque<>(kDegreeRange); | |
// initialize all nodes and test to see if they fit the constraints | |
for (int arm = 0; arm < kDegreeRange; arm += kBinning) { | |
for (int wrist = 0; wrist < kDegreeRange; wrist += kBinning) { | |
final Node node = new Node(arm, wrist); | |
// if the node is a member of the constraints, | |
// add it to the constraint openset for later | |
if (!node.isValid()) { | |
openSet.add(node); | |
} | |
mNodeMap[getIndex(arm, wrist)] = node; | |
} | |
} | |
// we want to be able to find the shortest path that also remains the | |
// furthest from all the constraints. doing a search will let us determine | |
// the distance each node is from the closest constraint. | |
Node next; | |
while ((next = openSet.poll()) != null) { | |
for (int dx = -1; dx <= 1; ++dx) { | |
for (int dy = -1; dy <= 1; ++dy) { | |
final Node node = next.getNeighbor(dx, dy); | |
if (node.expandConstraint(next)) { | |
openSet.add(node); | |
} | |
} | |
} | |
} | |
// do a standard BFS on all of the valid nodes. | |
openSet.add(getNode(targetArmDeg, targetWristDeg).makeSetpoint()); | |
while ((next = openSet.poll()) != null) { | |
for (int dx = -kArmNodeSpeed; dx <= kArmNodeSpeed; ++dx) { | |
for (int dy = -kWristNodeSpeed; dy <= kWristNodeSpeed; ++dy) { | |
final Node node = next.getNeighbor(dx, dy); | |
if (node.expandSearch(next)) { | |
openSet.add(node); | |
} | |
// if a node is invalid here, it means that it is touching | |
// a valid node, so it will be the beginning of our escape path. | |
else if (node.expandEscape(next)) { | |
openSet.add(node); | |
} | |
} | |
} | |
} | |
} | |
private int getIndex(int armDeg, int wristDeg) { | |
armDeg = normalize(armDeg) / kBinning; | |
wristDeg = normalize(wristDeg) / kBinning; | |
return armDeg * (kDegreeRange / kBinning) + wristDeg; | |
} | |
private Node getRawNode(int armDeg, int wristDeg) { | |
return mNodeMap[getIndex(armDeg, wristDeg)]; | |
} | |
public Node getNode(double armDeg, double wristDeg) { | |
return getRawNode( | |
(int) Math.round(armDeg - mArmDegOffset + kBinning / 2.0), | |
(int) Math.round(wristDeg - mWristDegOffset + kBinning / 2.0)); | |
} | |
public static void main(String... args) { | |
Constraint constraints = (a, w) -> a >= 0.0; | |
constraints = constraints.add((a, w) -> (Math.abs(a + 90) < 30) && (w > 150 || (30 > w))); | |
ArmBFSField field = new ArmBFSField(210.23, 289.43, constraints); | |
Node node = field.getNode(330, 250).next(); | |
System.out.println("arm,wrist"); | |
while (!node.isSetpoint()) { | |
System.out.println(node.getArmDeg() + "," + node.getWristDeg()); | |
node = node.travel(20); | |
} | |
/** | |
* arm,wrist | |
* > -31.769999999999982,-112.57 | |
* > -31.769999999999982,-132.57 | |
* > -33.76999999999998,-154.57 | |
* > -43.76999999999998,-174.57 | |
* > -53.76999999999998,165.43 | |
* > -65.76999999999998,149.43 | |
* > -85.76999999999998,149.43 | |
* > -105.77000000000001,149.43 | |
* > -123.77000000000001,157.43 | |
* > -133.77,177.43 | |
* > -143.77,-162.57 | |
* > -149.77,-142.57 | |
* > -149.77,-122.57 | |
* > -149.77,-102.57 | |
* > -149.77,-82.57 | |
*/ | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment