Revision 28:4dcebf67837e

View differences:

src/main/java/tt/jointtraj/demo/IterativeHomotopyPlannerDemo.java
1
package tt.jointtraj.demo;
2

  
3
import java.awt.Color;
4
import java.io.File;
5
import java.io.FileInputStream;
6
import java.io.FileNotFoundException;
7
import java.util.Arrays;
8
import java.util.Collection;
9
import java.util.HashSet;
10
import java.util.LinkedList;
11
import java.util.List;
12
import java.util.Random;
13
import java.util.Set;
14

  
15
import javax.vecmath.Point2d;
16

  
17
import org.jgrapht.DirectedGraph;
18
import org.jgrapht.Graph;
19
import org.jscience.mathematics.number.Complex;
20

  
21
import tt.euclid2i.EvaluatedTrajectory;
22
import tt.euclid2i.Line;
23
import tt.euclid2i.Point;
24
import tt.euclid2i.Region;
25
import tt.euclid2i.Trajectory;
26
import tt.euclid2i.discretization.AdditionalPointsExtension;
27
import tt.euclid2i.discretization.LazyGrid;
28
import tt.euclid2i.util.Util;
29
import tt.euclid2i.vis.RegionsLayer;
30
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
31
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
32
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
33
import tt.euclidtime3i.vis.TimeParameter;
34
import tt.jointeuclid2ni.probleminstance.AgentMission;
35
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblem;
36
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblemXMLDeserializer;
37
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblems;
38
import tt.jointeuclid2ni.probleminstance.generator.ConflictGenerator;
39
import tt.jointeuclid2ni.probleminstance.generator.exception.ProblemNotCreatedException;
40
import tt.jointtraj.homotopyiterative.IterativeHomotopyPlanner;
41
import tt.jointtraj.homotopyiterative.NewSolutionListener;
42
import tt.jointtraj.homotopyiterative.StochasticStrategy;
43
import tt.planner.homotopy.HomotopyUtils;
44
import tt.planner.homotopy.hvalue.HValueAnalyticIntegrator;
45
import tt.planner.homotopy.hvalue.HValueIntegrator;
46
import tt.util.AgentColors;
47
import tt.util.Verbose;
48
import tt.vis.ColoredTrajectoriesLayer;
49
import tt.vis.FastAgentsLayer;
50
import tt.vis.GraphLayer;
51
import tt.vis.LabeledPointLayer;
52
import tt.vis.ParameterControlLayer;
53
import cz.agents.alite.creator.Creator;
54
import cz.agents.alite.vis.VisManager;
55
import cz.agents.alite.vis.layer.common.ColorLayer;
56
import cz.agents.alite.vis.layer.common.VisInfoLayer;
57
import cz.agents.alite.vis.layer.toggle.KeyToggleLayer;
58

  
59
public class IterativeHomotopyPlannerDemo implements Creator {
60

  
61
    private static final int GRID_STEP = 30;
62
    private static final double MAX_PENALTY = 60;
63
    private static final int MAXTIME = 3000;
64
    private static final double EPS = 0.01;
65
    private static final int[][] PATTERN = LazyGrid.PATTERN_8_WAY;
66
    private Random random;
67

  
68
    public static void main(String[] args) {
69
        IterativeHomotopyPlannerDemo creator = new IterativeHomotopyPlannerDemo();
70
        creator.init(null);
71
        creator.create();
72
    }
73

  
74
    @Override
75
    public void init(String[] args) {
76
        random = new Random();
77
        Verbose.setVerbose(true);
78
        initVisualization();
79
    }
80

  
81
    @Override
82
    @SuppressWarnings("unchecked")
83
    public void create() {
84
        EarliestArrivalProblem problem = null;
85
        try {
86
            FileInputStream inputStream = new FileInputStream(new File("src/main/resources/problems/urbanmc.xml"));
87
            EarliestArrivalProblem serializedEnv = EarliestArrivalProblemXMLDeserializer.deserialize(inputStream);
88
            int[] bodyRadiuses = new int[3];
89
            Arrays.fill(bodyRadiuses, 50);
90
            problem = ConflictGenerator.generateSingleInstance(serializedEnv.getEnvironment(), bodyRadiuses, PATTERN, GRID_STEP, 4, serializedEnv.getBounds());
91
        } catch (FileNotFoundException e) {
92
            e.printStackTrace();
93
        } catch (ProblemNotCreatedException e) {
94
            e.printStackTrace();
95
        }
96

  
97
        DirectedGraph<Point, Line> graph[] = new DirectedGraph[problem.nAgents()];
98

  
99
        for (int i = 0; i < problem.nAgents(); i++) {
100
            graph[i] = createGrid(problem, i);
101
        }
102

  
103

  
104
        List<Complex> freeSpaceSamples = HomotopyUtils.sampleFreeSpace(problem.getObstacles(), problem.getBounds(), random);
105
        List<Complex> obstacleSamples = HomotopyUtils.sampleObstacles(problem.getObstacles(), random);
106
        HValueIntegrator integrator = new HValueAnalyticIntegrator(freeSpaceSamples, obstacleSamples);
107

  
108
        PairwiseConstraint constraint = new SeparationConstraint(new LinearSeparationPenaltyFunction(MAX_PENALTY), 10);
109
        AgentMission[] missions = EarliestArrivalProblems.agentMissions(problem);
110
        StochasticStrategy strategy = new StochasticStrategy(4, 120000, 444, true);
111

  
112
        long startTime = System.currentTimeMillis();
113

  
114
        NewSolutionListener solutionListener =  new NewSolutionListener() {
115

  
116
            public EvaluatedTrajectory[] solution = null;
117

  
118
            @Override
119
            public void notifyNewSolution(EvaluatedTrajectory[] trajs) {
120
                solution = trajs;
121
            }
122
        };
123

  
124
        final IterativeHomotopyPlanner planner = new IterativeHomotopyPlanner(strategy,
125
                graph,
126
                missions,
127
                constraint,
128
                integrator,
129
                MAXTIME,
130
                GRID_STEP
131
                /* waitMoveDuration */,
132
                solutionListener);
133

  
134
        visualizeProblem(problem, planner, graph[argmin(problem.getBodyRadiuses())]);
135

  
136
        planner.solve();
137
        System.out.printf("Real runtime %d%n", System.currentTimeMillis() - startTime);
138
    }
139

  
140
    private void initVisualization() {
141
        VisManager.setInitParam("Trajectory Tools Vis", 1024, 768, 200, 200);
142
        VisManager.setSceneParam(new VisManager.SceneParams() {
143

  
144
            @Override
145
            public Point2d getDefaultLookAt() {
146
                return new Point2d(500, 500);
147
            }
148

  
149
            @Override
150
            public double getDefaultZoomFactor() {
151
                return 0.5;
152
            }
153
        });
154
        VisManager.init();
155

  
156
        // background
157
        VisManager.registerLayer(ColorLayer.create(Color.WHITE));
158

  
159
        // Overlay
160
        VisManager.registerLayer(VisInfoLayer.create());
161
    }
162

  
163
    private void visualizeProblem(final EarliestArrivalProblem problem, final IterativeHomotopyPlanner planner, final Graph<Point, Line> graph) {
164

  
165
        VisManager.registerLayer(RegionsLayer.create(
166
                new RegionsLayer.RegionsProvider() {
167

  
168
                    @Override
169
                    public Collection<Region> getRegions() {
170
                        LinkedList<Region> list = new LinkedList<Region>();
171
                        list.add(problem.getBounds());
172
                        return list;
173
                    }
174

  
175
                }, Color.BLACK, Color.WHITE));
176

  
177
        Color inflatedRegionsColor = new Color(250, 250, 250);
178

  
179
        VisManager.registerLayer(RegionsLayer.create(
180
                new RegionsLayer.RegionsProvider() {
181

  
182
                    @Override
183
                    public Collection<Region> getRegions() {
184
                        return problem.getObstacles();
185
                    }
186

  
187
                }, Color.GRAY, Color.GRAY));
188

  
189
        VisManager.registerLayer(LabeledPointLayer.create(new LabeledPointLayer.LabeledPointsProvider<Point>() {
190

  
191
            @Override
192
            public Collection<LabeledPointLayer.LabeledPoint<Point>> getLabeledPoints() {
193
                LinkedList<LabeledPointLayer.LabeledPoint<Point>> list = new LinkedList<LabeledPointLayer.LabeledPoint<Point>>();
194

  
195
                for (int i = 0; i < problem.getStarts().length; i++) {
196
                    list.add(new LabeledPointLayer.LabeledPoint<Point>(problem.getStarts()[i], "s" + i));
197
                }
198
                return list;
199
            }
200

  
201
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.BLUE));
202

  
203

  
204
        VisManager.registerLayer(LabeledPointLayer.create(new LabeledPointLayer.LabeledPointsProvider<Point>() {
205

  
206
            @Override
207
            public Collection<LabeledPointLayer.LabeledPoint<Point>> getLabeledPoints() {
208
                LinkedList<LabeledPointLayer.LabeledPoint<Point>> list = new LinkedList<LabeledPointLayer.LabeledPoint<Point>>();
209

  
210
                for (int i = 0; i < problem.getTargets().length; i++) {
211
                    list.add(new LabeledPointLayer.LabeledPoint<Point>(problem.getTarget(i), "g" + i));
212
                }
213
                return list;
214
            }
215

  
216
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.RED));
217

  
218
        final TimeParameter time = new TimeParameter(20);
219
        VisManager.registerLayer(ParameterControlLayer.create(time));
220

  
221
        // visualize the graph
222
        VisManager.registerLayer(GraphLayer.create(new GraphLayer.GraphProvider<Point, Line>() {
223

  
224
            @Override
225
            public Graph<Point, Line> getGraph() {
226
                return ((AdditionalPointsExtension) graph).generateFullGraph(problem.getStart(0));
227
            }
228
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.GRAY, Color.GRAY, 1, 4));
229

  
230
        KeyToggleLayer bKeyLayer = KeyToggleLayer.create("b");
231
        bKeyLayer.addSubLayer(ColoredTrajectoriesLayer.create(
232
                new ColoredTrajectoriesLayer.TrajectoriesProvider<Point>() {
233
                    @Override
234
                    public tt.discrete.Trajectory<Point>[] getTrajectories() {
235
                        return planner.getBestSolution();
236
                    }
237
                }, new ColoredTrajectoriesLayer.ColorProvider() {
238
                    @Override
239
                    public Color getColor(int i) {
240
                        return AgentColors.getColorForAgent(i);
241
                    }
242
                }, new tt.euclid2i.vis.ProjectionTo2d(), 10, MAXTIME, 6, 's'
243
        ));
244
        bKeyLayer.addSubLayer(FastAgentsLayer.create(
245
                new FastAgentsLayer.TrajectoriesProvider() {
246

  
247
                    @Override
248
                    public Trajectory[] getTrajectories() {
249
                        return planner.getBestSolution();
250
                    }
251

  
252
                    @Override
253
                    public int[] getBodyRadiuses() {
254
                        return problem.getBodyRadiuses();
255
                    }
256

  
257
                }, new FastAgentsLayer.ColorProvider() {
258
                    @Override
259
                    public Color getColor(int i) {
260
                        return AgentColors.getColorForAgent(i);
261
                    }
262
                }, time
263
        ));
264

  
265
        VisManager.registerLayer(bKeyLayer);
266

  
267

  
268
        KeyToggleLayer cKeyLayer = KeyToggleLayer.create("c");
269
        cKeyLayer.addSubLayer(ColoredTrajectoriesLayer.create(
270
                new ColoredTrajectoriesLayer.TrajectoriesProvider<Point>() {
271
                    @Override
272
                    public tt.discrete.Trajectory<Point>[] getTrajectories() {
273
                        return planner.getCurrentSolution();
274
                    }
275
                }, new ColoredTrajectoriesLayer.ColorProvider() {
276
                    @Override
277
                    public Color getColor(int i) {
278
                        return AgentColors.getColorForAgent(i).brighter();
279
                    }
280
                }, new tt.euclid2i.vis.ProjectionTo2d(), GRID_STEP/4, MAXTIME, 6, 's'
281
        ));
282
        cKeyLayer.addSubLayer(FastAgentsLayer.create(
283
                new FastAgentsLayer.TrajectoriesProvider() {
284

  
285
                    @Override
286
                    public Trajectory[] getTrajectories() {
287
                        return planner.getCurrentSolution();
288
                    }
289

  
290
                    @Override
291
                    public int[] getBodyRadiuses() {
292
                        return problem.getBodyRadiuses();
293
                    }
294

  
295
                }, new FastAgentsLayer.ColorProvider() {
296
                    @Override
297
                    public Color getColor(int i) {
298
                        return AgentColors.getColorForAgent(i).brighter();
299
                    }
300
                }, time
301
        ));
302
        VisManager.registerLayer(cKeyLayer);
303
        VisManager.init();
304
    }
305

  
306
    private DirectedGraph<Point, Line> createGrid(EarliestArrivalProblem problem, int i) {
307
        Set<Point> points = new HashSet<Point>();
308
        points.addAll(Arrays.asList(problem.getStarts()));
309
        points.addAll(Arrays.asList(problem.getTargets()));
310

  
311
        Collection<Region> inflatedObstacles = Util.inflateRegions(problem.getObstacles(), problem.getBodyRadius(i));
312

  
313
        // create discretization
314
        final DirectedGraph<tt.euclid2i.Point, tt.euclid2i.Line> spatialGraph
315
                = new AdditionalPointsExtension(
316
                new LazyGrid(problem.getStart(i), inflatedObstacles, problem.getBounds(),
317
                        PATTERN, GRID_STEP).generateFullGraph() //it's important to generate the full graph
318
                , points, GRID_STEP);
319

  
320
        // create spatio-temporal graph
321
        return spatialGraph;
322
    }
323

  
324
    private int argmin(int... arr) {
325
        int min = Integer.MAX_VALUE;
326
        int argmin = -1;
327

  
328
        for (int i = 0; i < arr.length; i++) {
329
            if (min > arr[i]) {
330
                min = arr[i];
331
                argmin = i;
332
            }
333
        }
334

  
335
        return argmin;
336
    }
337
}
src/main/java/tt/jointtraj/demo/IterativeSolverDemo.java
1
package tt.jointtraj.demo;
2

  
3
import cz.agents.alite.creator.Creator;
4
import cz.agents.alite.vis.VisManager;
5
import cz.agents.alite.vis.VisManager.SceneParams;
6
import cz.agents.alite.vis.layer.common.ColorLayer;
7
import cz.agents.alite.vis.layer.common.VisInfoLayer;
8
import org.jgrapht.DirectedGraph;
9
import org.jgrapht.Graph;
10
import tt.euclid2i.Line;
11
import tt.euclid2i.Point;
12
import tt.euclid2i.Region;
13
import tt.euclid2i.Trajectory;
14
import tt.euclid2i.discretization.LazyGrid;
15
import tt.euclid2i.discretization.ProbabilisticRoadmap;
16
import tt.euclid2i.discretization.ToGoalEdgeExtension;
17
import tt.euclid2i.util.Util;
18
import tt.euclid2i.vis.RegionsLayer;
19
import tt.euclid2i.vis.RegionsLayer.RegionsProvider;
20
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
21
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
22
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
23
import tt.euclidtime3i.vis.TimeParameter;
24
import tt.euclidtime3i.vis.TimeParameterProjectionTo2d;
25
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblem;
26
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblems;
27
import tt.jointeuclid2ni.probleminstance.NarrowPassageProblem;
28
import tt.jointtraj.solver.AStarGraphOptimizer;
29
import tt.jointtraj.solver.IterativeSolver;
30
import tt.jointtraj.solver.TrajectoryConstraintOptimizer;
31
import tt.vis.*;
32
import tt.vis.GraphLayer.GraphProvider;
33
import tt.vis.LabeledPointLayer.LabeledPoint;
34
import tt.vis.LabeledPointLayer.LabeledPointsProvider;
35
import tt.vis.TrajectoriesLayer.TrajectoriesProvider;
36

  
37
import javax.vecmath.Point2d;
38
import java.awt.*;
39
import java.util.Arrays;
40
import java.util.Collection;
41
import java.util.LinkedList;
42
import java.util.Random;
43

  
44
public class IterativeSolverDemo implements Creator {
45

  
46
    static final int GRID_STEP = 10;
47
    static final double MAX_PENALTY = 60;
48
    static final int MAXTIME = 8000;
49
    static final double EPS = 0.01;
50

  
51
    @Override
52
    public void init(String[] args) {
53
    }
54

  
55
    @Override
56
    public void create() {
57

  
58
        initVisualization();
59

  
60
        // create time parameter
61

  
62
        final TimeParameter time = new TimeParameter(20);
63
        VisManager.registerLayer(ParameterControlLayer.create(time));
64

  
65
        // start positions
66

  
67
        //EarliestArrivalProblem problem = new SuperconflictProblem(new RandomEnvironment(1000, 1000, 0, 100, 3), 6, 50, 1);
68
        EarliestArrivalProblem problem = new NarrowPassageProblem();
69

  
70
        Collection<Region> inflatedObstacles = Util.inflateRegions(problem.getObstacles(), problem.getBodyRadius(0));
71

  
72
        visualizeProblem(problem);
73

  
74
        // Roadmap
75

  
76
        DirectedGraph<Point, Line> roadmap
77
                = createRoadmap(problem.getStarts(), problem.getTargets(), inflatedObstacles, 3000, 150);
78

  
79
        // Optimizers
80

  
81
        TrajectoryConstraintOptimizer optimizers[] = new TrajectoryConstraintOptimizer[problem.nAgents()];
82

  
83
        for (int i = 0; i < problem.nAgents(); i++) {
84
            int[] separations = EarliestArrivalProblems.getRelativeSeparations(problem.getBodyRadiuses(), i);
85
            optimizers[i] = new AStarGraphOptimizer(roadmap, new tt.euclidtime3i.Point(problem.getStart(i), 0), problem.getTarget(i), 1, separations, MAXTIME);
86
        }
87

  
88
        // Constraint
89

  
90
        PairwiseConstraint constraint = new SeparationConstraint(new LinearSeparationPenaltyFunction(MAX_PENALTY), 10);
91

  
92
        final IterativeSolver solver = new IterativeSolver(optimizers, constraint);
93

  
94
        VisManager.registerLayer(TrajectoriesLayer.create(new TrajectoriesProvider<tt.euclid2i.Point>() {
95
            @Override
96
            public tt.discrete.Trajectory<tt.euclid2i.Point>[] getTrajectories() {
97
                return solver.getTrajs();
98
            }
99
        }, new tt.euclid2i.vis.ProjectionTo2d(), 10, MAXTIME, 6, 's'));
100

  
101
        int i = 0;
102
        for (double w = 0; w <= 1.0; w = w + EPS) {
103
            System.out.print("Iteration: " + i++ + " Weight: " + String.format("%.4f", w));
104
            solver.iterate(w);
105
            System.out.println(" ...finished." + " Cost:" + solver.getCost());
106
            try {
107
                Thread.sleep(0);
108
            } catch (InterruptedException e) {
109
                e.printStackTrace();
110
            }
111
        }
112

  
113
        VisManager.registerLayer(CylindricAgentsLayer.create(new CylindricAgentsLayer.TrajectoriesProvider() {
114

  
115
            @Override
116
            public Trajectory[] getTrajectories() {
117
                return solver.getTrajs();
118
            }
119
        }, new TimeParameterProjectionTo2d(time), problem.getBodyRadiuses(), 1));
120

  
121
    }
122

  
123
    private DirectedGraph<tt.euclid2i.Point, Line> createRoadmap(final tt.euclid2i.Point[] inits, final tt.euclid2i.Point[] goals, Collection<Region> obstacles, int n, int radius) {
124

  
125
        LinkedList<tt.euclid2i.Point> customPoints = new LinkedList<tt.euclid2i.Point>();
126
        customPoints.addAll(Arrays.asList(inits));
127
        customPoints.addAll(Arrays.asList(goals));
128

  
129
        // create discretization
130
        final DirectedGraph<tt.euclid2i.Point, tt.euclid2i.Line> spatialGraph
131
                = new ProbabilisticRoadmap(n, radius, customPoints.toArray(new tt.euclid2i.Point[]{}),
132
                new tt.euclid2i.region.Rectangle(new tt.euclid2i.Point(0, 0),
133
                        new tt.euclid2i.Point(1000, 1000)),
134
                obstacles,
135
                new Random(1));
136

  
137
        VisManager.registerLayer(GraphLayer.create(new GraphProvider<tt.euclid2i.Point, tt.euclid2i.Line>() {
138

  
139
            @Override
140
            public Graph<tt.euclid2i.Point, tt.euclid2i.Line> getGraph() {
141
                return spatialGraph;
142
            }
143
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.LIGHT_GRAY, Color.GRAY, 1, 4));
144

  
145
        // create spatio-temporal graph
146
        return spatialGraph;
147
    }
148

  
149
    private DirectedGraph<tt.euclid2i.Point, Line> createGrid(final tt.euclid2i.Point init, final tt.euclid2i.Point goalPoint) {
150

  
151
        // create discretization
152
        final DirectedGraph<tt.euclid2i.Point, tt.euclid2i.Line> spatialGraph
153
                = new ToGoalEdgeExtension(
154
                new LazyGrid(init,
155
                        new LinkedList<tt.euclid2i.Region>(),
156
                        new tt.euclid2i.region.Rectangle(new tt.euclid2i.Point(0, 0), new tt.euclid2i.Point(1000, 1000)),
157
                        LazyGrid.PATTERN_16_WAY,
158
                        GRID_STEP)
159
                , goalPoint, GRID_STEP);
160

  
161
        // visualize the graph
162
//        VisManager.registerLayer(GraphLayer.create(new GraphProvider<tt.euclid2i.Point, tt.euclid2i.Line>() {
163
//
164
//            @Override
165
//            public Graph<tt.euclid2i.Point, tt.euclid2i.Line> getGraph() {
166
//                return ((ToGoalEdgeExtension) spatialGraph).generateFullGraph(init);
167
//            }
168
//        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.GRAY, Color.GRAY, 1, 4));
169

  
170
        // create spatio-temporal graph
171
        return spatialGraph;
172
    }
173

  
174
    private void initVisualization() {
175
        VisManager.setInitParam("Trajectory Tools Vis", 1024, 768, 200, 200);
176
        VisManager.setSceneParam(new SceneParams() {
177

  
178
            @Override
179
            public Point2d getDefaultLookAt() {
180
                return new Point2d(500, 500);
181
            }
182

  
183
            @Override
184
            public double getDefaultZoomFactor() {
185
                return 0.5;
186
            }
187
        });
188

  
189
        VisManager.init();
190

  
191
        // background
192
        VisManager.registerLayer(ColorLayer.create(Color.WHITE));
193

  
194
        // Overlay
195
        VisManager.registerLayer(VisInfoLayer.create());
196
    }
197

  
198
    private void visualizeProblem(final EarliestArrivalProblem problem) {
199

  
200
        VisManager.registerLayer(RegionsLayer.create(
201
                new RegionsProvider() {
202

  
203
                    @Override
204
                    public Collection<Region> getRegions() {
205
                        LinkedList<Region> list = new LinkedList<Region>();
206
                        list.add(problem.getBounds());
207
                        return list;
208
                    }
209

  
210
                }, Color.BLACK, Color.WHITE));
211

  
212
        Color inflatedRegionsColor = new Color(250, 250, 250);
213

  
214
        VisManager.registerLayer(RegionsLayer.create(
215
                new RegionsProvider() {
216

  
217
                    @Override
218
                    public Collection<Region> getRegions() {
219
                        return problem.getObstacles();
220
                    }
221

  
222
                }, Color.GRAY, Color.GRAY));
223

  
224
        VisManager.registerLayer(LabeledPointLayer.create(new LabeledPointsProvider<Point>() {
225

  
226
            @Override
227
            public Collection<LabeledPoint<Point>> getLabeledPoints() {
228
                LinkedList<LabeledPoint<Point>> list = new LinkedList<LabeledPoint<Point>>();
229

  
230
                for (int i = 0; i < problem.getStarts().length; i++) {
231
                    list.add(new LabeledPoint<Point>(problem.getStarts()[i], "s" + i));
232
                }
233
                return list;
234
            }
235

  
236
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.BLUE));
237

  
238
        VisManager.registerLayer(LabeledPointLayer.create(new LabeledPointsProvider<Point>() {
239

  
240
            @Override
241
            public Collection<LabeledPoint<Point>> getLabeledPoints() {
242
                LinkedList<LabeledPoint<Point>> list = new LinkedList<LabeledPoint<Point>>();
243

  
244
                for (int i = 0; i < problem.getTargets().length; i++) {
245
                    list.add(new LabeledPoint<Point>(problem.getTarget(i), "g" + i));
246
                }
247
                return list;
248
            }
249

  
250
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.RED));
251
    }
252

  
253
    public static void main(String[] args) {
254
        new IterativeSolverDemo().create();
255
    }
256

  
257

  
258
}
src/main/java/tt/jointtraj/demo/NarrowPassageSwapDemo.java
1
package tt.jointtraj.demo;
2

  
3
import java.awt.Color;
4
import java.util.Arrays;
5
import java.util.Collection;
6
import java.util.LinkedList;
7
import java.util.Random;
8

  
9
import javax.vecmath.Point2d;
10

  
11
import org.jgrapht.DirectedGraph;
12
import org.jgrapht.Graph;
13

  
14
import tt.euclid2i.Line;
15
import tt.euclid2i.Point;
16
import tt.euclid2i.Region;
17
import tt.euclid2i.Trajectory;
18
import tt.euclid2i.discretization.LazyGrid;
19
import tt.euclid2i.discretization.ProbabilisticRoadmap;
20
import tt.euclid2i.discretization.ToGoalEdgeExtension;
21
import tt.euclid2i.util.Util;
22
import tt.euclid2i.vis.RegionsLayer;
23
import tt.euclid2i.vis.RegionsLayer.RegionsProvider;
24
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
25
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
26
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
27
import tt.euclidtime3i.vis.TimeParameter;
28
import tt.euclidtime3i.vis.TimeParameterProjectionTo2d;
29
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblem;
30
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblems;
31
import tt.jointeuclid2ni.probleminstance.NarrowPassageProblem;
32
import tt.jointtraj.solver.AStarGraphOptimizer;
33
import tt.jointtraj.solver.IterativeSolver;
34
import tt.jointtraj.solver.TrajectoryConstraintOptimizer;
35
import tt.vis.CylindricAgentsLayer;
36
import tt.vis.GraphLayer;
37
import tt.vis.GraphLayer.GraphProvider;
38
import tt.vis.LabeledPointLayer;
39
import tt.vis.LabeledPointLayer.LabeledPoint;
40
import tt.vis.LabeledPointLayer.LabeledPointsProvider;
41
import tt.vis.ParameterControlLayer;
42
import tt.vis.TrajectoriesLayer;
43
import tt.vis.TrajectoriesLayer.TrajectoriesProvider;
44
import cz.agents.alite.creator.Creator;
45
import cz.agents.alite.vis.VisManager;
46
import cz.agents.alite.vis.VisManager.SceneParams;
47
import cz.agents.alite.vis.layer.common.ColorLayer;
48
import cz.agents.alite.vis.layer.common.VisInfoLayer;
49

  
50
public class NarrowPassageSwapDemo implements Creator {
51

  
52
    static final int GRID_STEP = 10;
53
    static final double MAX_PENALTY = 55;
54
    static final int MAXTIME = 8000;
55
    static final double EPS = 0.02;
56

  
57
    @Override
58
    public void init(String[] args) {}
59

  
60
    @Override
61
    public void create() {
62

  
63
        initVisualization();
64

  
65
        // create time parameter
66

  
67
        final TimeParameter time = new TimeParameter(20);
68
        VisManager.registerLayer(ParameterControlLayer.create(time));
69

  
70
        // start positions
71

  
72
        //EarliestArrivalProblem problem = new SuperconflictProblem(new RandomEnvironment(1000, 1000, 0, 100, 3), 6, 50, 1);
73
        EarliestArrivalProblem problem = new NarrowPassageProblem();
74

  
75
        visualizeProblem(problem);
76

  
77
        // Roadmap
78

  
79
        Collection<Region> inflatedObstacles = Util.inflateRegions(problem.getObstacles(), problem.getBodyRadius(0));
80
        DirectedGraph<Point, Line> roadmap = createRoadmap(problem.getStarts(), problem.getTargets(), inflatedObstacles, 3000, 90);
81

  
82
        // Optimizers
83

  
84
        TrajectoryConstraintOptimizer optimizers[] = new TrajectoryConstraintOptimizer[problem.nAgents()];
85

  
86
        for(int i=0; i<problem.nAgents(); i++) {
87
            int[] separations = EarliestArrivalProblems.getRelativeSeparations(problem.getBodyRadiuses(), i);
88
            optimizers[i] = new AStarGraphOptimizer(
89
                    roadmap,
90
                    new tt.euclidtime3i.Point(problem.getStart(i),0),
91
                    problem.getTarget(i),
92
                    1,
93
                    separations,
94
                    MAXTIME);
95
        }
96

  
97
        // Constraint
98

  
99
        PairwiseConstraint constraint = new SeparationConstraint(new LinearSeparationPenaltyFunction(MAX_PENALTY), 10);
100

  
101
        final IterativeSolver solver = new IterativeSolver(optimizers,  constraint);
102

  
103
        VisManager.registerLayer(TrajectoriesLayer.create(new TrajectoriesProvider<tt.euclid2i.Point>() {
104
            @Override
105
            public tt.discrete.Trajectory<tt.euclid2i.Point>[] getTrajectories() {
106
                return solver.getTrajs();
107
            }
108
        }, new tt.euclid2i.vis.ProjectionTo2d(), 10, MAXTIME, 6, 's'));
109

  
110
        int i=0;
111
           for(double w=0; w <= 1.0; w = w + EPS) {
112
            System.out.print("Iteration: " + i++ + " Weight: " + String.format("%.4f", w));
113
            solver.iterate(w);
114
            System.out.println(" ...finished."  + " Cost:" + solver.getCost());
115
            try {
116
                Thread.sleep(0);
117
            } catch (InterruptedException e) {
118
                e.printStackTrace();
119
            }
120
        }
121

  
122
        VisManager.registerLayer(CylindricAgentsLayer.create(new CylindricAgentsLayer.TrajectoriesProvider() {
123

  
124
            @Override
125
            public Trajectory[] getTrajectories() {
126
                return solver.getTrajs();
127
            }
128
        }, new TimeParameterProjectionTo2d(time), problem.getBodyRadiuses(), 1));
129

  
130
    }
131

  
132
    private DirectedGraph<tt.euclid2i.Point, Line> createRoadmap(final tt.euclid2i.Point[] inits, final tt.euclid2i.Point[] goals, Collection<Region> obstacles, int n, int radius) {
133

  
134
        LinkedList<tt.euclid2i.Point> customPoints = new LinkedList<tt.euclid2i.Point>();
135
        customPoints.addAll(Arrays.asList(inits));
136
        customPoints.addAll(Arrays.asList(goals));
137

  
138
        // create discretization
139
        final DirectedGraph<tt.euclid2i.Point, tt.euclid2i.Line> spatialGraph
140
            = new ProbabilisticRoadmap(n, radius, customPoints.toArray(new tt.euclid2i.Point[] {}),
141
                    new tt.euclid2i.region.Rectangle(new tt.euclid2i.Point(0,0),
142
                            new tt.euclid2i.Point(1000,1000)),
143
                            obstacles,
144
                            new Random(1));
145

  
146
        VisManager.registerLayer(GraphLayer.create(new GraphProvider<tt.euclid2i.Point, tt.euclid2i.Line>() {
147

  
148
            @Override
149
            public Graph<tt.euclid2i.Point, tt.euclid2i.Line> getGraph() {
150
               return spatialGraph;
151
            }
152
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.LIGHT_GRAY, Color.GRAY, 1, 4));
153

  
154
        // create spatio-temporal graph
155
        return spatialGraph;
156
    }
157

  
158
    private DirectedGraph<tt.euclid2i.Point, Line> createGrid(final tt.euclid2i.Point init, final tt.euclid2i.Point goalPoint) {
159

  
160
        // create discretization
161
        final DirectedGraph<tt.euclid2i.Point, tt.euclid2i.Line> spatialGraph
162
            = new ToGoalEdgeExtension(
163
                new LazyGrid(init,
164
                        new LinkedList<tt.euclid2i.Region>(),
165
                        new tt.euclid2i.region.Rectangle(new tt.euclid2i.Point(0,0), new tt.euclid2i.Point(1000,1000)),
166
                        LazyGrid.PATTERN_16_WAY,
167
                        GRID_STEP)
168
                ,goalPoint, GRID_STEP);
169

  
170
        // visualize the graph
171
//        VisManager.registerLayer(GraphLayer.create(new GraphProvider<tt.euclid2i.Point, tt.euclid2i.Line>() {
172
//
173
//            @Override
174
//            public Graph<tt.euclid2i.Point, tt.euclid2i.Line> getGraph() {
175
//                return ((ToGoalEdgeExtension) spatialGraph).generateFullGraph(init);
176
//            }
177
//        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.GRAY, Color.GRAY, 1, 4));
178

  
179
        // create spatio-temporal graph
180
        return spatialGraph;
181
    }
182

  
183
    private void initVisualization() {
184
        VisManager.setInitParam("Trajectory Tools Vis", 1024, 768, 200, 200);
185
        VisManager.setSceneParam(new SceneParams(){
186

  
187
            @Override
188
            public Point2d getDefaultLookAt() {
189
                return new Point2d(500,500);
190
            }
191

  
192
            @Override
193
            public double getDefaultZoomFactor() {
194
                return 0.5;
195
            } } );
196

  
197
        VisManager.init();
198

  
199
        // background
200
        VisManager.registerLayer(ColorLayer.create(Color.WHITE));
201

  
202
        // Overlay
203
        VisManager.registerLayer(VisInfoLayer.create());
204
    }
205

  
206
    private void visualizeProblem(final EarliestArrivalProblem problem) {
207

  
208
        VisManager.registerLayer(RegionsLayer.create(
209
                new RegionsProvider() {
210

  
211
                    @Override
212
                    public Collection<Region> getRegions() {
213
                        LinkedList<Region> list = new LinkedList<Region>();
214
                        list.add(problem.getBounds());
215
                        return list;
216
                    }
217

  
218
                }, Color.BLACK, Color.WHITE));
219

  
220
        Color inflatedRegionsColor = new Color(250,250,250);
221

  
222
        VisManager.registerLayer(RegionsLayer.create(
223
                new RegionsProvider() {
224

  
225
                    @Override
226
                    public Collection<Region> getRegions() {
227
                        return problem.getObstacles();
228
                    }
229

  
230
                }, Color.GRAY, Color.GRAY));
231

  
232
        VisManager.registerLayer(LabeledPointLayer.create(new LabeledPointsProvider<Point>() {
233

  
234
            @Override
235
            public Collection<LabeledPoint<Point>> getLabeledPoints() {
236
                LinkedList<LabeledPoint<Point>> list = new LinkedList<LabeledPoint<Point>>();
237

  
238
                for (int i=0; i < problem.getStarts().length; i++) {
239
                    list.add(new LabeledPoint<Point>(problem.getStarts()[i], "s"+i));
240
                }
241
                return list;
242
            }
243

  
244
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.BLUE));
245

  
246

  
247
        VisManager.registerLayer(LabeledPointLayer.create(new LabeledPointsProvider<Point>() {
248

  
249
            @Override
250
            public Collection<LabeledPoint<Point>> getLabeledPoints() {
251
                LinkedList<LabeledPoint<Point>> list = new LinkedList<LabeledPoint<Point>>();
252

  
253
                for (int i=0; i < problem.getTargets().length; i++) {
254
                    list.add(new LabeledPoint<Point>(problem.getTarget(i), "g"+i));
255
                }
256
                return list;
257
            }
258

  
259
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.RED));
260
    }
261

  
262
    public static void main(String[] args) {
263
        new NarrowPassageSwapDemo().create();
264
    }
265

  
266

  
267

  
268

  
269
}
src/main/java/tt/jointtraj/demo/SuperconflictDemo.java
1
package tt.jointtraj.demo;
2

  
3
import java.awt.Color;
4
import java.util.Arrays;
5
import java.util.Collection;
6
import java.util.HashSet;
7
import java.util.LinkedList;
8
import java.util.Random;
9
import java.util.Set;
10

  
11
import javax.vecmath.Point2d;
12

  
13
import org.jgrapht.DirectedGraph;
14
import org.jgrapht.Graph;
15

  
16
import tt.euclid2i.Line;
17
import tt.euclid2i.Point;
18
import tt.euclid2i.Region;
19
import tt.euclid2i.Trajectory;
20
import tt.euclid2i.discretization.AdditionalPointsExtension;
21
import tt.euclid2i.discretization.LazyGrid;
22
import tt.euclid2i.discretization.ProbabilisticRoadmap;
23
import tt.euclid2i.probleminstance.RandomEnvironment;
24
import tt.euclid2i.vis.RegionsLayer;
25
import tt.euclid2i.vis.RegionsLayer.RegionsProvider;
26
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
27
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
28
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
29
import tt.euclidtime3i.vis.TimeParameter;
30
import tt.euclidtime3i.vis.TimeParameterProjectionTo2d;
31
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblem;
32
import tt.jointeuclid2ni.probleminstance.EarliestArrivalProblems;
33
import tt.jointeuclid2ni.probleminstance.SuperconflictProblem;
34
import tt.jointtraj.solver.AStarGraphOptimizer;
35
import tt.jointtraj.solver.IterativeSolver;
36
import tt.jointtraj.solver.TrajectoryConstraintOptimizer;
37
import tt.vis.CylindricAgentsLayer;
38
import tt.vis.GraphLayer;
39
import tt.vis.GraphLayer.GraphProvider;
40
import tt.vis.LabeledPointLayer;
41
import tt.vis.LabeledPointLayer.LabeledPoint;
42
import tt.vis.LabeledPointLayer.LabeledPointsProvider;
43
import tt.vis.ParameterControlLayer;
44
import tt.vis.TrajectoriesLayer;
45
import tt.vis.TrajectoriesLayer.TrajectoriesProvider;
46
import cz.agents.alite.creator.Creator;
47
import cz.agents.alite.vis.VisManager;
48
import cz.agents.alite.vis.VisManager.SceneParams;
49
import cz.agents.alite.vis.layer.common.ColorLayer;
50
import cz.agents.alite.vis.layer.common.VisInfoLayer;
51

  
52
public class SuperconflictDemo implements Creator {
53

  
54
    static final int GRID_STEP = 40;
55
    static final double MAX_PENALTY = 10;
56
    static final int MAXTIME = 8000;
57
    static final double EPS = 0.01;
58

  
59
    @Override
60
    public void init(String[] args) {
61
    }
62

  
63
    @Override
64
    public void create() {
65

  
66
        initVisualization();
67

  
68
        // create time parameter
69

  
70
        final TimeParameter time = new TimeParameter(20);
71
        VisManager.registerLayer(ParameterControlLayer.create(time));
72

  
73
        // start positions
74
        EarliestArrivalProblem problem = new SuperconflictProblem(new RandomEnvironment(1000, 1000, 0, 100, 3), 6, 50);
75

  
76
        visualizeProblem(problem);
77

  
78
        // Roadmap
79

  
80
        DirectedGraph<Point, Line> roadmap = createGrid(problem.getStarts(), problem.getTargets());
81

  
82
        // Optimizers
83

  
84
        TrajectoryConstraintOptimizer optimizers[] = new TrajectoryConstraintOptimizer[problem.nAgents()];
85

  
86
        for (int i = 0; i < problem.nAgents(); i++) {
87
            int[] separations = EarliestArrivalProblems.getRelativeSeparations(problem.getBodyRadiuses(), i);
88
            optimizers[i] = new AStarGraphOptimizer(
89
                    roadmap,
90
                    new tt.euclidtime3i.Point(problem.getStart(i), 0),
91
                    problem.getTarget(i),
92
                    1,
93
                    separations,
94
                    MAXTIME);
95
        }
96

  
97
        // Constraint
98

  
99
        PairwiseConstraint constraint = new SeparationConstraint(new LinearSeparationPenaltyFunction(MAX_PENALTY), 10);
100

  
101
        final IterativeSolver solver = new IterativeSolver(optimizers, constraint);
102

  
103
        VisManager.registerLayer(TrajectoriesLayer.create(new TrajectoriesProvider<tt.euclid2i.Point>() {
104
            @Override
105
            public tt.discrete.Trajectory<tt.euclid2i.Point>[] getTrajectories() {
106
                return solver.getTrajs();
107
            }
108
        }, new tt.euclid2i.vis.ProjectionTo2d(), 10, MAXTIME, 6, 's'));
109

  
110
        // **** solve ****
111
        int i = 0;
112
        for (double w = 0; w <= 1.0; w = w + EPS) {
113
            System.out.print("Iteration: " + i++ + " Weight: " + String.format("%.4f", w));
114
            solver.iterate(w);
115
            System.out.println(" ...finished." + " Cost:" + solver.getCost());
116
        }
117

  
118
        VisManager.registerLayer(CylindricAgentsLayer.create(new CylindricAgentsLayer.TrajectoriesProvider() {
119

  
120
            @Override
121
            public Trajectory[] getTrajectories() {
122
                return solver.getTrajs();
123
            }
124
        }, new TimeParameterProjectionTo2d(time), problem.getBodyRadiuses(), 1));
125

  
126
    }
127

  
128
    private DirectedGraph<tt.euclid2i.Point, Line> createRoadmap(final tt.euclid2i.Point[] inits, final tt.euclid2i.Point[] goals, Collection<Region> obstacles, int n, int radius) {
129

  
130
        LinkedList<tt.euclid2i.Point> customPoints = new LinkedList<tt.euclid2i.Point>();
131
        customPoints.addAll(Arrays.asList(inits));
132
        customPoints.addAll(Arrays.asList(goals));
133

  
134
        // create discretization
135
        final DirectedGraph<tt.euclid2i.Point, tt.euclid2i.Line> spatialGraph
136
                = new ProbabilisticRoadmap(n, radius, customPoints.toArray(new tt.euclid2i.Point[]{}),
137
                new tt.euclid2i.region.Rectangle(new tt.euclid2i.Point(0, 0),
138
                        new tt.euclid2i.Point(1000, 1000)),
139
                obstacles,
140
                new Random(1));
141

  
142
        VisManager.registerLayer(GraphLayer.create(new GraphProvider<tt.euclid2i.Point, tt.euclid2i.Line>() {
143

  
144
            @Override
145
            public Graph<tt.euclid2i.Point, tt.euclid2i.Line> getGraph() {
146
                return spatialGraph;
147
            }
148
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.LIGHT_GRAY, Color.GRAY, 1, 4));
149

  
150
        // create spatio-temporal graph
151
        return spatialGraph;
152
    }
153

  
154
    private DirectedGraph<tt.euclid2i.Point, Line> createGrid(final tt.euclid2i.Point[] inits, final tt.euclid2i.Point[] goals) {
155
        Set<tt.euclid2i.Point> points = new HashSet<tt.euclid2i.Point>();
156
        points.addAll(Arrays.asList(inits));
157
        points.addAll(Arrays.asList(goals));
158

  
159
        // create discretization
160
        final DirectedGraph<tt.euclid2i.Point, tt.euclid2i.Line> spatialGraph
161
                = new AdditionalPointsExtension(
162
                new LazyGrid(
163
                        inits[0],
164
                        new LinkedList<tt.euclid2i.Region>(),
165
                        new tt.euclid2i.region.Rectangle(
166
                                new tt.euclid2i.Point(0, 0),
167
                                new tt.euclid2i.Point(1000, 1000)),
168
                        LazyGrid.PATTERN_16_WAY,
169
                        GRID_STEP).generateFullGraph() //it's important to generate the full graph
170
                , points, GRID_STEP);
171

  
172
        // visualize the graph
173
        VisManager.registerLayer(GraphLayer.create(new GraphProvider<tt.euclid2i.Point, tt.euclid2i.Line>() {
174

  
175
            @Override
176
            public Graph<tt.euclid2i.Point, tt.euclid2i.Line> getGraph() {
177
                return ((AdditionalPointsExtension) spatialGraph).generateFullGraph(inits[0]);
178
            }
179
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.GRAY, Color.GRAY, 1, 4));
180

  
181
        // create spatio-temporal graph
182
        return spatialGraph;
183
    }
184

  
185
    private void initVisualization() {
186
        VisManager.setInitParam("Trajectory Tools Vis", 1024, 768, 200, 200);
187
        VisManager.setSceneParam(new SceneParams() {
188

  
189
            @Override
190
            public Point2d getDefaultLookAt() {
191
                return new Point2d(500, 500);
192
            }
193

  
194
            @Override
195
            public double getDefaultZoomFactor() {
196
                return 0.5;
197
            }
198
        });
199

  
200
        VisManager.init();
201

  
202
        // background
203
        VisManager.registerLayer(ColorLayer.create(Color.WHITE));
204

  
205
        // Overlay
206
        VisManager.registerLayer(VisInfoLayer.create());
207
    }
208

  
209
    private void visualizeProblem(final EarliestArrivalProblem problem) {
210

  
211
        VisManager.registerLayer(RegionsLayer.create(
212
                new RegionsProvider() {
213

  
214
                    @Override
215
                    public Collection<Region> getRegions() {
216
                        LinkedList<Region> list = new LinkedList<Region>();
217
                        list.add(problem.getBounds());
218
                        return list;
219
                    }
220

  
221
                }, Color.BLACK, Color.WHITE));
222

  
223

  
224
        VisManager.registerLayer(RegionsLayer.create(
225
                new RegionsProvider() {
226

  
227
                    @Override
228
                    public Collection<Region> getRegions() {
229
                        return problem.getObstacles();
230
                    }
231

  
232
                }, Color.GRAY, Color.GRAY));
233

  
234
        VisManager.registerLayer(LabeledPointLayer.create(new LabeledPointsProvider<Point>() {
235

  
236
            @Override
237
            public Collection<LabeledPoint<Point>> getLabeledPoints() {
238
                LinkedList<LabeledPoint<Point>> list = new LinkedList<LabeledPoint<Point>>();
239

  
240
                for (int i = 0; i < problem.getStarts().length; i++) {
241
                    list.add(new LabeledPoint<Point>(problem.getStarts()[i], "s" + i));
242
                }
243
                return list;
244
            }
245

  
246
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.BLUE));
247

  
248
        VisManager.registerLayer(LabeledPointLayer.create(new LabeledPointsProvider<Point>() {
249

  
250
            @Override
251
            public Collection<LabeledPoint<Point>> getLabeledPoints() {
252
                LinkedList<LabeledPoint<Point>> list = new LinkedList<LabeledPoint<Point>>();
253

  
254
                for (int i = 0; i < problem.getTargets().length; i++) {
255
                    list.add(new LabeledPoint<Point>(problem.getTarget(i), "g" + i));
256
                }
257
                return list;
258
            }
259

  
260
        }, new tt.euclid2i.vis.ProjectionTo2d(), Color.RED));
261
    }
262

  
263
    public static void main(String[] args) {
264
        new SuperconflictDemo().create();
265
    }
266

  
267
}
src/main/java/tt/jointtraj/solver/AStarGraphOptimizer.java
1
package tt.jointtraj.solver;
2

  
3
import org.jgrapht.DirectedGraph;
4
import org.jgrapht.Graph;
5
import org.jgrapht.GraphPath;
6
import org.jgrapht.alg.AStarShortestPathSimple;
7
import org.jgrapht.util.Goal;
8
import org.jgrapht.util.HeuristicToGoal;
9

  
10
import tt.euclid2i.EvaluatedTrajectory;
11
import tt.euclid2i.Line;
12
import tt.euclid2i.Trajectory;
13
import tt.euclid2i.trajectory.StraightSegmentTrajectory;
14
import tt.euclidtime3i.Point;
15
import tt.euclidtime3i.discretization.ConstantSpeedTimeExtension;
16
import tt.euclidtime3i.discretization.Straight;
17
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
18
import tt.euclidtime3i.discretization.softconstraints.PairwiseSoftConstraintWrapper;
19

  
20
public class AStarGraphOptimizer implements TrajectoryConstraintOptimizer {
21

  
22
    final tt.euclidtime3i.Point start;
23
    final tt.euclid2i.Point target;
24
    final DirectedGraph<tt.euclidtime3i.Point, Straight> agentMotions;
25

  
26
    final int vmax;
27
    int[] separations;
28
    final int maxTime;
29

  
30
    public AStarGraphOptimizer(
31
            DirectedGraph<tt.euclid2i.Point, Line> spatialGraph,
32
            Point start,
33
            tt.euclid2i.Point target,
34
            int speed,
35
            int[] separations,
36
            int maxTime) {
37

  
38
        super();
39
        this.start = start;
40
        this.target = target;
41
        this.vmax = speed;
42
        this.separations = separations;
43
        this.agentMotions = new ConstantSpeedTimeExtension(spatialGraph, maxTime, new int[]{vmax});
44
        this.maxTime = maxTime;
45
    }
46

  
47
    @Override
48
    public EvaluatedTrajectory getOptimalTrajectory(
49
            Trajectory[] otherTrajectories,
50
            final PairwiseConstraint constraint,
51
            final double weight) {
52

  
53
        Graph<tt.euclidtime3i.Point, Straight> graphWithConstraints
54
                = new PairwiseSoftConstraintWrapper<tt.euclidtime3i.Point, Straight>(agentMotions, otherTrajectories, separations, constraint, weight);
55

  
56
        GraphPath<tt.euclidtime3i.Point, Straight> path = AStarShortestPathSimple.findPathBetween(graphWithConstraints,
57
                new HeuristicToGoal<tt.euclidtime3i.Point>() {
58
                    @Override
59
                    public double getCostToGoalEstimate(
60
                            tt.euclidtime3i.Point current) {
61
                        return (current.getPosition().distance(target)) / vmax;
62
                    }
63
                }, start, new Goal<tt.euclidtime3i.Point>() {
64
                    @Override
65
                    public boolean isGoal(tt.euclidtime3i.Point current) {
66
                        return current.getPosition().equals(target);
67
                    }
68
                }
69
        );
70

  
71
        if (path == null) {
72
            throw new RuntimeException();
73
        }
74

  
75
        EvaluatedTrajectory newTraj = new StraightSegmentTrajectory(path, maxTime);
76

  
77
        return newTraj;
78
    }
79

  
80
}
src/main/java/tt/jointtraj/solver/IterativeSolver.java
1
package tt.jointtraj.solver;
2

  
3
import java.util.LinkedList;
4

  
5
import tt.euclid2i.EvaluatedTrajectory;
6
import tt.euclid2i.Trajectory;
7
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
8

  
9
public class IterativeSolver {
10

  
11
    private TrajectoryConstraintOptimizer[] optimizers;
12
    private PairwiseConstraint constraint;
13

  
14
    private EvaluatedTrajectory[] trajs;
15
    private int j=0;
16

  
17
    public IterativeSolver(TrajectoryConstraintOptimizer[] optimizers,
18
            PairwiseConstraint constraint) {
19
        super();
20
        this.optimizers = optimizers;
21
        this.constraint = constraint;
22
        this.trajs = new EvaluatedTrajectory[nAgents()];
23
    }
24

  
25
    public void iterate(double weight) {
26
            // replan for each agent
27

  
28
            LinkedList<Trajectory> trajList = new LinkedList<Trajectory>();
29
            for (int k=0; k<nAgents(); k++) {
30
                if (trajs[k] != null && k != j) {
31
                    trajList.add(trajs[k]);
32
                }
33
            }
34

  
35
            trajs[j] = optimizers[j].getOptimalTrajectory(trajList.toArray(new Trajectory[] {}), constraint, weight);
36

  
37
            j++;
38

  
39
            if (j == nAgents()) {
40
                j=0;
41
            }
42
    }
43

  
44
    private int nAgents() {
45
        return optimizers.length;
46
    }
47

  
48
    public EvaluatedTrajectory[] getTrajs() {
49
        return trajs;
50
    }
51

  
52
    public double getCost() {
53
        double cost = 0;
54
        for (int i=0; i<trajs.length; i++) {
55
            if (trajs[i] != null) {
56
                cost += trajs[i].getCost();
57
            } else {
58
                return Double.POSITIVE_INFINITY;
59
            }
60
        }
61
        return cost;
62
    }
63

  
64
}
src/main/java/tt/jointtraj/solver/TrajectoryConstraintOptimizer.java
1
package tt.jointtraj.solver;
2

  
3
import tt.euclid2i.EvaluatedTrajectory;
4
import tt.euclid2i.Trajectory;
5
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
6

  
7
public interface TrajectoryConstraintOptimizer {
8
    EvaluatedTrajectory getOptimalTrajectory(
9
            Trajectory[] otherTrajectories,
10
            PairwiseConstraint constraint,
11
            double weight);
12
}
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/main/resources/problems/dejvice.xml -timeout 5000 -maxtime 2500 -gridstep 25 -showvis -summary"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-method KSFO -problemfile src/main/resources/problems/cross_conflict.xml -timeout 500000 -maxtime 1000 -gridstep 25 -showvis -summary -grid 4"/>
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