Skip to content

Commit

Permalink
Backup commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@markmaker
markmaker committed Jun 11, 2020
1 parent 46a3be1 commit 019436b
Show file tree
Hide file tree
Showing 9 changed files with 533 additions and 187 deletions.
7 changes: 4 additions & 3 deletions src/main/java/org/openpnp/machine/reference/driver/AbstractMotionPlanner.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import org.openpnp.machine.reference.ReferenceHeadMountable;
import org.openpnp.machine.reference.ReferenceMachine;
import org.openpnp.machine.reference.axis.ReferenceControllerAxis;
import org.openpnp.machine.reference.axis.ReferenceVirtualAxis;
import org.openpnp.model.AxesLocation;
import org.openpnp.model.Configuration;
import org.openpnp.model.Length;
Expand Down Expand Up @@ -84,7 +85,6 @@ public MoveToOption[] getOptions() {

protected TreeMap<Double, Motion> motionPlan = new TreeMap<Double, Motion>();


@Override
public synchronized void home() throws Exception {
// Home all the drivers with their respective mapped axes (can be an empty map).
Expand Down Expand Up @@ -192,8 +192,8 @@ public void executeMotionPlan(CompletionType completionType) throws Exception {
ReferenceMachine machine = (ReferenceMachine) Configuration.get().getMachine();
List<Head> movedHeads = new ArrayList<>();
for (Entry<Double, Motion> plannedMotionEntry : motionPlan.tailMap(planExecutedTime, false).entrySet()) {
// Remember this motion was executed up-front, so an exception in the execution will not matter i.e. we
// are not stuck with this motion again and again.
// Advance the planExecutedTime up-front, so an exception in the execution will not mean we
// are stuck with this motion again and again.
planExecutedTime = plannedMotionEntry.getKey();
Motion plannedMotion = plannedMotionEntry.getValue();
if (!plannedMotion.hasOption(MotionOption.Stillstand)) {
Expand Down Expand Up @@ -358,6 +358,7 @@ protected void waitForDriverCompletion(HeadMountable hm, CompletionType completi
}
}



@Override
public ReferenceMachine getMachine() {
Expand Down
114 changes: 70 additions & 44 deletions src/main/java/org/openpnp/machine/reference/driver/AdvancedMotionSolver.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -246,7 +246,7 @@ public void elaborateMotionPath(List<Motion> motionPath, MotionElaboration elabo
if (coordinatedMotionLimits != null) {
AxesLocation vector = coordinatedMotionLimits.getVector(Motion.Derivative.Velocity);
for (ControllerAxis axis : axes) {
double weight = vector.getCoordinate(axis)/vector.getCoordinate(Motion.Euclidean);
double weight = vector.getCoordinate(axis)/vector.getCoordinate(Motion.EuclideanAxis);
if (weight > coordinatedLeadAxisWeight) {
coordinatedLeadAxis = axis;
coordinatedLeadAxisWeight = weight;
Expand Down Expand Up @@ -545,14 +545,15 @@ else if (axis instanceof ReferenceControllerAxis) {

final double leadFactor;
if (coordinatedLeadAxis != null) {
leadFactor = coordinatedVector.getCoordinate(axis)/coordinatedVector.getCoordinate(Motion.Euclidean)
leadFactor = coordinatedVector.getCoordinate(axis)/coordinatedVector.getCoordinate(Motion.EuclideanAxis)
/coordinatedLeadAxisWeight;
}
else {
leadFactor = 0.0;
}
final double softError = 1/(1+Math.sqrt(getFunctionEvalCount()));

final int segmentPhase = motion.getSegmentPhase();
final double signumPhase = Math.signum(leadFactor);

// Formulate the per-motion-per-axis constraints.
addConstraints(new Constraints() {
Expand All @@ -572,47 +573,72 @@ else if (axis instanceof ReferenceControllerAxis) {
// Powers of time t.
double t = x[t_];
double t2 = t*t;
double t3 = t2*t;
double t4 = t3*t;
double t5 = t4*t;

// These are the motion equations, half period, supposed to meet in the middle.
double a0eq =
1./2*j*t + a0;
double v0eq =
1./8*j*t2 + 1./2*a0*t + v0;
double s0eq =
1./48*j*t3 + 1./8*a0*t2 + 1./2*v0*t + s0;
double a1eq =
-1./2*j*t + a1;
double v1eq =
1./8*j*t2 - 1./2*a1*t + v1;
double s1eq =
-1./48*j*t3 + 1./8*a1*t2 - 1./2*v1*t + s1;

// Errors meeting in the middle.
double error = Math.pow(s0eq - s1eq, 2) + Math.pow((v0eq - v1eq)*t, 2) + Math.pow((a0eq - a1eq)*t2, 2);

// No optimization yet. Trust the JIT compiler and/or do optimization once the math works.

//g[_t] +=
// 2*(j*t + a0 - a1)*j*t4 + 4*Math.pow(j*t + a0 - a1, 2)*t3 + 1./2*(a0*t + a1*t + 2*V0 - 2*V1)*(a0 + a1)*t2 + 1./2*Math.pow(a0*t + a1*t + 2*V0 - 2*V1, 2)*t + 1./96*(j*t3 + 3*a0*t2 - 3*a1*t2 + 12*V0*t + 12*V1*t + 24*s0 - 24*s1)*(j*t2 + 2*a0*t - 2*a1*t + 4*V0 + 4*V1);
g[s0_] +=
1./12*j*t3 + 1./4*a0*t2 - 1./4*a1*t2 + v0*t + v1*t + 2*s0 - 2*s1;
g[v0_] +=
(a0*t + a1*t + 2*v0 - 2*v1)*t2 + 1./24*(j*t3 + 3*a0*t2 - 3*a1*t2 + 12*v0*t + 12*v1*t + 24*s0 - 24*s1)*t;
g[a0_] +=
2*(j*t + a0 - a1)*t4 + 1./2*(a0*t + a1*t + 2*v0 - 2*v1)*t3 + 1./96*(j*t3 + 3*a0*t2 - 3*a1*t2 + 12*v0*t + 12*v1*t + 24*s0 - 24*s1)*t2;
g[s1_] +=
-1./12*j*t3 - 1./4*a0*t2 + 1/4*a1*t2 - v0*t - v1*t - 2*s0 + 2*s1;
g[v1_] +=
-(a0*t + a1*t + 2*v0 - 2*v1)*t2 + 1./24*(j*t3 + 3*a0*t2 - 3*a1*t2 + 12*v0*t + 12*v1*t + 24*s0 - 24*s1)*t;
g[a1_] +=
-2*(j*t + a0 - a1)*t4 + 1/2*(a0*t + a1*t + 2*v0 - 2*v1)*t3 - 1./96*(j*t3 + 3*a0*t2 - 3*a1*t2 + 12*v0*t + 12*v1*t + 24*s0 - 24*s1)*t2;
g[j_] +=
2*(j*t + a0 - a1)*t5 + 1./288*(j*t3 + 3*a0*t2 - 3*a1*t2 + 12*v0*t + 12*v1*t + 24*s0 - 24*s1)*t3;

double teq = t;
double seq = s1;
double veq = v1;
double aeq = a1;
double jeq = j;
int hardness = 0;

switch (segmentPhase) {
case 1:
// Jerk phase to accelerate.
jeq = signumPhase*jMax;
teq = (aMax - signumPhase*a0)/jMax;
hardness = 5;
break;
case 2:
// Constant acceleration phase.
jeq = 0;
aeq = signumPhase*aMax;
// Jerk time to 0 acceleration.
double t3 = signumPhase*aMax/jMax;
double v3 = signumPhase*(vMax - 1./2*jMax*Math.pow(t3,2));
teq = (v3 - v0)/aMax;
hardness = 4;
break;
case 3:
// Negative jerk phase to constant velocity.
jeq = -signumPhase*jMax;
teq = signumPhase*a0/jMax;
hardness = 5;
break;
case 4:
// Constant velocity phase.
jeq = 0;
aeq = 0;
hardness = 3;
break;
case 5:
// Negative jerk phase to decelerate.
jeq = -signumPhase*jMax;
teq = -signumPhase*a1/jMax;
hardness = 5;
break;
case 6:
// Constant deceleration phase.
jeq = 0;
aeq = -signumPhase*aMax;
// Jerk time to 0 acceleration.
double t5 = signumPhase*aMax/jMax;
double v5 = signumPhase*(vMax - 1./2*jMax*Math.pow(t5,2));
teq = (v5 - v1)/aMax;
hardness = 4;
break;
case 7:
// Jerk phase to target acceleration.
jeq = signumPhase*jMax;
teq = -signumPhase*a0/jMax;
hardness = 5;
break;
}

if (t < 0) {
hardness = 7;

}
double error = 0.0;
// Coordinated moves.
if (coordinatedLeadAxis != null
&& coordinatedLeadAxis != axis) {
Expand All @@ -634,7 +660,7 @@ else if (axis instanceof ReferenceControllerAxis) {

// Soft time error
//error += softError*t2;
// g[_t] += 2*softError*t;
// g[_t] += 2*softError*t;
return error;
}
});
Expand Down Expand Up @@ -665,7 +691,7 @@ public double solve(int maxfneval, double tolerance) throws Exception {
upper[var.i()] = var.getUpperLimit();
}
// Solve it.
lastError = solve(x, lower, upper, TNC_MSG_NONE, maxfneval, tolerance*tolerance*tolerance, 0, tolerance*tolerance*tolerance);
lastError = solve(x, lower, upper, TNC_MSG_NONE, maxfneval, tolerance*tolerance, 0, tolerance*tolerance);
// Write back the variables to the motion.
for (Var var : variables) {
var.set(x[var.i()]);
Expand Down
39 changes: 5 additions & 34 deletions src/main/java/org/openpnp/machine/reference/driver/ReferenceAdvancedMotionPlanner.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,6 @@ public void waitForCompletion(HeadMountable hm, CompletionType completionType) t
super.waitForCompletion(hm, completionType);
}


public void planMotion(CompletionType completionType) throws Exception {
// Plan the tail of the queued motions.
ArrayList<Motion> motionPath = new ArrayList<>();
Expand All @@ -71,49 +70,21 @@ public void planMotion(CompletionType completionType) throws Exception {
if (prevMotion != null
&& !prevMotion.getLocation().motionSegmentTo(queuedMotion.getLocation()).isEmpty()) {
// The queued motion are the fixed way-points. We need to insert segments for 3rd order motion control.
// The simplest general form is a four-segment "mirror-S" curve. A co-linear motion sequence could be even simpler,
// but we ignore this case here.
AxesLocation zeroVectorAxes = new AxesLocation(queuedMotion.getLocation().getControllerAxes(),
(a) -> new Length(0, AxesLocation.getUnits()));

// Kill current "solution", we only interpolate the location.
AxesLocation delta = prevMotion.getLocation().motionSegmentTo(queuedMotion.getLocation());
// TODO: better heuristics
final int segments = 4;
for (int interpolate = 1; interpolate < segments; interpolate++) {
Motion interpolateMotion = prevMotion.interpolate(queuedMotion, interpolate*1.0/segments);
interpolateMotion.setTime(queuedMotion.getTime()/segments);
interpolateMotion.clearOption(Motion.MotionOption.FixedWaypoint);
if (isInSaveZZone(prevMotion) && isInSaveZZone(queuedMotion)) {
interpolateMotion.clearOption(Motion.MotionOption.CoordinatedWaypoint);
interpolateMotion.setOption(Motion.MotionOption.LimitToSafeZZone);
}
interpolateMotion.setVector(Motion.Derivative.Velocity, delta.multiply(1./(queuedMotion.getTime()+0.01)));
interpolateMotion.setVector(Motion.Derivative.Acceleration, zeroVectorAxes);
interpolateMotion.setVector(Motion.Derivative.Jerk, zeroVectorAxes);
motionPath.add(interpolateMotion);
if (isInSaveZZone(queuedMotion)) {

}
queuedMotion.setTime(queuedMotion.getTime()/segments);
queuedMotion.setVector(Motion.Derivative.Velocity, delta.multiply(1./(queuedMotion.getTime()+0.01)));
queuedMotion.setVector(Motion.Derivative.Acceleration, zeroVectorAxes);
queuedMotion.setVector(Motion.Derivative.Jerk, zeroVectorAxes);

}
// Add to list.
motionPath.add(queuedMotion);
// Next, please.
prevMotion = queuedMotion;
}

if (completionType == CompletionType.WaitForStillstand) {
prevMotion.setOption(Motion.MotionOption.Stillstand);
}
AdvancedMotionSolver solver = new AdvancedMotionSolver();
solver.outputMotionPath("before", motionPath);
solver.elaborateMotionPath(motionPath, solver.new ConstraintsModeller());
solver.solve(1000000, precision.convertToUnits(AxesLocation.getUnits()).getValue()*0.00000001);
Logger.debug("solver state: {}, iterations: {}, error: {}", solver.getSolverState(), solver.getFunctionEvalCount(), solver.getLastError());
solver.outputMotionPath("after", motionPath);



// Remove the old plan
while (motionPlan.size() > 0) {
double last = motionPlan.lastKey();
Expand Down
32 changes: 25 additions & 7 deletions src/main/java/org/openpnp/model/Motion.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ protected int flag(){
private double time;
private AxesLocation [] vector;
private int options;
private int segmentPhase;
private MotionCommand motionCommand;

public Motion(double time, MotionCommand motionCommand, AxesLocation[] vector, MotionOption... options) {
Expand Down Expand Up @@ -100,14 +101,22 @@ int getOrder() {
}


public int getSegmentPhase() {
return segmentPhase;
}
public void setSegmentPhase(int segmentPhase) {
this.segmentPhase = segmentPhase;
}


public enum Derivative {
Location,
Velocity,
Acceleration,
Jerk
}

static public final ReferenceVirtualAxis Euclidean = new ReferenceVirtualAxis() {
static public final ReferenceVirtualAxis EuclideanAxis = new ReferenceVirtualAxis() {
{ this.setName("Euclidean"); }
};

Expand Down Expand Up @@ -319,8 +328,8 @@ public static Motion computeWithLimits(MotionCommand motionCommand, AxesLocation
// Also include the given speed factor.
double speedEffective = (time > 0 ? euclideanTime/time : 1.0) * Math.max(0.01, speed);
// Add a virtual Distance axis to store the limits in the motion, as these are applicable per overall distance.
AxesLocation motionLocation = location1.put(new AxesLocation(Motion.Euclidean, euclideanLimits[0]));
AxesLocation motionDistance = distance.put(new AxesLocation(Motion.Euclidean, euclideanLimits[0]));
AxesLocation motionLocation = location1.put(new AxesLocation(Motion.EuclideanAxis, euclideanLimits[0]));
AxesLocation motionDistance = distance.put(new AxesLocation(Motion.EuclideanAxis, euclideanLimits[0]));
AxesLocation [] motionDerivatives = new AxesLocation [ControllerAxis.motionLimitsOrder+1];
// The 0th derivative is the location itself.
motionDerivatives[0] = motionLocation;
Expand All @@ -345,16 +354,25 @@ public static Motion computeWithLimits(MotionCommand motionCommand, AxesLocation
public AxesLocation getLocation() {
return getVector(Derivative.Location);
}
public AxesLocation getVelocity() {
return getVector(Derivative.Velocity);
}
public AxesLocation getAcceleration() {
return getVector(Derivative.Acceleration);
}
public AxesLocation getJerk() {
return getVector(Derivative.Jerk);
}
public double getFeedRatePerSecond(LengthUnit units) {
return getVector(Derivative.Velocity).getCoordinate(Euclidean, units);
return getVector(Derivative.Velocity).getCoordinate(EuclideanAxis, units);
}
public double getFeedRatePerMinute(LengthUnit units) {
return getVector(Derivative.Velocity).getCoordinate(Euclidean, units)*60.0;
return getVector(Derivative.Velocity).getCoordinate(EuclideanAxis, units)*60.0;
}
public double getAccelerationPerSecond2(LengthUnit units) {
return getVector(Derivative.Acceleration).getCoordinate(Euclidean, units);
return getVector(Derivative.Acceleration).getCoordinate(EuclideanAxis, units);
}
public double getJerkPerSecond3(LengthUnit units) {
return getVector(Derivative.Jerk).getCoordinate(Euclidean, units);
return getVector(Derivative.Jerk).getCoordinate(EuclideanAxis, units);
}
}

0 comments on commit 019436b

Please sign in to comment.