Revision 60:b288ade468ec

View differences:

misc/eclipse/Solver (kDPM-C).launch
1
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
2
<launchConfiguration type="org.eclipse.jdt.launching.localJavaApplication">
3
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_PATHS">
4
<listEntry value="/deconflictiontools/src/main/java/tt/jointeuclid2ni/Solver.java"/>
5
</listAttribute>
6
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_TYPES">
7
<listEntry value="1"/>
8
</listAttribute>
9
<stringAttribute key="org.eclipse.jdt.launching.CLASSPATH_PROVIDER" value="org.eclipse.m2e.launchconfig.classpathProvider"/>
10
<stringAttribute key="org.eclipse.jdt.launching.MAIN_TYPE" value="tt.jointeuclid2ni.Solver"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-method KDPMC -k 50 -problemfile  src/main/resources/problems/99.xml  -maxtime 2500 -gridstep 50 -summary -showvis -verbose"/>
12
<stringAttribute key="org.eclipse.jdt.launching.PROJECT_ATTR" value="deconflictiontools"/>
13
<stringAttribute key="org.eclipse.jdt.launching.SOURCE_PATH_PROVIDER" value="org.eclipse.m2e.launchconfig.sourcepathProvider"/>
14
</launchConfiguration>
src/main/java/tt/jointeuclid2ni/Solver.java
173 173
                algorithm = new AlgorithmPP();
174 174
                break;
175 175

  
176
            case KSFO:
177
                algorithm = new AlgorithmKSFO(args);
176
            case KDPMD:
177
                algorithm = new AlgorithmDPMD(args);
178 178
                break;
179 179

  
180 180
            case KDPMC:
src/main/java/tt/jointeuclid2ni/solver/Algorithms.java
2 2

  
3 3
public enum Algorithms {
4 4
    PP,
5
    KSFO, // k-step separable flow optimizer
6
    KDPMC, // k-step distributed penalty method with continuous optimzers
5
    KDPMD, // k-step distributed penalty method with discrete optimizers
6
    KDPMC, // k-step distributed penalty method with continuous optimizers
7 7
    ASFO, // anytime separable flow optimizer
8 8
    IIHP, // incremental homotopy optimizer
9 9
    ODPIN,
src/main/java/tt/jointeuclid2ni/solver/impl/AbstractDPMasedAlgorithm.java
1
package tt.jointeuclid2ni.solver.impl;
2

  
3

  
4
import org.jgrapht.DirectedGraph;
5
import org.jgrapht.util.HeuristicToGoal;
6

  
7
import tt.euclid2i.EvaluatedTrajectory;
8
import tt.euclid2i.Line;
9
import tt.euclid2i.Point;
10
import tt.euclidtime3i.L1Heuristic;
11
import tt.euclidtime3i.L2Heuristic;
12
import tt.euclidtime3i.PerfectBasedHeuristic;
13
import tt.euclidtime3i.discretization.softconstraints.BumpSeparationPenaltyFunction;
14
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
15
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
16
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
17
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
18
import tt.jointtraj.separableflow.AStarTrajectoryOptimizer;
19
import tt.jointtraj.separableflow.TrajectoryOptimizer;
20
import tt.jointtraj.solver.SearchResult;
21
import tt.util.NotImplementedException;
22
import tt.util.Verbose;
23

  
24

  
25
public abstract class AbstractDPMasedAlgorithm extends AbstractAlgorithm {
26

  
27
    private static final double MAX_PENALTY = 1;
28

  
29
    public AbstractDPMasedAlgorithm() {
30
        this(new String[0]);
31
    }
32

  
33
    public AbstractDPMasedAlgorithm(String[] args) {
34
        super();
35
    }
36

  
37
    @Override
38
    public SearchResult solveProblem() {
39
        Verbose.setVerbose(params.verbose);
40

  
41
        // create spatial graph for each agent
42
        @SuppressWarnings("unchecked")
43
        DirectedGraph<Point, Line> graph[] = new DirectedGraph[problem.nAgents()];
44

  
45
        for (int i = 0; i < problem.nAgents(); i++) {
46
            graph[i] = createAndShowGridForIthAgent(i);
47
        }
48

  
49
        // Compute heuristic based on optimal policies on the spatial graph
50
        HeuristicToGoal<tt.euclidtime3i.Point>[] heuristics = new HeuristicToGoal[problem.nAgents()];
51

  
52
        for (int i = 0; i < problem.nAgents(); i++) {
53
        	switch (params.heuristic) {
54
			case PERFECT:
55
				heuristics[i] = new PerfectBasedHeuristic(graph[i], problem.getTarget(i));
56
				break;
57

  
58
			case L1:
59
				heuristics[i] = new L1Heuristic(problem.getTarget(i));
60
				break;
61

  
62
			case L2:
63
				heuristics[i] = new L2Heuristic(problem.getTarget(i));
64
				break;
65

  
66
			default:
67
				throw new NotImplementedException();
68
			}
69
        }
70

  
71
        // Create trajectory optimizers
72
        TrajectoryOptimizer[] trajectoryOptimizers = new TrajectoryOptimizer[problem.nAgents()];
73
        for (int i = 0; i < problem.nAgents(); i++) {
74
            trajectoryOptimizers[i] = new AStarTrajectoryOptimizer(graph[i],
75
                                        new tt.euclidtime3i.Point(problem.getStart(i),0),
76
                                        new tt.euclidtime3i.Point(problem.getTarget(i),  params.maxTime),
77
                                        problem.getMaxSpeeds()[i],
78
                                        params.waitMoveDuration,
79
                                        heuristics[i],
80
                                        10);
81
        }
82

  
83
        // Create constraints
84
        PenaltyFunction[][] softSeparationFunctions = new PenaltyFunction[problem.nAgents()][problem.nAgents()];
85
        for (int i = 0; i < problem.nAgents(); i++) {
86
            for (int j = 0; j < problem.nAgents(); j++) {
87
                if (i != j) {
88
                    softSeparationFunctions[i][j] = new BumpSeparationPenaltyFunction(MAX_PENALTY, problem.getBodyRadius(i) + problem.getBodyRadius(j));
89
                }
90
            }
91
        }
92

  
93
        EvaluatedTrajectory[] trajs = solveCore(trajectoryOptimizers, softSeparationFunctions);
94

  
95
        return new SearchResult(trajs, true);
96

  
97
    }
98

  
99
    abstract protected EvaluatedTrajectory[] solveCore(
100
            TrajectoryOptimizer[] trajectoryOptimizers,
101
            PenaltyFunction[][] penaltyFunctions);
102
}
src/main/java/tt/jointeuclid2ni/solver/impl/AbstractSFBasedAlgorithm.java
1
package tt.jointeuclid2ni.solver.impl;
2

  
3

  
4
import org.jgrapht.DirectedGraph;
5
import org.jgrapht.util.HeuristicToGoal;
6

  
7
import tt.euclid2i.EvaluatedTrajectory;
8
import tt.euclid2i.Line;
9
import tt.euclid2i.Point;
10
import tt.euclidtime3i.L1Heuristic;
11
import tt.euclidtime3i.L2Heuristic;
12
import tt.euclidtime3i.PerfectBasedHeuristic;
13
import tt.euclidtime3i.discretization.softconstraints.BumpSeparationPenaltyFunction;
14
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
15
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
16
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
17
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
18
import tt.jointtraj.separableflow.AStarTrajectoryOptimizer;
19
import tt.jointtraj.separableflow.TrajectoryOptimizer;
20
import tt.jointtraj.solver.SearchResult;
21
import tt.util.NotImplementedException;
22
import tt.util.Verbose;
23

  
24

  
25
public abstract class AbstractSFBasedAlgorithm extends AbstractAlgorithm {
26

  
27
    private static final double MAX_PENALTY = 1;
28

  
29
    public AbstractSFBasedAlgorithm() {
30
        this(new String[0]);
31
    }
32

  
33
    public AbstractSFBasedAlgorithm(String[] args) {
34
        super();
35
    }
36

  
37
    @Override
38
    public SearchResult solveProblem() {
39
        Verbose.setVerbose(params.verbose);
40

  
41
        // create spatial graph for each agent
42
        @SuppressWarnings("unchecked")
43
        DirectedGraph<Point, Line> graph[] = new DirectedGraph[problem.nAgents()];
44

  
45
        for (int i = 0; i < problem.nAgents(); i++) {
46
            graph[i] = createAndShowGridForIthAgent(i);
47
        }
48

  
49
        // Compute heuristic based on optimal policies on the spatial graph
50
        HeuristicToGoal<tt.euclidtime3i.Point>[] heuristics = new HeuristicToGoal[problem.nAgents()];
51

  
52
        for (int i = 0; i < problem.nAgents(); i++) {
53
        	switch (params.heuristic) {
54
			case PERFECT:
55
				heuristics[i] = new PerfectBasedHeuristic(graph[i], problem.getTarget(i));
56
				break;
57

  
58
			case L1:
59
				heuristics[i] = new L1Heuristic(problem.getTarget(i));
60
				break;
61

  
62
			case L2:
63
				heuristics[i] = new L2Heuristic(problem.getTarget(i));
64
				break;
65

  
66
			default:
67
				throw new NotImplementedException();
68
			}
69
        }
70

  
71
        // Create trajectory optimizers
72
        TrajectoryOptimizer[] trajectoryOptimizers = new TrajectoryOptimizer[problem.nAgents()];
73
        for (int i = 0; i < problem.nAgents(); i++) {
74
            trajectoryOptimizers[i] = new AStarTrajectoryOptimizer(graph[i],
75
                                        new tt.euclidtime3i.Point(problem.getStart(i),0),
76
                                        new tt.euclidtime3i.Point(problem.getTarget(i),  params.maxTime),
77
                                        problem.getMaxSpeeds()[i],
78
                                        params.waitMoveDuration,
79
                                        heuristics[i],
80
                                        10);
81
        }
82

  
83
        // Create constraints
84
        PenaltyFunction[][] softSeparationFunctions = new PenaltyFunction[problem.nAgents()][problem.nAgents()];
85
        for (int i = 0; i < problem.nAgents(); i++) {
86
            for (int j = 0; j < problem.nAgents(); j++) {
87
                if (i != j) {
88
                    softSeparationFunctions[i][j] = new BumpSeparationPenaltyFunction(MAX_PENALTY, problem.getBodyRadius(i) + problem.getBodyRadius(j));
89
                }
90
            }
91
        }
92

  
93
        EvaluatedTrajectory[] trajs = solveCore(trajectoryOptimizers, softSeparationFunctions);
94

  
95
        return new SearchResult(trajs, true);
96

  
97
    }
98

  
99
    abstract protected EvaluatedTrajectory[] solveCore(
100
            TrajectoryOptimizer[] trajectoryOptimizers,
101
            PenaltyFunction[][] penaltyFunctions);
102
}
src/main/java/tt/jointeuclid2ni/solver/impl/AlgorithmDPMD.java
1
package tt.jointeuclid2ni.solver.impl;
2

  
3

  
4
import tt.euclid2i.EvaluatedTrajectory;
5
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
6
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
7
import tt.jointtraj.separableflow.SeparableFlowOptimizer;
8
import tt.jointtraj.separableflow.TrajectoryOptimizer;
9
import tt.util.Args;
10

  
11

  
12
public class AlgorithmDPMD extends AbstractDPMasedAlgorithm {
13

  
14
    private int k = 50;
15

  
16
    public AlgorithmDPMD() {
17
        this(new String[0]);
18
    }
19

  
20
    public AlgorithmDPMD(String[] args) {
21
        k = Integer.parseInt(Args.getArgumentValue(args, "-k", true));
22
    }
23

  
24
    @Override
25
    protected EvaluatedTrajectory[] solveCore(
26
            TrajectoryOptimizer[] trajectoryOptimizers,
27
            PenaltyFunction[][] penalties) {
28

  
29
        EvaluatedTrajectory[] trajs = SeparableFlowOptimizer.solve(
30
                trajectoryOptimizers, penalties, k, 1000000,
31
                params.runtimeDeadlineMs, params.verbose);
32

  
33
        return trajs;
34
    }
35

  
36

  
37
}
src/main/java/tt/jointeuclid2ni/solver/impl/AlgorithmKSFO.java
1
package tt.jointeuclid2ni.solver.impl;
2

  
3

  
4
import tt.euclid2i.EvaluatedTrajectory;
5
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
6
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
7
import tt.jointtraj.separableflow.SeparableFlowOptimizer;
8
import tt.jointtraj.separableflow.TrajectoryOptimizer;
9
import tt.util.Args;
10

  
11

  
12
public class AlgorithmKSFO extends AbstractSFBasedAlgorithm {
13

  
14
    private int k = 50;
15

  
16
    public AlgorithmKSFO() {
17
        this(new String[0]);
18
    }
19

  
20
    public AlgorithmKSFO(String[] args) {
21
        k = Integer.parseInt(Args.getArgumentValue(args, "-k", true));
22
    }
23

  
24
    @Override
25
    protected EvaluatedTrajectory[] solveCore(
26
            TrajectoryOptimizer[] trajectoryOptimizers,
27
            PenaltyFunction[][] penalties) {
28

  
29
        EvaluatedTrajectory[] trajs = SeparableFlowOptimizer.solve(
30
                trajectoryOptimizers, penalties, k, 1000000,
31
                params.runtimeDeadlineMs, params.verbose);
32

  
33
        return trajs;
34
    }
35

  
36

  
37
}
src/main/resources/problems/55.xml
1
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
2
<multiagentproblem>
3
<environment>
4
<obstacles/>
5
<bounds>
6
<point>
7
<x>-1000</x>
8
<y>-1000</y>
9
</point>
10
<point>
11
<x>2000</x>
12
<y>2000</y>
13
</point>
14
</bounds>
15
</environment>
16
<agents>
17
<agent>
18
<start>
19
<point>
20
<x>800</x>
21
<y>250</y>
22
</point>
23
</start>
24
<target>
25
<point>
26
<x>200</x>
27
<y>800</y>
28
</point>
29
</target>
30
<radius>70</radius>
31
<maxspeed>1</maxspeed>
32
</agent>
33
<agent>
34
<start>
35
<point>
36
<x>650</x>
37
<y>800</y>
38
</point>
39
</start>
40
<target>
41
<point>
42
<x>100</x>
43
<y>200</y>
44
</point>
45
</target>
46
<radius>70</radius>
47
<maxspeed>1</maxspeed>
48
</agent>
49
<agent>
50
<start>
51
<point>
52
<x>600</x>
53
<y>300</y>
54
</point>
55
</start>
56
<target>
57
<point>
58
<x>550</x>
59
<y>500</y>
60
</point>
61
</target>
62
<radius>70</radius>
63
<maxspeed>1</maxspeed>
64
</agent>
65
<agent>
66
<start>
67
<point>
68
<x>150</x>
69
<y>200</y>
70
</point>
71
</start>
72
<target>
73
<point>
74
<x>500</x>
75
<y>800</y>
76
</point>
77
</target>
78
<radius>70</radius>
79
<maxspeed>1</maxspeed>
80
</agent>
81
</agents>
82
</multiagentproblem>
src/main/resources/problems/99.xml
1
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
2
<multiagentproblem>
3
<environment>
4
<obstacles/>
5
<bounds>
6
<point>
7
<x>-1000</x>
8
<y>-1000</y>
9
</point>
10
<point>
11
<x>2000</x>
12
<y>2000</y>
13
</point>
14
</bounds>
15
</environment>
16
<agents>
17
<agent>
18
<start>
19
<point>
20
<x>200</x>
21
<y>400</y>
22
</point>
23
</start>
24
<target>
25
<point>
26
<x>500</x>
27
<y>400</y>
28
</point>
29
</target>
30
<radius>70</radius>
31
<maxspeed>1</maxspeed>
32
</agent>
33
<agent>
34
<start>
35
<point>
36
<x>900</x>
37
<y>350</y>
38
</point>
39
</start>
40
<target>
41
<point>
42
<x>250</x>
43
<y>550</y>
44
</point>
45
</target>
46
<radius>70</radius>
47
<maxspeed>1</maxspeed>
48
</agent>
49
<agent>
50
<start>
51
<point>
52
<x>450</x>
53
<y>350</y>
54
</point>
55
</start>
56
<target>
57
<point>
58
<x>750</x>
59
<y>200</y>
60
</point>
61
</target>
62
<radius>70</radius>
63
<maxspeed>1</maxspeed>
64
</agent>
65
<agent>
66
<start>
67
<point>
68
<x>750</x>
69
<y>550</y>
70
</point>
71
</start>
72
<target>
73
<point>
74
<x>400</x>
75
<y>600</y>
76
</point>
77
</target>
78
<radius>70</radius>
79
<maxspeed>1</maxspeed>
80
</agent>
81
<agent>
82
<start>
83
<point>
84
<x>650</x>
85
<y>400</y>
86
</point>
87
</start>
88
<target>
89
<point>
90
<x>450</x>
91
<y>250</y>
92
</point>
93
</target>
94
<radius>70</radius>
95
<maxspeed>1</maxspeed>
96
</agent>
97
<agent>
98
<start>
99
<point>
100
<x>150</x>
101
<y>600</y>
102
</point>
103
</start>
104
<target>
105
<point>
106
<x>800</x>
107
<y>850</y>
108
</point>
109
</target>
110
<radius>70</radius>
111
<maxspeed>1</maxspeed>
112
</agent>
113
</agents>
114
</multiagentproblem>
src/test/java/tt/jointeuclid2ni/SolverTest.java
59 59

  
60 60
    @Test
61 61
    public void testKSFO1OnCrossconflict() throws FileNotFoundException {
62
        testSolver("-method KSFO -k 1 -problemfile src/test/resources/problems/cross_conflict.xml -timeout 2000 -maxtime 2500 -gridstep 50 -summary -grid 8",
62
        testSolver("-method KDPMD -k 1 -problemfile src/test/resources/problems/cross_conflict.xml -timeout 2000 -maxtime 2500 -gridstep 50 -summary -grid 8",
63 63
                "^1541.00;[0-9]*;[0-9]*;\n");
64 64
    }
65 65

  
66 66
    @Test
67 67
    public void testKSFO1OnCrossconflictL1() throws FileNotFoundException {
68
        testSolver("-method KSFO -k 1 -problemfile src/test/resources/problems/cross_conflict.xml -timeout 2000 -maxtime 2500 -gridstep 50 -summary -grid 8 -heuristic L1",
68
        testSolver("-method KDPMD -k 1 -problemfile src/test/resources/problems/cross_conflict.xml -timeout 2000 -maxtime 2500 -gridstep 50 -summary -grid 8 -heuristic L1",
69 69
                "^1570.00;[0-9]*;[0-9]*;\n");
70 70
    }
71 71

  
......
89 89

  
90 90
  @Test
91 91
  public void testKSFO1OnNarrowCorridorPrioritizedSolutionDoesntExist() throws FileNotFoundException {
92
      testSolver("-method KSFO -k 1 -problemfile src/test/resources/problems/stopatgoaltest.xml -timeout 2000 -maxtime 2500 -gridstep 30 -summary -grid 4",
92
      testSolver("-method KDPMD -k 1 -problemfile src/test/resources/problems/stopatgoaltest.xml -timeout 2000 -maxtime 2500 -gridstep 30 -summary -grid 4",
93 93
              "^inf;[0-9]*;[0-9]*;\n");
94 94
  }
95 95

  

Also available in: Unified diff