Revision 61:f02481c7eebc

View differences:

misc/eclipse/GenerateInstance.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.probleminstance.generator.GenerateInstance"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-env src/main/resources/environments/empty-unbounded.xml -nagents 24 -radius 90 -gridpattern 4  -gridstep 100 -seed 1000"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-env src/main/resources/environments/empty-unbounded.xml -nagents  13 -radius 98 -gridpattern 4  -gridstep 200 -seed 1000"/>
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>
misc/eclipse/Solver (kDPM-C).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 KDPMC -k 50 -problemfile  src/main/resources/problems/99.xml  -maxtime 2500 -gridstep 50 -summary -showvis -verbose"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-method KDPMC -k 10 -problemfile  src/main/resources/problems/99.xml  -maxtime 2500 -gridstep 50 -summary -showvis -verbose -objective TRAJECTORY"/>
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>
misc/eclipse/Solver (test).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 KDPMC -k 1 -problemfile src/test/resources/problems/cross_conflict.xml -timeout 2000 -maxtime 2500 -gridstep 50 -summary -showvis"/>
11
<stringAttribute key="org.eclipse.jdt.launching.PROGRAM_ARGUMENTS" value="-method KDPMD -k 500 -problemfile src/main/resources/problems/empty-unbounded-r98-n13.xml -maxtime 2500 -gridstep 50 -summary -showvis"/>
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>
src/main/java/tt/jointeuclid2ni/Solver.java
8 8
import tt.jointeuclid2ni.solver.Algorithm;
9 9
import tt.jointeuclid2ni.solver.Algorithms;
10 10
import tt.jointeuclid2ni.solver.HeuristicType;
11
import tt.jointeuclid2ni.solver.ObjectiveType;
11 12
import tt.jointeuclid2ni.solver.Parameters;
12 13
import tt.jointeuclid2ni.solver.impl.*;
13 14
import tt.jointtraj.solver.SearchResult;
......
63 64
        String heuristicStr = Args.getArgumentValue(args, "-heuristic", false, "PERFECT");
64 65
        params.heuristic = HeuristicType.valueOf(heuristicStr);
65 66

  
67
        String objectiveStr = Args.getArgumentValue(args, "-objective", false, "ARRIVAL");
68
        params.objective = ObjectiveType.valueOf(objectiveStr);
69

  
66 70
        params.showVis = Args.isArgumentSet(args, "-showvis");
67 71

  
68 72
        params.printSummary = Args.isArgumentSet(args, "-summary");
......
118 122
        throw new RuntimeException("Unsupported grid pattern:" + gridPatternStr);
119 123
    }
120 124

  
121
    private void printHelp() {
125
    static public void printHelp() {
122 126
        System.out.println("Name");
123 127
        System.out.println("      solver.jar -- the multi-robot deconfliction problem solver");
124 128
        System.out.println();
......
130 134
        System.out.println("Options:");
131 135
        System.out.println();
132 136
        System.out.println("  -method: the name of method to be used for solving, currently supported:");
133
        System.out.println("  	'PP' = prioritized planning, 'ODCN' = operator decomposition, IIHP = 'incremental iterative homotopy planner'");
137
        System.out.println("  	'PP' = prioritized planning, 'ODCN' = operator decomposition, 'IIHP' = incremental iterative homotopy planner");
138
        System.out.println("  	'KDPMD' = k-step distributed penalty method with discrete trajectory optimizers, ");
139
        System.out.println("  	'KDPMC' = k-step distributed penalty method with continuous trajectory optimizers ");
134 140
        System.out.println();
135 141
        System.out.println("  -problemfile: a path to an XML file containing description fo the environment and and the mission");
136 142
        System.out.println();
src/main/java/tt/jointeuclid2ni/probleminstance/generator/ConflictGenerator.java
1 1
package tt.jointeuclid2ni.probleminstance.generator;
2 2

  
3
import java.awt.Color;
3 4
import java.util.ArrayList;
4 5
import java.util.List;
5 6
import java.util.Random;
6 7
import java.util.Set;
7 8

  
8 9
import org.jgrapht.DirectedGraph;
10
import org.jgrapht.Graph;
9 11
import org.jgrapht.GraphPath;
10 12
import org.jgrapht.alg.AStarShortestPathSimple;
11 13

  
14
import cz.agents.alite.vis.Vis;
15
import cz.agents.alite.vis.VisManager;
16
import cz.agents.alite.vis.layer.common.ColorLayer;
17

  
12 18
import tt.euclid2i.HeuristicToPoint;
13 19
import tt.euclid2i.Line;
14 20
import tt.euclid2i.Point;
......
25 31
import tt.jointeuclid2ni.probleminstance.generator.exception.ProblemNotCreatedException;
26 32
import tt.jointtraj.util.Discretization;
27 33
import tt.util.Verbose;
34
import tt.vis.GraphLayer;
35
import tt.vis.ProjectionTo2d;
36
import tt.vis.GraphLayer.GraphProvider;
28 37
import tt.vis.problemcreator.main.ExtensibleProblem;
29 38

  
30 39
public class ConflictGenerator {
31 40

  
32 41
    private static final int MAX_SPEED = 1;
33
    private static final int MAX_ATTEMTPS_PROBLEM = 50;
42
    private static final int MAX_ATTEMTPS_PROBLEM = 500;
34 43
    private static final int MAX_ATTEMPTS_MISSION = 150;
35
    private static final int MAX_ATTEMPTS_START = 2500;
36
    private static final int MAX_ATTEMPTS_TARGET = 2500;
44
    private static final int MAX_ATTEMPTS_START = 5000;
45
    private static final int MAX_ATTEMPTS_TARGET = 5000;
37 46

  
38 47
    private List<ProblemCreatedListener> listeners;
39 48

  
......
58 67
    }
59 68

  
60 69
    private DirectedGraph<Point, Line>[] createGraphs(Environment environment, int[] bodyRadiuses, int[][] gridPattern, int step) {
61
        DirectedGraph[] directedGraphs = new DirectedGraph[agents];
70
        final DirectedGraph<Point, Line>[] directedGraphs = new DirectedGraph[agents];
71

  
72
        VisManager.setInitParam("", 100, 100);
73
        VisManager.init();
74
        VisManager.registerLayer(ColorLayer.create(Color.WHITE));
75

  
62 76
        for (int a = 0; a < agents; a++) {
63 77
            directedGraphs[a] = Discretization.createGrid(environment, bodyRadiuses[a], gridPattern, step);
78

  
79

  
80
            final DirectedGraph<Point, Line> graph = directedGraphs[a];
81
            VisManager.registerLayer(GraphLayer.create(new GraphProvider() {
82
				@Override
83
				public Graph getGraph() {
84
					return graph;
85
				}
86
			}, new tt.euclid2i.vis.ProjectionTo2d(), Color.BLACK, Color.BLACK, 1, 4));
87

  
64 88
        }
65 89
        return directedGraphs;
66 90
    }
src/main/java/tt/jointeuclid2ni/solver/ObjectiveType.java
1
package tt.jointeuclid2ni.solver;
2

  
3
public enum ObjectiveType {
4
	TRAJECTORY,
5
	ARRIVAL
6
}
src/main/java/tt/jointeuclid2ni/solver/Parameters.java
18 18
    public int waitMoveDuration;
19 19
    public int[][] gridPattern = LazyGrid.PATTERN_4_WAY;
20 20
    public HeuristicType heuristic;
21
    public ObjectiveType objective;
21 22
}
src/main/java/tt/jointeuclid2ni/solver/impl/AbstractDPMBasedAlgorithm.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.ConstantSeparationPenaltyFunction;
15
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
16
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
17
import tt.euclidtime3i.discretization.softconstraints.SeparationConstraint;
18
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
19
import tt.jointtraj.separableflow.AStarTrajectoryOptimizer;
20
import tt.jointtraj.separableflow.TrajectoryOptimizer;
21
import tt.jointtraj.solver.SearchResult;
22
import tt.util.NotImplementedException;
23
import tt.util.Verbose;
24

  
25

  
26
public abstract class AbstractDPMBasedAlgorithm extends AbstractAlgorithm {
27

  
28
    private static final double MAX_PENALTY = 1;
29

  
30
    public AbstractDPMBasedAlgorithm() {
31
        this(new String[0]);
32
    }
33

  
34
    public AbstractDPMBasedAlgorithm(String[] args) {
35
        super();
36
    }
37

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

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

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

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

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

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

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

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

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

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

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

  
96
        return new SearchResult(trajs, true);
97

  
98
    }
99

  
100
    abstract protected EvaluatedTrajectory[] solveCore(
101
            TrajectoryOptimizer[] trajectoryOptimizers,
102
            PenaltyFunction[][] penaltyFunctions);
103
}
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/AlgorithmDPMC.java
1 1
package tt.jointeuclid2ni.solver.impl;
2 2

  
3 3

  
4
import java.awt.Color;
5
import java.util.Arrays;
6
import java.util.Collection;
7
import java.util.Collections;
8

  
9
import org.jgrapht.GraphPath;
10
import org.jgrapht.WeightedGraph;
11
import org.jgrapht.alg.AStarShortestPathSimple;
12
import org.jgrapht.util.HeuristicToGoal;
13

  
14
import cz.agents.alite.vis.VisManager;
15

  
16
import tt.discrete.util.Trajectories;
17
import tt.euclid2d.trajectory.DesiredTrajectoryDifferenceObjective;
18
import tt.euclid2d.trajectory.GoalArrivalObjective;
19
import tt.euclid2d.trajectory.SoftGoalArrivalObjective;
20
import tt.euclid2d.trajectory.TrajectoryObjectiveFunctionAtPoint;
4 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.VisibilityGraph;
27
import tt.euclid2i.trajectory.LineSegmentsConstantSpeedTrajectory;
28
import tt.euclid2i.util.Util;
5 29
import tt.euclidtime3i.discretization.softconstraints.BumpSeparationPenaltyFunction;
30
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
6 31
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
32
import tt.jointeuclid2ni.solver.ObjectiveType;
7 33
import tt.jointtraj.separableflow.GradientOptimizer;
8 34
import tt.jointtraj.separableflow.SeparableFlowOptimizer;
9 35
import tt.jointtraj.separableflow.TrajectoryOptimizer;
10 36
import tt.jointtraj.solver.SearchResult;
37
import tt.util.AgentColors;
11 38
import tt.util.Args;
12 39
import tt.util.Verbose;
40
import tt.vis.FastTrajectoriesLayer;
41
import tt.vis.FastTrajectoriesLayer.ColorProvider;
42
import tt.vis.FastTrajectoriesLayer.TrajectoriesProvider;
13 43

  
14 44
public class AlgorithmDPMC extends AbstractAlgorithm {
15 45

  
16 46
    private static final double MAX_PENALTY = 1;
17
    private static final int DESCENT_MAX_ITER = 200;
18
    private static final double DESCENT_STEP = 3;
47
    private static final int DESCENT_MAX_ITER = 100;
48
    private static final double DESCENT_STEP = 1;
19 49
    private static final double COST_EPS = 0.01;
20 50

  
21

  
22 51
    private int k = 50;
23 52

  
24 53
    public AlgorithmDPMC(String[] args) {
......
29 58
    public SearchResult solveProblem() {
30 59
        Verbose.setVerbose(params.verbose);
31 60

  
61
        final Trajectory[] desiredTrajectories = findDesiredTrajectories(problem.getStarts(), problem.getTargets(), problem.getBodyRadiuses(), problem.getObstacles(),
62
        		params.maxSpeed, params.maxTime);
63

  
64
        VisManager.registerLayer(FastTrajectoriesLayer.create(new TrajectoriesProvider() {
65
			@Override
66
			public Trajectory[] getTrajectories() {
67
				return desiredTrajectories;
68
			}
69
		}, new ColorProvider() {
70

  
71
			@Override
72
			public Color getColor(int i) {
73
				return AgentColors.getColorForAgent(i).brighter().brighter();
74
			}
75
		}, 1, 50));
76

  
77

  
32 78
        // Create trajectory optimizers
33 79
        TrajectoryOptimizer[] trajectoryOptimizers = new TrajectoryOptimizer[problem.nAgents()];
34 80
        for (int i = 0; i < problem.nAgents(); i++) {
35
            trajectoryOptimizers[i] = new GradientOptimizer(problem.getStart(i), problem.getTarget(i), problem.getMaxSpeeds()[i], params.maxTime,
81

  
82
        	TrajectoryObjectiveFunctionAtPoint objective = null;
83
        	if (params.objective == ObjectiveType.TRAJECTORY) {
84
        		objective = new DesiredTrajectoryDifferenceObjective(desiredTrajectories[i], 0.01 * 1/params.gridStep);
85
        	}else if (params.objective == ObjectiveType.ARRIVAL) {
86
        		objective = new SoftGoalArrivalObjective(problem.getTarget(i), params.gridStep/2, 0);
87
        	} else {
88
        		assert false;
89
        	}
90

  
91
            trajectoryOptimizers[i] = new GradientOptimizer(problem.getStart(i), problem.getTarget(i), objective, problem.getMaxSpeeds()[i], params.maxTime,
36 92
            		params.gridStep, DESCENT_STEP, COST_EPS, DESCENT_MAX_ITER, false);
37 93
        }
38 94

  
......
41 97
        for (int i = 0; i < problem.nAgents(); i++) {
42 98
            for (int j = 0; j < problem.nAgents(); j++) {
43 99
                if (i != j) {
44
                    softSeparationFunctions[i][j] = new BumpSeparationPenaltyFunction(MAX_PENALTY, problem.getBodyRadius(i) + problem.getBodyRadius(j));
100
                    softSeparationFunctions[i][j] = new LinearSeparationPenaltyFunction(MAX_PENALTY, problem.getBodyRadius(i) + problem.getBodyRadius(j));
45 101
                }
46 102
            }
47 103
        }
48 104

  
49 105
        EvaluatedTrajectory[] trajs = SeparableFlowOptimizer.solve(
50
                trajectoryOptimizers, softSeparationFunctions, k, 1000000,
106
                trajectoryOptimizers, softSeparationFunctions, k, Double.POSITIVE_INFINITY,
51 107
                params.runtimeDeadlineMs, params.verbose);
52 108

  
53 109
        return new SearchResult(trajs, true);
54 110
    }
55 111

  
112
	private Trajectory[] findDesiredTrajectories(Point[] starts,
113
			Point[] targets, int[] bodyRadiuses, Collection<Region> obstacles, int speed, int maxtime) {
114

  
115
		Trajectory[] optimalTrajs = new Trajectory[starts.length];
116
		for (int i=0; i<starts.length; i++) {
117
			optimalTrajs[i] = findShortestPath(starts[i], targets[i], bodyRadiuses[i], obstacles, speed, maxtime);
118
		}
119

  
120
		return optimalTrajs;
121
	}
122

  
123
	private Trajectory findShortestPath(Point start, final Point target, int radius,
124
			Collection<Region> obstacles, int speed, int maxTime) {
125

  
126
		Collection<Point> significantPoints = Arrays.asList(new Point[] {start, target});
127
		WeightedGraph<Point, Line> visGraph = VisibilityGraph.createVisibilityGraph(obstacles, radius, significantPoints);
128

  
129
		GraphPath<Point, Line> path = AStarShortestPathSimple.findPathBetween(visGraph, new HeuristicToGoal<Point>() {
130

  
131
			@Override
132
			public double getCostToGoalEstimate(Point current) {
133
				return current.distance(target);
134
			}
135
		}, start, target);
136

  
137
		if (path != null) {
138
			Trajectory traj = new LineSegmentsConstantSpeedTrajectory<Point, Line>(0, path, speed, maxTime);
139
			return traj;
140
		} else {
141
			throw new RuntimeException("No unconstrained trajectory found on the visibility graph! ");
142
		}
143

  
144

  
145
	}
146

  
56 147

  
57 148

  
58 149

  
src/main/java/tt/jointeuclid2ni/solver/impl/AlgorithmDPMD.java
9 9
import tt.util.Args;
10 10

  
11 11

  
12
public class AlgorithmDPMD extends AbstractDPMasedAlgorithm {
12
public class AlgorithmDPMD extends AbstractDPMBasedAlgorithm {
13 13

  
14 14
    private int k = 50;
15 15

  
......
28 28

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

  
33 33
        return trajs;
34 34
    }
src/main/java/tt/jointeuclid2ni/solver/impl/AlgorithmIIHP.java
75 75
        for (int i = 0; i < problem.nAgents(); i++) {
76 76
        	for (int j = 0; j < problem.nAgents(); j++) {
77 77
        		if (i != j) {
78
        			constraints[i][j] = new SeparationConstraint(new BumpSeparationPenaltyFunction(IIHP_MAX_PENALTY, problem.getBodyRadius(i)+problem.getBodyRadius(j)),
78
        			constraints[i][j] = new SeparationConstraint(new BumpSeparationPenaltyFunction(IIHP_MAX_PENALTY, problem.getBodyRadius(i)+problem.getBodyRadius(j), 1.0),
79 79
        					10);
80 80
        		}
81 81

  
src/main/java/tt/jointtraj/homotopyiterative/AStarTrajectoryOptimizerWithHomotopyConstriants.java
81 81
    	HomotopyWeightedTimeGraph homotopyGraph = new HomotopyWeightedTimeGraph(spatialGraph, target.getPosition(), integrator, penalty, vmax, target.getTime(), waitMoveDuration);
82 82

  
83 83
        Graph<tt.euclidtime3i.Point, Straight> graphWithPenalty
84
        	= new PenaltyFunctionWrapper<tt.euclidtime3i.Point, Straight>(agentMotions, penalty);
84
        	= new PenaltyFunctionWrapper(agentMotions, penalty);
85 85

  
86 86

  
87 87
        HNode<tt.euclidtime3i.Point> startHNode = homotopyGraph.wrapNode(start, Complex.ZERO);
src/main/java/tt/jointtraj/separableflow/AStarTrajectoryOptimizer.java
74 74
        StraightSegmentPenaltyFunction penalty = new PairwiseConstraintStraightSegmentPenalty(constraints, otherTrajectories);
75 75

  
76 76
		Graph<tt.euclidtime3i.Point, Straight> graphWithPenalty
77
        	= new PenaltyFunctionWrapper<tt.euclidtime3i.Point, Straight>(agentMotions, penalty);
77
        	= new PenaltyFunctionWrapper(agentMotions, penalty);
78 78

  
79 79
		AStarShortestPathSimple<Point, Straight> alg
80 80
			= new AStarShortestPathSimple<Point, Straight>(graphWithPenalty, heuristic, start, new Goal<Point>() {
src/main/java/tt/jointtraj/separableflow/GradientOptimizer.java
6 6
import javax.vecmath.Tuple2d;
7 7
import javax.vecmath.Vector2d;
8 8

  
9
import tt.euclid2d.trajectory.DesiredTrajectoryDifferenceObjective;
10
import tt.euclid2d.trajectory.TrajectoryObjectiveFunctionAtPoint;
9 11
import tt.euclid2i.EvaluatedTrajectory;
10 12
import tt.euclid2i.Point;
11 13
import tt.euclid2i.Trajectory;
12 14
import tt.euclid2i.Vector;
15
import tt.euclid2i.trajectory.EvaluatedTrajectoryWrapper;
13 16
import tt.euclid2i.trajectory.LinearTrajectory;
14 17
import tt.euclid2i.trajectory.PointArrayTrajectory;
15 18
import tt.euclid2i.trajectory.StraightSegmentTrajectory;
......
30 33

  
31 34
	Point2d start;
32 35
	Point2d target;
33
	int speed;
36
	TrajectoryObjectiveFunctionAtPoint objective;
37
	int maxSpeed;
34 38
	int maxTime;
35 39
	int samplingInterval;
36 40
	double descentStep;
......
40 44
	boolean verbose;
41 45
	int maxIter;
42 46

  
43
	public GradientOptimizer(Point start, Point target, int speed, int maxTime,
47
	public GradientOptimizer(Point start, Point target, TrajectoryObjectiveFunctionAtPoint objective, int maxSpeed, int maxTime,
44 48
			int samplingInterval, double step, double costEps, int maxIter, boolean verbose) {
45 49
		super();
46 50
		this.start = new Point2d(start.x, start.y);
47 51
		this.target = new Point2d(target.x, target.y);
48
		this.speed = speed;
52
		this.objective = objective;
53
		this.maxSpeed = maxSpeed;
49 54
		this.maxTime = maxTime;
50 55
		this.samplingInterval = samplingInterval;
51 56
		this.descentStep = step;
......
59 64
	public EvaluatedTrajectory getOptimalTrajectoryUnconstrained(
60 65
			double maxAllowedCost, long runtimeDeadlineMs) {
61 66

  
62
		LinearTrajectory traj = new LinearTrajectory(0, new Point(
63
				(int) start.x, (int) start.y), new Point((int) target.x,
64
				(int) target.y), speed, maxTime, start.distance(target));
65
		return traj;
67
		if (objective instanceof DesiredTrajectoryDifferenceObjective) {
68
			Trajectory desiredTraj = ((DesiredTrajectoryDifferenceObjective)objective).getDesiredTrajectory();
69
			return new EvaluatedTrajectoryWrapper(desiredTraj, 0);
70
		} else {
71
			LinearTrajectory traj = new LinearTrajectory(0, new Point(
72
					(int) start.x, (int) start.y), new Point((int) target.x,
73
					(int) target.y), maxSpeed, maxTime, start.distance(target));
74
			return traj;
75
		}
66 76
	}
67 77

  
68 78
	@Override
......
72 82
			long runtimeDeadlineMs) {
73 83

  
74 84
		if (initialTrajectory == null)  {
75
			initialTrajectory = getOptimalTrajectoryUnconstrained(maxAllowedCost, runtimeDeadlineMs);
85
			if (objective instanceof DesiredTrajectoryDifferenceObjective) {
86
				initialTrajectory = ((DesiredTrajectoryDifferenceObjective)objective).getDesiredTrajectory();
87
			} else {
88
				initialTrajectory = getOptimalTrajectoryUnconstrained(maxAllowedCost, runtimeDeadlineMs);
89
			}
76 90
		}
77 91

  
78 92
		// Convert initial trajectory to the trajectory vector
......
85 99
		traj = simpleGradientDescent(traj, penaltyFunctions, otherTrajectories);
86 100
		//traj = conjugateGradientDescent(traj, penaltyFunctions, otherTrajectories);
87 101

  
88
		double goalArrivaCost = computeGoalArrivalPenalties(traj, target, samplingInterval);
89
		EvaluatedTrajectory eTraj = toTrajectory(traj, goalArrivaCost);
102
		double objectiveValue = computeObjective(objective, traj, samplingInterval);
103
		EvaluatedTrajectory eTraj = toTrajectory(traj, objectiveValue);
90 104
		this.currentTraj = eTraj;
91 105
		return eTraj;
92 106
	}
......
98 112

  
99 113
			@Override
100 114
			public double value(double[] point) {
101
				double cost = computeTrajectoryCost(doubleArrayToPointArray(point), target, speed, penaltyFunctions, otherTrajectories, samplingInterval);
115
				double cost = computeTrajectoryCost(doubleArrayToPointArray(point), objective, maxSpeed, penaltyFunctions, otherTrajectories, samplingInterval);
102 116
				Verbose.println("Asking for value at " + GradientOptimizer.toString(doubleArrayToPointArray(point)));
103 117
				return cost;
104 118
			}
......
113 127

  
114 128
				final double h = 0.0001;
115 129
				Vector2d[] gradient = computeGradient(
116
						doubleArrayToPointArray(point), Double.NaN,  penaltyFunctions,
117
						otherTrajectories, h, target,
118
						speed,
130
						doubleArrayToPointArray(point), objective, Double.NaN,  penaltyFunctions,
131
						otherTrajectories, h,
132
						maxSpeed,
119 133
						samplingInterval);
120 134

  
121 135
				//Verbose.print("Returning gradient: " +  GradientOptimizer.toString(gradient) + "\n");
......
149 163

  
150 164
	private Point2d[] simpleGradientDescent(Point2d[] traj,
151 165
			PenaltyFunction[] penaltyFunctions, Trajectory[] otherTrajectories) {
152
		double cost = computeTrajectoryCost(traj, target, speed,
166
		double cost = computeTrajectoryCost(traj, objective, maxSpeed,
153 167
				penaltyFunctions, otherTrajectories, samplingInterval);
154 168

  
155 169
		if (verbose) {
156
			printPenalties(0, traj, target, speed, penaltyFunctions,
170
			printPenalties(0, traj, objective, maxSpeed, penaltyFunctions,
157 171
					otherTrajectories, samplingInterval);
158 172
		}
159 173

  
......
165 179

  
166 180

  
167 181
			// compute gradient
168
			Vector2d[] gradient = computeGradient(traj, cost, penaltyFunctions,
169
					otherTrajectories, H, target, speed, samplingInterval);
182
			Vector2d[] gradient = computeGradient(traj, objective, cost, penaltyFunctions,
183
					otherTrajectories, H, maxSpeed, samplingInterval);
170 184

  
171 185
			if (verbose) {
172
				System.out.printf("Iter: %d GradNorm: %.4f Grad: %s \n", i, norm(gradient), toString(gradient));
186
				System.out.printf("\t\tGradNorm: %.4f Grad: %s \n", norm(gradient), toString(gradient));
173 187
			}
174 188

  
175 189
			normalize(gradient);
......
180 194
			currentTraj = toTrajectory(traj, cost);
181 195

  
182 196
			double oldCost = cost;
183
			cost = computeTrajectoryCost(traj, target, speed, penaltyFunctions,
197
			cost = computeTrajectoryCost(traj, objective, maxSpeed, penaltyFunctions,
184 198
					otherTrajectories, samplingInterval);
185 199

  
186 200
			if (verbose) {
187
				printPenalties(i, traj, target, speed, penaltyFunctions,
201
				printPenalties(i, traj, objective, maxSpeed, penaltyFunctions,
188 202
						otherTrajectories, samplingInterval);
189 203
			}
190 204

  
......
193 207

  
194 208
			// System.out.println("Current traj:" + toString(traj));
195 209
			if (verbose) {
196
				System.out.printf("Cost: %.2f CostDiff: %.8f \n", cost, costDiff);
210
				System.out.printf("\t\tCurrent Cost: %.2f CostDiff: %.8f \n", cost, costDiff);
197 211
			}
198 212

  
199 213
			if (Math.abs(costDiff) < costEps || i > maxIter) {
......
240 254
		return traj;
241 255
	}
242 256

  
243
	private static Vector2d[] computeGradient(Point2d[] traj,
257
	private static Vector2d[] computeGradient(Point2d[] traj, TrajectoryObjectiveFunctionAtPoint objective,
244 258
			double originalCost, PenaltyFunction[] penaltyFunctions,
245
			Trajectory[] otherTrajectories, double h, Point2d target,
259
			Trajectory[] otherTrajectories, double h,
246 260
			int speed, int samplingInterval) {
247 261
		Vector2d[] gradient = new Vector2d[traj.length];
248 262

  
249 263
		if (Double.isNaN(originalCost)) {
250
			originalCost = computeTrajectoryCost(traj, target, speed, penaltyFunctions, otherTrajectories, samplingInterval);
264
			originalCost = computeTrajectoryCost(traj, objective, speed, penaltyFunctions, otherTrajectories, samplingInterval);
251 265
		}
252 266

  
253 267
		if (Double.isInfinite(originalCost)) {
......
264 278
				// compute gradient at X
265 279
				Point2d[] newTraj = Arrays.copyOf(traj, traj.length);
266 280
				newTraj[i] = new Point2d(traj[i].x + h, traj[i].y);
267
				double newCost = computeTrajectoryCost(newTraj, target, speed,
281
				double newCost = computeTrajectoryCost(newTraj, objective, speed,
268 282
						penaltyFunctions, otherTrajectories, samplingInterval);
269 283
				double iPosGradientX = (newCost - originalCost) / (double) h;
270 284
				gradient[i].x = iPosGradientX;
......
274 288
				// compute gradient at Y
275 289
				Point2d[] newTraj = Arrays.copyOf(traj, traj.length);
276 290
				newTraj[i] = new Point2d(traj[i].x, traj[i].y + h);
277
				double newCost = computeTrajectoryCost(newTraj, target, speed,
291
				double newCost = computeTrajectoryCost(newTraj, objective, speed,
278 292
						penaltyFunctions, otherTrajectories, samplingInterval);
279 293
				double iPosGradientY = (newCost - originalCost) / (double) h;
280 294
				gradient[i].y = iPosGradientY;
......
296 310
		return new PointArrayTrajectory(traj, samplingInterval, cost);
297 311
	}
298 312

  
299
	static double computeTrajectoryCost(Point2d[] traj, Point2d goal,
313
	static double computeTrajectoryCost(Point2d[] traj, TrajectoryObjectiveFunctionAtPoint objectiveFunction,
300 314
			int maxSpeed, PenaltyFunction[] penaltyFunctions,
301 315
			Trajectory[] otherTrajectories, int samplingInterval) {
302 316
		double cost = 0;
303
		cost += computeGoalArrivalPenalties(traj, goal, samplingInterval);
317
		cost += computeObjective(objectiveFunction, traj, samplingInterval);
304 318
		cost += computeMaxSpeedPenalties(traj, maxSpeed, samplingInterval);
305 319
		cost += computeSeparationPenalties(traj, penaltyFunctions,
306 320
				otherTrajectories, samplingInterval);
307 321
		return cost;
308 322
	}
309 323

  
310
	static void printPenalties(int i, Point2d[] traj, Point2d goal,
324
	static void printPenalties(int iter, Point2d[] traj, TrajectoryObjectiveFunctionAtPoint objectiveFunction,
311 325
			int maxSpeed, PenaltyFunction[] penaltyFunctions,
312 326
			Trajectory[] otherTrajectories, int samplingInterval) {
313 327

  
314
		double goalArrival = computeGoalArrivalPenalties(traj, goal, samplingInterval);
328
		double objectiveValue = computeObjective(objectiveFunction, traj, samplingInterval);
315 329
		double maxSpeedPenalty = computeMaxSpeedPenalties(traj, maxSpeed, samplingInterval);
316 330
		double separation = computeSeparationPenalties(traj, penaltyFunctions,
317 331
				otherTrajectories, samplingInterval);
318 332

  
319
		double total = goalArrival + maxSpeed + separation;
333
		double total = objectiveValue + maxSpeed + separation;
320 334

  
321
		System.out.printf("Iter: %d Goal arrival: %.4f, Max speed: %.4f, Separation: %.4f. Total: %.4f \n", i, goalArrival, maxSpeedPenalty, separation, total);
335
		System.out.printf("Iter: %d objectiveValue: %.4f, Max speed: %.4f, Separation: %.4f. Total: %.4f \n", iter, objectiveValue, maxSpeedPenalty, separation, total);
322 336
	}
323 337

  
324 338
	static double computeSeparationPenalties(Point2d[] traj,
......
347 361
		return cost;
348 362
	}
349 363

  
350
	static double computeGoalArrivalPenalties(Point2d[] traj, Point2d goal,
351
			int samplingInterval) {
364
	static double computeObjective(TrajectoryObjectiveFunctionAtPoint objective, Point2d[] traj, int samplingInterval) {
352 365
		double cost = 0;
353
		for (int i = 0; i < traj.length; i++) {
354
			double penalty = softEquality(goal.distance(traj[i]), samplingInterval/2, 0);
355
			cost += samplingInterval * penalty;
366
		for (int t = 0; t < traj.length; t++) {
367
			double value = objective.getCost(new tt.euclid2d.Point(traj[t].x, traj[t].y), t*samplingInterval);
368
			cost += samplingInterval * value;
356 369
		}
357 370
		return cost;
358 371
	}
......
370 383
		return Math.pow(base, Math.abs(x) - dmax) - Math.pow(base, -dmax);
371 384
	}
372 385

  
373
	static double softEquality(double x, double eps, double slope) {
374
		return (slope * Math.abs(x)) + 1 - bump(x, eps, 1, 1);
375
	}
376

  
377
	static double bump(double x, double dmax, double pmax, double s) {
378
		if (Math.abs(x) < dmax) {
379
			return (pmax / Math.exp(-s))
380
					* Math.exp((-s) / (1 - Math.pow(x / dmax, 2)));
381
		} else {
382
			return 0;
383
		}
384
	}
385

  
386 386
	public EvaluatedTrajectory getCurrentTraj() {
387 387
		return currentTraj;
388 388
	}
src/main/java/tt/jointtraj/separableflow/GradientOptimizerDemo.java
4 4

  
5 5
import javax.vecmath.Point2d;
6 6

  
7
import tt.euclid2d.trajectory.DesiredTrajectoryDifferenceObjective;
8
import tt.euclid2d.trajectory.SoftGoalArrivalObjective;
9
import tt.euclid2d.trajectory.TrajectoryObjectiveFunctionAtPoint;
7 10
import tt.euclid2i.EvaluatedTrajectory;
8 11
import tt.euclid2i.Point;
9 12
import tt.euclid2i.Trajectory;
13
import tt.euclid2i.trajectory.LinearTrajectory;
10 14
import tt.euclidtime3i.discretization.softconstraints.BumpSeparationPenaltyFunction;
15
import tt.euclidtime3i.discretization.softconstraints.ConstantSeparationPenaltyFunction;
11 16
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
12
import tt.euclidtime3i.trajectory.LinearTrajectory;
17
import tt.euclidtime3i.util.TrajectoryAsPathWrapper;
13 18
import tt.euclidtime3i.vis.TimeParameter;
14 19
import tt.util.AgentColors;
15 20
import tt.vis.FastAgentsLayer;
......
34 39
    public void create() {
35 40

  
36 41

  
37
		Point start = new Point(300, 1000);
38
		Point end = new Point(500, 0);
42
    	final Trajectory obstacleTraj = new LinearTrajectory(0,
43
    			new Point(500, 0),
44
    			new Point(500, 1000), 1, 1000, 0);
39 45

  
40
		final int radiusA = 100;
41
		final int radiusB = 100;
46
    	final int radiusObstacle = 100;
42 47

  
43
		final Trajectory obstacleTraj = new LinearTrajectory(
44
				new tt.euclidtime3i.Point(500, 0, 0),
45
				new tt.euclidtime3i.Point(500, 1000, 1000), 1000);
48
		Point start = new Point(499, 1000);
49
		Point target = new Point(500, 0);
46 50

  
51
		final int radiusAgent = 100;
52
		final int MAXTIME = 3000;
47 53
		final double COST_EPS = 0.001;
48 54
		final double STEP = 0.5;
49 55
		final int SAMPLING_INTERVAL = 50;
50
		final int MAXTIME = 3000;
51
		final int MAXITER = 3000;
56
		final int MAXITER = 5000;
52 57

  
53
		final int SPEED = 1;
58
		final Trajectory desiredTrajectory = new LinearTrajectory(0, start, target, 1, MAXTIME, 0);
54 59

  
55
		final GradientOptimizer optimizer = new GradientOptimizer(start, end,
56
				SPEED, MAXTIME, SAMPLING_INTERVAL, STEP, COST_EPS, MAXITER, true);
57 60

  
58
		EvaluatedTrajectory traj = optimizer.getOptimalTrajectoryUnconstrained(
59
				Double.MAX_VALUE, Long.MAX_VALUE);
61
		final int MAXSPEED = 2;
62

  
63
		//TrajectoryObjectiveFunctionAtPoint objective = new SoftGoalArrivalObjective(target, SAMPLING_INTERVAL/2, 0);
64

  
65
		TrajectoryObjectiveFunctionAtPoint objective = new DesiredTrajectoryDifferenceObjective(desiredTrajectory, 0.001*1.0/SAMPLING_INTERVAL );
66

  
67
		// continue here with desired traj objective
68

  
69

  
70
		final GradientOptimizer optimizer = new GradientOptimizer(start, target, objective,
71
				MAXSPEED, MAXTIME, SAMPLING_INTERVAL, STEP, COST_EPS, MAXITER, true);
72

  
73
//		EvaluatedTrajectory traj = optimizer.getOptimalTrajectoryUnconstrained(
74
//				Double.MAX_VALUE, Long.MAX_VALUE);
60 75

  
61 76
		final double pmax = 100;
62
		PenaltyFunction penaltyFunction = new BumpSeparationPenaltyFunction(
63
				pmax, radiusA + radiusB);
77
		PenaltyFunction penaltyFunction
78
			= new BumpSeparationPenaltyFunction(pmax, radiusAgent + radiusObstacle, 1.0);
79
        	//= new ConstantSeparationPenaltyFunction(pmax, radiusAgent + radiusObstacle);
64 80

  
65 81
		initVisualization();
66 82

  
......
71 87
					@Override
72 88
					public Trajectory[] getTrajectories() {
73 89
						return new Trajectory[] { obstacleTraj,
74
								optimizer.getCurrentTraj() };
90
								optimizer.getCurrentTraj(), desiredTrajectory};
75 91
					}
76 92
				}, new ColorProvider() {
77 93

  
......
89 105
					@Override
90 106
					public Trajectory[] getTrajectories() {
91 107
						return new Trajectory[] { obstacleTraj,
92
								optimizer.getCurrentTraj() };
108
								optimizer.getCurrentTraj(), desiredTrajectory };
93 109
					}
94 110

  
95 111
					@Override
96 112
					public int[] getBodyRadiuses() {
97
						return new int[] { radiusA, radiusB };
113
						return new int[] { radiusObstacle, radiusAgent, radiusAgent};
98 114
					}
99 115
				}, new FastAgentsLayer.ColorProvider() {
100 116

  
......
111 127
		final EvaluatedTrajectory bestResponse = optimizer
112 128
				.getOptimalTrajectoryConstrained(
113 129
						new PenaltyFunction[] { penaltyFunction },
114
						new Trajectory[] {obstacleTraj}, traj,
130
						new Trajectory[] {obstacleTraj}, desiredTrajectory,
115 131
						Double.MAX_VALUE, Long.MAX_VALUE);
116

  
117

  
118

  
119 132
    }
120 133

  
121 134
    private void initVisualization() {
src/main/java/tt/jointtraj/separableflow/PenaltyFunctionWrapper.java
1 1
package tt.jointtraj.separableflow;
2 2

  
3
import java.util.LinkedHashSet;
4
import java.util.Set;
5

  
3 6
import org.jgrapht.DirectedGraph;
4 7
import org.jgrapht.graph.GraphDelegator;
5 8

  
6
import tt.euclid2i.Trajectory;
7 9
import tt.euclidtime3i.Point;
8 10
import tt.euclidtime3i.discretization.Straight;
9 11

  
10 12
@SuppressWarnings("serial")
11
public class PenaltyFunctionWrapper<V extends Point, E extends Straight> extends GraphDelegator<V, E> {
13
public class PenaltyFunctionWrapper extends GraphDelegator<Point, Straight> implements DirectedGraph<Point, Straight>  {
12 14
    private StraightSegmentPenaltyFunction penaltyFunction;
13 15

  
14
	public PenaltyFunctionWrapper(DirectedGraph<V, E> g, StraightSegmentPenaltyFunction penaltyFunction) {
16
	public PenaltyFunctionWrapper(DirectedGraph<Point, Straight> g, StraightSegmentPenaltyFunction penaltyFunction) {
15 17
        super(g);
16 18
        this.penaltyFunction =  penaltyFunction;
17 19
    }
18 20

  
21
    @Override
22
    public Set<Straight> outgoingEdgesOf(Point vertex) {
23
        Set<Straight> allEdges = super.outgoingEdgesOf(vertex);
24
        Set<Straight> consistentEdges = new LinkedHashSet<Straight>();
25

  
26
        for (Straight edge : allEdges) {
27
            if (!Double.isInfinite(getEdgeWeight(edge))) {
28
                consistentEdges.add(edge);
29
            }
30
        }
31

  
32
        return consistentEdges;
33
    }
19 34

  
20 35
    @Override
21
    public double getEdgeWeight(E e) {
36
    public double getEdgeWeight(Straight e) {
22 37
        double cost = super.getEdgeWeight(e);
23 38
        return cost + penaltyFunction.getStraightTrajectoryCost(e.getStart(), e.getEnd());
24 39
    }
src/main/java/tt/jointtraj/separableflow/SeparableFlowOptimizer.java
1 1
package tt.jointtraj.separableflow;
2 2

  
3 3
import java.awt.Color;
4
import java.awt.image.SampleModel;
4 5
import java.util.Arrays;
5 6
import java.util.Iterator;
6 7

  
8
import org.jgrapht.Graph;
9

  
10
import com.vividsolutions.jts.index.bintree.Key;
11

  
7 12
import tt.euclid2i.EvaluatedTrajectory;
13
import tt.euclid2i.Line;
8 14
import tt.euclid2i.Trajectory;
15
import tt.euclidtime3i.discretization.softconstraints.BumpSeparationPenaltyFunction;
16
import tt.euclidtime3i.discretization.softconstraints.ConstantSeparationPenaltyFunction;
17
import tt.euclidtime3i.discretization.softconstraints.LinearSeparationPenaltyFunction;
9 18
import tt.euclidtime3i.discretization.softconstraints.PairwiseConstraint;
10 19
import tt.euclidtime3i.discretization.softconstraints.PenaltyFunction;
11 20
import tt.euclidtime3i.discretization.softconstraints.PenaltyIntegrator;
21
import tt.euclidtime3i.vis.TimeParameter;
12 22
import tt.util.AgentColors;
13 23
import tt.util.Verbose;
24
import tt.vis.FastAgentsLayer;
14 25
import tt.vis.FastTrajectoriesLayer;
26
import tt.vis.GraphLayer;
15 27
import tt.vis.FastTrajectoriesLayer.ColorProvider;
16 28
import tt.vis.FastTrajectoriesLayer.TrajectoriesProvider;
29
import tt.vis.ParameterControlLayer;
17 30
import cz.agents.alite.vis.VisManager;
31
import cz.agents.alite.vis.layer.toggle.KeyToggleLayer;
18 32

  
19 33
public class SeparableFlowOptimizer {
20 34

  
21
	private static final int TAN_COEF = 3;
35
	private static final int TAN_COEF = 5;
22 36
	private static boolean showProgress;
23 37
	protected static EvaluatedTrajectory[] currentTrajectories;
38
	protected static int[] bodyRadiuses;
24 39

  
25 40
	public static EvaluatedTrajectory[] solve(
26 41
			TrajectoryOptimizer[] trajectoryOptimizers,
......
33 48

  
34 49
		// Debug visualisation
35 50
		if (showProgress) {
51
			bodyRadiuses = new int[penaltyFunctions.length];
52
			// HACK: Assuming that everyone has the same radius and that we have at least two agents
53
			int radius = 1;
54
			if (penaltyFunctions[0][1] instanceof BumpSeparationPenaltyFunction) {
55
				 radius = (int) (((BumpSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
56
			} else if (penaltyFunctions[0][1] instanceof LinearSeparationPenaltyFunction) {
57
				radius = (int) (((LinearSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
58
			} else if (penaltyFunctions[0][1] instanceof ConstantSeparationPenaltyFunction) {
59
				radius = (int) (((ConstantSeparationPenaltyFunction)penaltyFunctions[0][1]).getMinSeparation()/2);
60
			}
61

  
62
			for (int i=0; i<trajectoryOptimizers.length; i++) {
63
				bodyRadiuses[i] = radius;
64
			}
36 65
			initProgressVisio();
37 66
		}
38 67

  
......
60 89
				return null;
61 90

  
62 91
			// Replan with hard-constraints
63
//			trajectories = replanWithInfiniteWeight(trajectories,
64
//					trajectoryOptimizers, penaltyFunctions, maxCost,
65
//					runtimeDeadlineMs);
92
			trajectories = replanWithInfiniteWeight(trajectories,
93
			trajectoryOptimizers, penaltyFunctions, maxCost,
94
			runtimeDeadlineMs);
66 95

  
67 96
			// !!! TODO check if the trajectories are conflict-free !!!
68 97
		}
......
71 100
	}
72 101

  
73 102
	private static void initProgressVisio() {
74
		VisManager.registerLayer(FastTrajectoriesLayer.create(
75
				new TrajectoriesProvider() {
76 103

  
77
					@Override
78
					public Trajectory[] getTrajectories() {
79
						return currentTrajectories;
80
					}
81
				}, new ColorProvider() {
104
		TimeParameter time = new TimeParameter(5);
105
		VisManager.registerLayer(ParameterControlLayer.create(time));
82 106

  
83
					@Override
84
					public Color getColor(int i) {
85
						return AgentColors.getColorForAgent(i);
86
					}
107
        KeyToggleLayer toggleLayer = KeyToggleLayer.create("p");
87 108

  
88
				}, 2, 5));
109
        toggleLayer.addSubLayer(FastTrajectoriesLayer.create(
110
        				new TrajectoriesProvider() {
111

  
112
        					@Override
113
        					public Trajectory[] getTrajectories() {
114
        						return currentTrajectories;
115
        					}
116
        				}, new ColorProvider() {
117

  
118
        					@Override
119
        					public Color getColor(int i) {
120
        						return AgentColors.getColorForAgent(i);
121
        					}
122

  
123
        				}, 2, 5));
124

  
125

  
126

  
127
        toggleLayer.addSubLayer(FastAgentsLayer.create(new FastAgentsLayer.TrajectoriesProvider() {
128

  
129
			@Override
130
			public Trajectory[] getTrajectories() {
131
				return currentTrajectories;
132
			}
133

  
134
			@Override
135
			public int[] getBodyRadiuses() {
136
				return bodyRadiuses;
137
			}
138
		}, new FastAgentsLayer.ColorProvider() {
139
			@Override
140
			public Color getColor(int i) {
141
				return AgentColors.getColorForAgent(i);
142
			}
143
		}, time));
144

  
145
        VisManager.registerLayer(toggleLayer);
89 146
	}
90 147

  
91 148
	private static EvaluatedTrajectory[] prioritizedPlanning(
......
130 187
			double maxCost, long runtimeDeadlineMs) {
131 188
		int nAgents = trajectoryOptimizers.length;
132 189

  
190
		double gcost = computeGlobalCost(trajectories);
191
		double gpenalty = computeGlobalPenalty(penaltyFunctions, trajectories);
192

  
133 193
		for (int i = 0; i < nIterations; i++) {
134 194
			int r = i % nAgents;
195

  
196

  
197

  
198

  
135 199
			double w = calculateSoftConstraintsWeight(i, nIterations);
200
			Verbose.printf("Agent: %d; k:%d/%d  w: %.4f. \n", r, i, nIterations, w);
136 201

  
137
			Verbose.printf("Soft constraints planning w=%.2f for agent %d. \n",
138
					w, r);
139 202

  
140 203
			Trajectory[] otherTrajectories = otherTrajectories(trajectories, r);
141 204
			PenaltyFunction[] penaltyFunctionsOfOtherTrajectories = constraintsOf(
142 205
					penaltyFunctions, r);
206

  
143 207
			PenaltyFunction[] penaltyFunctionsToOtherTrajectoriesWeighted = applyWeight(
144 208
					penaltyFunctionsOfOtherTrajectories, w);
145 209

  
......
158 222
			} else {
159 223
				return null;
160 224
			}
225

  
226
			double newgcost = computeGlobalCost(trajectories);
227
			double newgpenalty = computeGlobalPenalty(penaltyFunctions, trajectories);
228

  
229
			Verbose.printf("Global cost: %.4f (Δ: %.4f) Global Penalty: %.4f (Δ: %.4f)\n\n", newgcost, (newgcost-gcost), newgpenalty, (newgpenalty-gpenalty));
230

  
231
			gcost = newgcost;
232
			gpenalty = newgpenalty;
161 233
		}
162 234

  
163 235
		return trajectories;
164 236
	}
165 237

  
238
	private static double computeGlobalPenalty(
239
			PenaltyFunction[][] penaltyFunctions,
240
			EvaluatedTrajectory[] trajectories) {
241
		double penalty = 0;
242
		for (int i = 0; i < penaltyFunctions.length; i++) {
243
			for (int j = 0; j < i; j++) {
244
				penalty += PenaltyIntegrator.integratePenalty(trajectories[i], penaltyFunctions[i][j], trajectories[j], 10);
245
			}
246
		}
247
		return penalty;
248

  
249
	}
250

  
251
	private static double computeGlobalCost(EvaluatedTrajectory[] trajectories) {
252
		double cost = 0;
253
		for (int i = 0; i < trajectories.length; i++) {
254
			cost += trajectories[i].getCost();
255
		}
256
		return cost;
257
	}
258

  
166 259
	private static void printPenaltyBreakout(EvaluatedTrajectory thisTraj,
167 260
			PenaltyFunction[] penaltyFunctionsOfOtherTrajectories,
168 261
			Trajectory[] otherTrajectories, double w) {
......
304 397
		return otherConstraints;
305 398
	}
306 399

  
400

  
401

  
307 402
}
src/main/java/tt/jointtraj/util/Discretization.java
1 1
package tt.jointtraj.util;
2 2

  
3
import org.jgraph.graph.Edge;
3 4
import org.jgrapht.DirectedGraph;
4 5
import tt.euclid2i.Line;
5 6
import tt.euclid2i.Point;
......
13 14

  
14 15
import java.util.Collection;
15 16
import java.util.Collections;
17
import java.util.LinkedList;
16 18

  
17 19
public class Discretization {
18 20

  
......
37 39
        int y = (Math.max(corner1.y, corner2.y) % gridStep) / 2;
38 40
        Point start = new Point(x, y);
39 41

  
42
        LazyGrid lazyGrid = new LazyGrid(start, Collections.EMPTY_LIST, bounds, gridPattern, gridStep);
43
        DirectedGraph<Point, Line> fullGrid = lazyGrid.generateFullGraph();
44

  
40 45
        Collection<Region> inflatedObstacles = Util.inflateRegions(environment.getObstacles(), bodyRadius);
41
        LazyGrid lazyGrid = new LazyGrid(start, inflatedObstacles, bounds, gridPattern, gridStep);
42 46

  
43
        return lazyGrid.generateFullGraph();
47
        Collection<Point> invalidVertices = new LinkedList<Point>();
48
        for (Point vertex : fullGrid.vertexSet()) {
49
        	if (!Util.isInFreeSpace(vertex, inflatedObstacles)) {
50
        		invalidVertices.add(vertex);
51
        	}
52
        }
53
        fullGrid.removeAllVertices(invalidVertices);
54

  
55
        Collection<Line> invalidEdges = new LinkedList<Line>();
56
        for (Line edge : fullGrid.edgeSet()) {
57
        	if (!Util.isVisible(edge.getStart(), edge.getEnd(), inflatedObstacles)) {
58
        		invalidEdges.add(edge);
59
        	}
60
        }
61
		fullGrid.removeAllEdges(invalidEdges);
62

  
63
        return fullGrid;
44 64
    }
45 65
}
src/main/java/tt/vis/FastAgentsLayer.java
62 62
                    Vis.transX(projectedPoint.x - radius),
63 63
                    Vis.transY(projectedPoint.y - radius),
64 64
                    Vis.transH(radius * 2), Vis.transW(radius * 2));
65
            canvas.setColor(Color.WHITE);
66
            canvas.drawString(Integer.toString(i), Vis.transX(projectedPoint.x), Vis.transY(projectedPoint.y));
65 67
        }
66 68
    }
67 69
}
src/main/java/tt/vis/FastTrajectoriesLayer.java
45 45
    public void paint(Graphics2D canvas) {
46 46
        Trajectory[] trajectories = trajectoryProvider.getTrajectories();
47 47

  
48
        if (trajectories == null)
49
        	return;
50

  
48 51
        for (int i = 0; i < trajectories.length; i++) {
49 52
            if (trajectories[i] == null)
50 53
                continue;
src/main/resources/problems/empty-unbounded-r98-n13.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>600</x>
21
<y>600</y>
22
</point>
23
</start>
24
<target>
25
<point>
26
<x>400</x>
27
<y>400</y>
28
</point>
29
</target>
30
<radius>98</radius>
31
<maxspeed>1</maxspeed>
32
</agent>
33
<agent>
34
<start>
35
<point>
36
<x>800</x>
37
<y>800</y>
38
</point>
39
</start>
40
<target>
41
<point>
42
<x>200</x>
43
<y>200</y>
44
</point>
45
</target>
46
<radius>98</radius>
47
<maxspeed>1</maxspeed>
48
</agent>
49
<agent>
50
<start>
51
<point>
52
<x>200</x>
53
<y>200</y>
54
</point>
55
</start>
56
<target>
57
<point>
58
<x>800</x>
59
<y>600</y>
60
</point>
61
</target>
62
<radius>98</radius>
63
<maxspeed>1</maxspeed>
64
</agent>
65
<agent>
66
<start>
67
<point>
68
<x>800</x>
69
<y>200</y>
70
</point>
71
</start>
72
<target>
73
<point>
74
<x>800</x>
75
<y>400</y>
76
</point>
77
</target>
78
<radius>98</radius>
79
<maxspeed>1</maxspeed>
80
</agent>
81
<agent>
82
<start>
83
<point>
84
<x>800</x>
85
<y>600</y>
86
</point>
87
</start>
88
<target>
89
<point>
90
<x>800</x>
91
<y>800</y>
92
</point>
93
</target>
94
<radius>98</radius>
95
<maxspeed>1</maxspeed>
96
</agent>
97
<agent>
98
<start>
99
<point>
100
<x>200</x>
101
<y>600</y>
102
</point>
103
</start>
104
<target>
105
<point>
106
<x>600</x>
107
<y>600</y>
108
</point>
109
</target>
110
<radius>98</radius>
111
<maxspeed>1</maxspeed>
112
</agent>
113
<agent>
114
<start>
115
<point>
116
<x>200</x>
117
<y>400</y>
118
</point>
119
</start>
120
<target>
121
<point>
122
<x>200</x>
123
<y>800</y>
124
</point>
125
</target>
126
<radius>98</radius>
127
<maxspeed>1</maxspeed>
128
</agent>
129
<agent>
130
<start>
131
<point>
132
<x>600</x>
133
<y>200</y>
134
</point>
135
</start>
136
<target>
137
<point>
138
<x>600</x>
139
<y>400</y>
140
</point>
141
</target>
142
<radius>98</radius>
143
<maxspeed>1</maxspeed>
144
</agent>
145
<agent>
146
<start>
147
<point>
148
<x>800</x>
149
<y>400</y>
150
</point>
151
</start>
152
<target>
153
<point>
154
<x>200</x>
155
<y>600</y>
156
</point>
157
</target>
158
<radius>98</radius>
159
<maxspeed>1</maxspeed>
160
</agent>
161
<agent>
162
<start>
163
<point>
164
<x>400</x>
165
<y>200</y>
166
</point>
167
</start>
168
<target>
169
<point>
170
<x>400</x>
171
<y>600</y>
172
</point>
173
</target>
174
<radius>98</radius>
175
<maxspeed>1</maxspeed>
176
</agent>
177
<agent>
178
<start>
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff