Revision 36:2b456e10f783

View differences:

src/main/java/tt/jointeuclid2ni/solver/impl/AbstractAlgorithm.java
175 175
        // Overlay
176 176
        VisManager.registerLayer(VisInfoLayer.create());
177 177

  
178
        time = new TimeParameter((params.gridStep / params.maxSpeed) / 4);
178
        time = new TimeParameter((int) Math.round(((double)params.gridStep / (double)params.maxSpeed) / (double) 5));
179 179

  
180 180
        VisManager.registerLayer(ParameterControlLayer.create(time));
181 181
    }
src/main/java/tt/jointeuclid2ni/solver/impl/AbstractSFBasedAlgorithm.java
10 10
import tt.euclidtime3i.L1Heuristic;
11 11
import tt.euclidtime3i.L2Heuristic;
12 12
import tt.euclidtime3i.PerfectBasedHeuristic;
13
import tt.euclidtime3i.discretization.softconstraints.BumpSeparationPenaltyFunction;
13 14
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
14 15
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
15 16
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
......
22 23

  
23 24
public abstract class AbstractSFBasedAlgorithm extends AbstractAlgorithm {
24 25

  
25
    private static final double IIHP_MAX_PENALTY = 10;
26
    private static final double MAX_PENALTY = 10;
26 27

  
27 28
    public AbstractSFBasedAlgorithm() {
28 29
        this(new String[0]);
......
82 83
        for (int i = 0; i < problem.nAgents(); i++) {
83 84
            for (int j = 0; j < problem.nAgents(); j++) {
84 85
                if (i != j) {
85
                    constraints[i][j] = new SeparationConstraint(new LinearSeparationPenaltyFunction(IIHP_MAX_PENALTY),
86
                    constraints[i][j] = new SeparationConstraint(new BumpSeparationPenaltyFunction(MAX_PENALTY),
86 87
                            10, problem.getBodyRadius(i)+problem.getBodyRadius(j));
87 88
                }
88 89

  
src/main/java/tt/jointtraj/separableflow/SeparableFlowOptimizer.java
28 28
		if (k > 2) {
29 29
			// Find initial unconstrained trajectories
30 30
			trajectories = initUnconstrainedTrajectories(trajectoryOptimizers, maxCost, runtimeDeadlineMs);
31

  
31 32
			// Iteratively replan
32 33
			int iterations = nAgents * (k - 2);
33 34
		    trajectories = iterativelyReplan(trajectories, trajectoryOptimizers, constraints, iterations,
34 35
					maxCost, runtimeDeadlineMs);
36

  
37
		    // Replan with hard-constraints
38
		    trajectories = replanWithInfiniteWeight(trajectories, trajectoryOptimizers, constraints, maxCost, runtimeDeadlineMs);
35 39
		}
36 40

  
37 41
		return trajectories;
......
98 102
	    return trajectories;
99 103
	}
100 104

  
105
	private static EvaluatedTrajectory[] replanWithInfiniteWeight(
106
			EvaluatedTrajectory[] trajectories,
107
			TrajectoryOptimizer[] trajectoryOptimizers,
108
			final PairwiseConstraint[][] constraints, double maxCost, long runtimeDeadlineMs
109
			) {
110
		int nAgents = trajectoryOptimizers.length;
111

  
112
	    for (int i = 0; i < nAgents; i++) {
113
	        Trajectory[] otherTrajectories = otherTrajectories(trajectories, i);
114
	        PairwiseConstraint[] constraintsToOtherTrajectories = constraintsOf(constraints, i);
115

  
116
	        StraightSegmentPenaltyFunction penaltyFunctions
117
	        	= new PairwiseConstraintStraightSegmentPenalty(constraintsToOtherTrajectories, otherTrajectories, Double.POSITIVE_INFINITY);
118

  
119
	        EvaluatedTrajectory bestResponseTraj = trajectoryOptimizers[i].getOptimalTrajectory(penaltyFunctions, maxCost, runtimeDeadlineMs);
120

  
121
	        if (bestResponseTraj != null) {
122
	        	trajectories[i] = bestResponseTraj;
123
	        } else {
124
	        	return null;
125
	        }
126
	    }
127

  
128
	    return trajectories;
129
	}
130

  
101 131
    private static EvaluatedTrajectory[] initUnconstrainedTrajectories(
102 132
			TrajectoryOptimizer[] trajectoryOptimizers, double maxCost, long runtimeDeadlineMs) {
103 133

  
src/main/java/tt/jointtrajineuclidtime3i/solver/PrioritizedPlanningSolver.java
89 89
            if (path != null) {
90 90
                trajs[i] = new StraightSegmentTrajectory(path, maxTime);
91 91
            } else {
92
                return new SearchResult(null, true);
92
                return new SearchResult(trajs, true);
93
            	//return new SearchResult(null, true);
93 94
            }
94 95
        }
95 96

  
src/main/resources/eclipse/Solver.launch
8 8
</listAttribute>
9 9
<stringAttribute key="org.eclipse.jdt.launching.CLASSPATH_PROVIDER" value="org.eclipse.m2e.launchconfig.classpathProvider"/>
10 10
<stringAttribute key="org.eclipse.jdt.launching.MAIN_TYPE" value="tt.jointeuclid2ni.Solver"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-method PP -problemfile src/test/resources/problems/stopatgoaltest2.xml -timeout 500000 -maxtime 3500 -gridstep 25 -showvis -summary -grid 4 -k 1"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-method KSFO -problemfile /home/capino/projects/deconfliction/repo/experiments/ksfo/instances/empty-unbounded-g4/1088.xml -timeout 2000000 -maxtime 3500 -gridstep 50 -showvis -summary -grid 4 -k 1"/>
12 12
<stringAttribute key="org.eclipse.jdt.launching.PROJECT_ATTR" value="deconflictiontools"/>
13 13
<stringAttribute key="org.eclipse.jdt.launching.SOURCE_PATH_PROVIDER" value="org.eclipse.m2e.launchconfig.sourcepathProvider"/>
14 14
</launchConfiguration>

Also available in: Unified diff