From a1f3955e8fe9304cac9eaf554bea6179433b02fe Mon Sep 17 00:00:00 2001 From: PaoloForte95 Date: Mon, 18 Oct 2021 18:59:53 +0300 Subject: [PATCH] Initial commit --- .../taskassignment/TaskAssignment.java | 3236 ----------------- .../taskassignment/TaskAssignmentSimple.java | 1797 --------- .../TaskAssignmentSimulatedAnnealing.java | 2048 ----------- .../test/TaskAssignmentCorridors2.java | 253 -- .../TaskAssignmentMultiRobotsWithoutMap.java | 228 -- .../TaskAssignmentMultiRobotsWithoutMap2.java | 198 - .../TaskAssignmentMultiRobotsWithoutMap3.java | 196 - .../TaskAssignmentMultiRobotsWithoutMap4.java | 216 -- ...kAssignmentMultiRobotsWithoutMapExact.java | 209 -- ...AssignmentMultiRobotsWithoutMapGreedy.java | 206 -- ...ssignmentMultiRobotsWithoutMapGreedy2.java | 174 - ...ssignmentMultiRobotsWithoutMapGreedy3.java | 209 -- ...AssignmentMultiRobotsWithoutMapOnline.java | 211 -- ...ssignmentMultiRobotsWithoutMapOnline2.java | 221 -- ...nmentMultiRobotsWithoutMapRandomGoals.java | 210 -- .../test/TaskAssignmentOrebroWarehouse.java | 826 ----- .../test/TaskAssignmentOrebroWarehouse2.java | 486 --- .../TaskAssignmentOrebroWarehouseOld.java | 209 -- .../TaskAssignmentOrebroWarehouseOnline2.java | 850 ----- ...AssignmentOrebroWarehouseWithDistance.java | 956 ----- ...ignmentOrebroWarehouseWithPropagation.java | 955 ----- .../test/TaskAssignmentRobotsGrid.java | 255 -- .../test/TaskAssignmentRobotsGridExact.java | 153 - .../test/TaskAssignmentRobotsGridGreedy.java | 151 - .../test/TaskAssignmentRobotsInLine.java | 209 -- .../TaskAssignmentRobotsInLineGreedy.java | 207 -- .../test/TaskAssignmentWithMap.java | 176 - .../test/TaskAssignmentWithMap2.java | 183 - .../test/TaskAssignmentWithMap3.java | 176 - .../test/TaskAssignmentWithMap4.java | 216 -- .../test/TaskAssignmentWithMap5.java | 189 - .../test/TaskAssignmentWithMap6.java | 193 - .../test/TaskAssignmentWithMap7.java | 216 -- .../test/TaskAssignmentWithMap8.java | 261 -- .../test/TaskAssignmentWithMapGreedy.java | 180 - .../test/TaskAssignmentWithMapGreedy2.java | 178 - .../test/TaskAssignmentWithMapGreedy3.java | 178 - .../test/TestCreateEnvelope.java | 471 --- .../taskassignment/test/TestScalability.java | 229 -- 39 files changed, 17715 deletions(-) delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignment.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignmentSimple.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignmentSimulatedAnnealing.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentCorridors2.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap2.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap3.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap4.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapExact.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy2.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy3.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapOnline.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapOnline2.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapRandomGoals.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouse.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouse2.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseOld.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseOnline2.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseWithDistance.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseWithPropagation.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGrid.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGridExact.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGridGreedy.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsInLine.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsInLineGreedy.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap2.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap3.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap4.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap5.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap6.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap7.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap8.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy2.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy3.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TestCreateEnvelope.java delete mode 100644 src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TestScalability.java diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignment.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignment.java deleted file mode 100644 index 18c5d96..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignment.java +++ /dev/null @@ -1,3236 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment; - -import java.io.BufferedWriter; -import java.io.File; -import java.io.FileNotFoundException; -import java.io.FileOutputStream; -import java.io.FileWriter; -import java.io.IOException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Calendar; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Iterator; -import java.util.List; -import java.util.Random; -import java.util.TreeSet; -import java.util.logging.Logger; - -import org.apache.commons.lang.ArrayUtils; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; -import org.metacsp.utility.UI.Callback; -import org.metacsp.utility.logging.MetaCSPLogging; - - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.Geometry; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.util.datastructure.Pair; -import se.oru.coordination.coordination_oru.AbstractTrajectoryEnvelopeCoordinator; - -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.IndexedDelay; -import se.oru.coordination.coordination_oru.Mission; - -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.TrajectoryEnvelopeCoordinator; - -import se.oru.coordination.coordination_oru.fleetmasterinterface.AbstractFleetMasterInterface; -import se.oru.coordination.coordination_oru.fleetmasterinterface.FleetMasterInterface; -import se.oru.coordination.coordination_oru.fleetmasterinterface.FleetMasterInterfaceLib.CumulatedIndexedDelaysList; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner.PLANNING_ALGORITHM; -import se.oru.coordination.coordination_oru.util.FleetVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import com.google.ortools.linearsolver.*; - - - -public class TaskAssignment { - - //Optimization Problem Parameters - protected int numRobot; - protected int numTask; - protected int dummyRobot; - protected int dummyTask; - protected int numRobotAug; - protected int numTaskAug; - protected int maxNumPaths = 1; - protected double linearWeight = 1; - protected double [][][] costValuesMatrix; - protected ArrayList taskQueue = new ArrayList (); - protected ArrayList taskPosponedQueue = new ArrayList (); - //Parameters of weights in Optimization Problem - protected double pathLengthWeight = 1; - protected double arrivalTimeWeight = 0; - protected double tardinessWeight = 0; - protected double timeOut = Double.POSITIVE_INFINITY; - protected int numIteration = -1; - - //Number of Idle Robots - protected ArrayList IDsIdleRobots = new ArrayList (); - protected ArrayList IDsRealTasks = new ArrayList (); - protected ArrayList IDsAllRobots = new ArrayList (); - protected ArrayList IDsAllTasks = new ArrayList (); - - protected int virtualRobotIDs = Integer.MAX_VALUE; - protected int virtualTaskIDs = Integer.MAX_VALUE; - - - //ROADMAP PArameters - protected String scenario; - protected double [][][] ScenarioAllocation; - protected int numAllocation = 1; - protected boolean saveFutureAllocation= false; - protected int previousAssignment = 0; - - //Path and arrival Time Parameters - //Infinity cost if path to reach a goal note exists - protected double MaxPathLength = 10000000; - //This is the sum of max path length for each robot - protected double sumMaxPathsLength = 1; - //This is the sum of arrival time considering max path length for each robot - protected double sumArrivalTime = 1; - //Parameters of mininum Velocity and Acceleration considering all robots - protected double minMaxVel; - protected double minMaxAcc; - //This is the sum of all tardiness - protected double sumTardiness = 1; - - //Motion planner and Coordinator Parameters - protected AbstractTrajectoryEnvelopeCoordinator coordinator; - - - - - //Time required by function parameters - protected long timeRequiretoEvaluatePaths; - protected long timeRequiretofillInPall; - protected long timeRequiretoComputeCriticalSection; - protected long timeRequiretoComputePathsDelay; - protected long initialTime; - protected boolean readyForNextAssignment = true; - - protected HashMap pathsToTargetGoal = new HashMap(); - protected ArrayList pathsDrivingRobot = new ArrayList (); - protected HashMap criticalSections = new HashMap(); - - //FleetMaster Interface Parameters - - protected AbstractFleetMasterInterface fleetMasterInterface = null; - protected boolean propagateDelays = false; - protected static Logger metaCSPLogger = MetaCSPLogging.getLogger(TrajectoryEnvelopeCoordinator.class); - - - - - //Task Allocation Thread Parameters - protected int CONTROL_PERIOD_Task = 20000; - public static int EFFECTIVE_CONTROL_PERIOD_task = 0; - protected FleetVisualization viz = null; - - - /** - * Set true if you want to save the scenario for allocations - * - */ - public void setSaveFutureAllocation(boolean saveFutureAllocation) { - this.saveFutureAllocation = saveFutureAllocation; - - } - - - - - /** - * Set the timeOut for the optimazion Problem in minutes. The algorithm will search a solution until this time. - * Use number from 0.1 to 0.6 for seconds - * @param timeOus -> timeout value in minutes - * - */ - public void setTimeOutinMin(double timeOut) { - if(timeOut < 0.1) { - throw new Error("Timeout cannot be negative!"); - } - if(timeOut < 0.6) { - this.timeOut = timeOut*1000; - } - else { - this.timeOut = timeOut*60*1000; - } - - } - - - /** - * Set the number of paths to reach a goal. - * @param maxNumPaths -> number of path to reach a goal - */ - public boolean getReadyForNextAssignment() { - - return this.readyForNextAssignment; - } - - /** - * Set the number of paths to reach a goal. - * @param maxNumPaths -> number of path to reach a goal - */ - public void setReadyForNextAssignment(boolean flag) { - - this.readyForNextAssignment = flag; - } - - /** - * Set the number of paths to reach a goal. - * @param maxNumPaths -> number of path to reach a goal - */ - public void setmaxNumPaths(int maxNumPaths) { - - this.maxNumPaths = maxNumPaths; - } - - - public void writeMatrix(String filename, double[][][] optimalAssignmentMatrix) { - try { - BufferedWriter bw = new BufferedWriter(new FileWriter((filename),true)); - bw.write("{{"); - for (int i = 0; i < optimalAssignmentMatrix.length; i++) { - for (int j = 0; j < optimalAssignmentMatrix[i].length; j++) { - bw.write("{"); - for (int s = 0; s < maxNumPaths; s++) { - bw.write(optimalAssignmentMatrix[i][j][s]+""); - } - bw.write("}"); - bw.write(","); - - } - bw.write("}"); - bw.write(","); - bw.newLine(); - bw.write("{"); - } - bw.write("-------------"); - bw.newLine(); - bw.flush(); - } catch (IOException e) {} - } - - - /** - * Load a Scenario - * @param scenario -> Scenario to load - */ - - public void LoadScenario(String scenario) { - this.scenario = scenario; - } - - /** - * Load an Optimal Task Allocation - * @param scenario -> Scenario to load - */ - - public void LoadScenarioAllocation(double [][][] Allocation) { - this.ScenarioAllocation = Allocation; - } - - /** - * Set the Coordinator - * @param viz -> Visualization to use - */ - - public void setCoordinator(AbstractTrajectoryEnvelopeCoordinator coordinator) { - this.coordinator = coordinator; - } - - - /** - * Get the Coordinator - * @param viz -> Visualization to use - */ - - public AbstractTrajectoryEnvelopeCoordinator getCoordinator() { - return this.coordinator; - } - - - /** - * Set the weights of cost functions in Optimization Problem. THese must be numbers between 0 and 1. - * @param The path length weight; - * @param The arrival time weight; - * @param The tardiness weight - */ - - public void setCostFunctionsWeight(double pathLengthWeight,double arrivalTimeWeight,double tardinessWeight) { - if(pathLengthWeight <0|| arrivalTimeWeight < 0 || tardinessWeight < 0) { - throw new Error("Weights cannot be numbers less than 0!"); - } - double sumWeight = pathLengthWeight +arrivalTimeWeight + tardinessWeight; - if(sumWeight != 1 || sumWeight < 0 ) { - throw new Error("Weights sum must be equal to 1!"); - } - this.pathLengthWeight = pathLengthWeight; - this.arrivalTimeWeight = arrivalTimeWeight; - this.tardinessWeight = tardinessWeight; - } - - - - /** - * Set the linear weight used in Optimization Problem - * @param viz -> Visualization to use - */ - - public void setLinearWeight(double linearWeight) { - this.linearWeight = linearWeight; - } - - - /** - * Set the Fleet Visualization - * @param viz -> Visualization to use - */ - - public void setFleetVisualization(FleetVisualization viz) { - this.viz = viz; - } - - /** - * Create the IDs for virtual robot only; Them are created starting from IDs of last - * @param viz -> Visualization to use - */ - - private void getAllRobotIDs() { - int virtualRobotID = this.virtualRobotIDs; - for(int i= 0; i < numRobotAug; i++) { - if(i < IDsIdleRobots.size()) { - IDsAllRobots.add(IDsIdleRobots.get(i)); - }else { - IDsAllRobots.add(virtualRobotID); - virtualRobotID = virtualRobotID-1; - this.virtualRobotIDs -=1; - } - } - } - - - public ArrayList getAllRobotIDsReal() { - return this.IDsAllRobots; - } - - - - /** - * Get IDs of all tasks given in Optimization problem - * @return - */ - public ArrayList getTaskIDs() { - ArrayList taskGivenIDs = new ArrayList (); - for(int j=0; j < taskQueue.size();j++) { - taskGivenIDs.add(taskQueue.get(j).getID()); - } - return taskGivenIDs; - } - - private void getRealTaskIDs() { - for(int j=0; j < taskQueue.size();j++) { - IDsRealTasks.add(taskQueue.get(j).getID()); - } - } - - - private void getAllTaskIDs() { - getRealTaskIDs(); - int virtaulTaskID = this.virtualTaskIDs; - for(int i= 0; i < numTaskAug; i++) { - if(i < IDsRealTasks.size()) { - IDsAllTasks.add(IDsRealTasks.get(i)); - }else { - IDsAllTasks.add(virtaulTaskID); - virtaulTaskID = virtaulTaskID-1; - this.virtualTaskIDs -=1; - } - } - } - - /** - * Check if a goal can be reached by at least one robot of the Fleet - * @param PAll -> Initial PAll - * @return PAll incremented a task cannot be reach by any robot - */ - - private double [][][] checkTargetGoals (double [][][] PAll){ - for (int j= 0; j< PAll[0].length ; j++) { - boolean targetEndCanBeReach = false; - for (int i = 0; i < PAll.length; i++) { - for (int s = 0; s < maxNumPaths; s++) { - if(PAll[i][j][s] != MaxPathLength) { - targetEndCanBeReach = true; - } - } - } - //no robot can reach the target end -> need to introduce a dummy task and robot - if(!targetEndCanBeReach) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - - } - } - - double [][][] PAllAug = new double [numRobotAug][numTaskAug][maxNumPaths]; - int robotID = 0; - int taskID = 0; - for(int i = 0;i < numRobotAug; i++) { - for(int j = 0; j< numTaskAug;j++) { - for (int s = 0; s < maxNumPaths; s++) { - - if(i < PAll.length && j< PAll[0].length) { - PAllAug[i][j][s] = PAll[i][j][s]; - }else { - if(i < IDsAllRobots.size()) { - robotID = IDsAllRobots.get(i); - - - }else { - robotID = this.virtualRobotIDs -1 ; - IDsAllRobots.add(robotID); - } - if (j < IDsAllTasks.size()) { - taskID = IDsAllTasks.get(j); - }else { - taskID = virtualTaskIDs - 1; - this.virtualTaskIDs -= 1; - IDsAllTasks.add(taskID); - } - PAllAug[i][j][s] = 1; - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s, null); - - } - } - - } - } - return PAllAug; - } - - /** - * The default footprint used for robots if none is specified. - * NOTE: coordinates in footprints must be given in in CCW or CW order. - */ - public static Coordinate[] DEFAULT_FOOTPRINT = new Coordinate[] { - new Coordinate(-1.7, 0.7), //back left - new Coordinate(-1.7, -0.7), //back right - new Coordinate(2.7, -0.7), //front right - new Coordinate(2.7, 0.7) //front left - }; - - /** - * Set the minimum values of maximum velocity and acceleration considering all robots of the fleet - * @param MaxVel -> minimum of maximum velocity of all robot models; - * @param MaxAccel -> minimum of maximum acceleration of all robot models; - */ - - public void setminMaxVelandAccel(double MaxVel,double MaxAccel) { - this.minMaxVel = MaxVel; - this.minMaxAcc = MaxAccel; - } - - class SortByDeadline implements Comparator{ - @Override - public int compare(Task task1, Task task2) { - return (int) (task1.getDeadline()-task2.getDeadline()); - } - } - - - /** - * Evaluate if a task has a deadline. Tasks with the closest deadline are priority. - */ - - private void checkOnTaskDeadline() { - ArrayList taskArray =new ArrayList (); - for(int j=0; j < taskQueue.size(); j++ ) { - taskArray.add(taskQueue.get(j)); - } - taskArray.sort(new SortByDeadline()); - for(int i=0;i < IDsIdleRobots.size(); i++) { - int index = taskQueue.indexOf(taskArray.get(i)); - if(taskQueue.get(index).getDeadline() != -1) { - taskQueue.get(index).setPriority(true); - } - - } - } - - /** - * Delete one or multiple tasks from taskQueue in order to avoid blocking - */ - - private void checkOnBlocking(AbstractTrajectoryEnvelopeCoordinator tec) { - - Coordinate[] taskFootprint = tec.getMaxFootprint(); - for(int j=0; j < taskQueue.size(); j++ ) { - Task taskProva = taskQueue.get(j); - double xTask=taskProva.getGoalPose().getX(); - double yTask=taskProva.getGoalPose().getY(); - double dist1 = taskFootprint[0].distance(taskFootprint[1])/2; - double dist2 = taskFootprint[1].distance(taskFootprint[2])/2; - Coordinate Taskfootprint1 = new Coordinate((xTask-dist1),(yTask+dist2)); - Coordinate Taskfootprint2 = new Coordinate((xTask+dist1),(yTask+dist2)); - Coordinate Taskfootprint3 = new Coordinate((xTask+dist1),(yTask-dist2)); - Coordinate Taskfootprint4 = new Coordinate((xTask-dist1),(yTask-dist2)); - Polygon ll=TrajectoryEnvelope.createFootprintPolygon(Taskfootprint1,Taskfootprint2,Taskfootprint3,Taskfootprint4); - - for(int k = 0; k < taskQueue.size(); k++ ) { - if(k != j) { - Task taskProva2 = taskQueue.get(k); - double xTask2=taskProva2.getGoalPose().getX(); - double yTask2=taskProva2.getGoalPose().getY(); - Coordinate Taskfootprint5 = new Coordinate((xTask2-dist1),(yTask2+dist1)); - Coordinate Taskfootprint6 = new Coordinate((xTask2+dist1),(yTask2+dist2)); - Coordinate Taskfootprint7 = new Coordinate((xTask2+dist1),(yTask2-dist2)); - Coordinate Taskfootprint8 = new Coordinate((xTask2-dist1),(yTask2-dist2)); - Polygon gg=TrajectoryEnvelope.createFootprintPolygon(Taskfootprint5,Taskfootprint6,Taskfootprint7,Taskfootprint8); - if(ll.intersects(gg) ) { - if(taskProva2.getDeadline() == -1 && taskProva.getDeadline() == -1) { - taskQueue.remove(k); - taskPosponedQueue.add(taskProva2); - }else { - if (taskProva2.getDeadline() == -1 && taskProva.getDeadline() != -1){ - taskQueue.remove(k); - taskPosponedQueue.add(taskProva2); - - }else if(taskProva2.getDeadline() != -1 && taskProva.getDeadline() == -1) { - taskQueue.remove(j); - taskPosponedQueue.add(taskProva); - j -=1; - break; - }else { - if(taskProva2.getDeadline() > taskProva.getDeadline() ) { - taskQueue.remove(k); - taskPosponedQueue.add(taskProva2); - }else { - taskQueue.remove(j); - taskPosponedQueue.add(taskProva); - j -=1; - break; - } - } - } - - } - - } - } - } - } - - /** - * Enable and initialize the fleetmaster library to estimate precedences to minimize the overall completion time. - * Note: this function should be called before placing the first robot. - * ATTENTION: If dynamic_size is false, then the user should check that all the paths will lay in the given area. - * @param origin_x The x coordinate (in meters and in global inertial frame) of the lower-left pixel of fleetmaster GridMap. - * @param origin_y The y coordinate (in meters and in global inertial frame) of the lower-left pixel of fleetmaster GridMap. - * @param origin_theta The theta coordinate (in rads) of the lower-left pixel map (counterclockwise rotation). Many parts of the system currently ignore it. - * @param resolution The resolution of the map (in meters/cell), 0.01 <= resolution <= 1. It is assumed this parameter to be global among the fleet. - * The highest the value, the less accurate the estimation, the lowest the more the computational effort. - * @param width Number of columns of the map (>= 1) if dynamic sizing is not enabled. - * @param height Number of rows of the map (>= 1) if dynamic sizing is not enabled. - * @param dynamic_size If true, it allows to store only the bounding box containing each path. - * @param propagateDelays If true, it enables the delay propagation. - * @param debug If true, it enables writing to screen debugging info. - */ - public void instantiateFleetMaster(double origin_x, double origin_y, double origin_theta, double resolution, long width, long height, boolean dynamic_size, boolean propagateDelays, boolean debug) { - this.fleetMasterInterface = new FleetMasterInterface(origin_x, origin_y, origin_theta, resolution, width, height, dynamic_size, debug); - this.fleetMasterInterface.setDefaultFootprint(DEFAULT_FOOTPRINT); - this.propagateDelays = propagateDelays; - } - - /** - * Enable and initialize the fleetmaster library to estimate precedences to minimize the overall completion time - * while minimizing the computational requirements (bounding box are used to set the size of each path-image). - * Note: this function should be called before placing the first robot. - * @param resolution The resolution of the map (in meters/cell), 0.01 <= resolution <= 1. It is assumed this parameter to be global among the fleet. - * The highest the value, the less accurate the estimation, the lowest the more the computational effort. - * @param propagateDelays If true, it enables the delay propagation. - */ - public void instantiateFleetMaster(double resolution, boolean propagateDelays) { - this.fleetMasterInterface = new FleetMasterInterface(0., 0., 0., resolution, 100, 100, true, false); - this.fleetMasterInterface.setDefaultFootprint(DEFAULT_FOOTPRINT); - this.propagateDelays = propagateDelays; - } - - - /** - * Add a path to the fleetmaster interface - * @param robotID -> The ID of the robot - * @param pathID -> the ID of the path - * @param pss -> the path expressed as a PoseSteering vector - * @param boundingBox -> the bounding box of the path - * @param coordinates -> footprint of the robot - */ - protected void addPath(int robotID, int pathID, PoseSteering[] pss, Geometry boundingBox, Coordinate... coordinates) { - if (!fleetMasterInterface.addPath(robotID, pathID, pss, boundingBox, coordinates)) - metaCSPLogger.severe("Unable to add the path to the fleetmaster gridmap. Check if the map contains the given path."); - } - - - - /** - * Delete the path from the fleetmaster interface - * @param pathID -> The ID of the path to remove - */ - protected void removePath(int pathID){ - if (!fleetMasterInterface.clearPath(pathID)) - metaCSPLogger.severe("Unable to remove the path to the fleetmaster gridmap. Check if the map contains the given path."); - } - - - protected CumulatedIndexedDelaysList toIndexedDelaysList(TreeSet delays, int max_depth) { - //Handle exceptions - if (delays == null) { - metaCSPLogger.severe("Invalid input in function toPropagationTCDelays!!"); - throw new Error("Invalid input in function toPropagationTCDelays!!"); - } - if (delays.isEmpty() || max_depth < 1) return new CumulatedIndexedDelaysList(); - - //Cast the type - ArrayList indices = new ArrayList(); - ArrayList values = new ArrayList(); - Iterator it = delays.descendingIterator(); - IndexedDelay prev = delays.last(); - while (it.hasNext()) { - IndexedDelay current = it.next(); - //Check unfeasible values - if (current.getValue() == Double.NaN) { - metaCSPLogger.severe("NaN input in function toPropagationTCDelays!!"); - throw new Error("NaN input in function toPropagationTCDelays!!"); - } - if (current.getValue() == Double.NEGATIVE_INFINITY) { - metaCSPLogger.severe("-Inf input in function toPropagationTCDelays!!"); - throw new Error("-Inf input in function toPropagationTCDelays!!"); - } - if (prev.getIndex() < current.getIndex()) { - metaCSPLogger.severe("Invalid IndexedDelays TreeSet!!"); - throw new Error("Invalid IndexedDelays TreeSet!!"); - } - - //Update the value only if positive and only if the index is lower than the max depth - if (current.getValue() > 0 && current.getValue() < Double.MAX_VALUE && current.getIndex() < max_depth) { - if (values.size() == 0) { - //Add the index the first time its value is positive - indices.add(new Long(current.getIndex())); - values.add(current.getValue()); - } - else if (prev.getIndex() == current.getIndex()) - //Handle multiple delays in the same critical point - values.set(values.size()-1, values.get(values.size()-1) + current.getValue()); - else { - //Add the cumulative value if it is not the first. - indices.add(new Long(current.getIndex())); - values.add(values.get(values.size()-1) + current.getValue()); - } - } - prev = current; - } - CumulatedIndexedDelaysList propTCDelays = new CumulatedIndexedDelaysList(); - if (indices.size() > 0) { - propTCDelays.size = indices.size(); - propTCDelays.indices = ArrayUtils.toPrimitive((Long[]) indices.toArray(new Long[indices.size()])); - ArrayUtils.reverse(propTCDelays.indices); - propTCDelays.values = ArrayUtils.toPrimitive((Double[]) values.toArray(new Double[values.size()])); - ArrayUtils.reverse(propTCDelays.values); - } - return propTCDelays; - } - - - protected Pair estimateTimeToCompletionDelays(int path1ID,PoseSteering[] pss1, TreeSet delaysRobot1, int path2ID,PoseSteering[] pss2, TreeSet delaysRobot2, CriticalSection cs) { - - if (this.fleetMasterInterface != null && fleetMasterInterface.checkPathHasBeenAdded(path1ID)&& fleetMasterInterface.checkPathHasBeenAdded(path2ID)) { - CumulatedIndexedDelaysList te1TCDelays = toIndexedDelaysList(delaysRobot1, pss1.length); - //metaCSPLogger.info("[estimateTimeToCompletionDelays] te1TCDelays: " + te1TCDelays.toString()); - CumulatedIndexedDelaysList te2TCDelays = toIndexedDelaysList(delaysRobot2, pss2.length); - //metaCSPLogger.info("[estimateTimeToCompletionDelays] te2TCDelays: " + te2TCDelays.toString()); - return fleetMasterInterface.queryTimeDelay(cs, te1TCDelays, te2TCDelays,pss1,pss2); - } - - return new Pair (Double.NaN, Double.NaN); - } - - - - /** - * Add a Task to Mission set - * @param task -> the task to add - * @return -> true if task is added correctly, otherwise false - */ - public boolean addTask(Task task) { - if (task == null) { - metaCSPLogger.severe("No task to add. Please give a correct Task."); - throw new Error("Cannot add the task"); - } - boolean TaskisAdded = taskQueue.add(task); - return TaskisAdded; - } - - /** - * Remove a task from the queue - * @param task The task to remove - * @return true if task is removed correctly, otherwise false - */ - public boolean removeTask(Task task) { - return taskQueue.remove(task); - } - - /** - * Remove the task in the index position from the queue - * @param task The task to remove - * @return Task that is removed - */ - public Task removeTask(int index) { - return taskQueue.remove(index); - } - - - - /** - * Get the task from Mission set in the index position - * @param index -> the index position of the task - * @return -> the task in index position - */ - public Task getTask(int index) { - if (index < 0 || index > taskQueue.size()) { - metaCSPLogger.severe("Wrong index."); - throw new Error("The task" + index + "not exist"); - }else { - return taskQueue.get(index); - } - } - - /** - * Compute the number of Dummy Robots and/or Tasks. Consider the possibility to have a different number of robots (N) and tasks (M). If N > M, dummy tasks are - * considered, where a dummy task is a task for which a robot stay in starting position; while if M > N dummy robots - * are considered, where a dummy robot is only a virtual robot. - * @param numRobot Number of robots - * @param numTasks Number of tasks - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - private void dummyRobotorTask(int numRobot, int numTasks,AbstractTrajectoryEnvelopeCoordinator tec) { - numRobotAug = numRobot; - numTaskAug = numTasks; - //Restore initial value for dummy robot and task - dummyTask = 0; - dummyRobot = 0; - //Considering the possibility to have n != m - //If n > m -> we have dummy robot, so at some robot is assign the task to stay in starting position - if (numRobot > numTasks) { - dummyTask = numRobot - numTasks; - numTaskAug = numTasks + dummyTask; - } - else if (numRobot < numTasks) { - dummyRobot = numTasks - numRobot; - numRobotAug = numRobot + dummyRobot; - } - //This second check is used when we have particular cases due to Robot Type and Task Type - //Only if we have already a dummy task this check can be avoided - //If A robot cannot be assigned to any task - if (dummyTask == 0 || dummyRobot != 0) { - for (int i = 0; i < numRobot; i++) { - boolean flagAllocateRobot = false; - for (int j = 0; j < numTasks; j++) { - //check if robot can be assigned to one task - //if (taskQueue.get(j).getTaskType() == tec.getRobotType(IDsIdleRobots[i])) { - if (taskQueue.get(j).isCompatible(tec.getRobot(IDsIdleRobots.get(i)))) { - flagAllocateRobot = true; - - } - } - //the robot cannot be assigned to any task -> add a dummy robot and task - if (!flagAllocateRobot) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - } - } - } - //Only if we have already a dummy robot this check can be avoided - //If A task cannot be assigned to any robot - if (dummyRobot == 0 || dummyTask != 0) { - for (int i = 0; i < numTasks; i++) { - boolean flagAllocateTask = false; - for (int j = 0; j < numRobot; j++) { - //check if task can be assigned to one robot - //if (taskQueue.get(i).getTaskType() == tec.getRobotType(IDsIdleRobots[j])) { - if (taskQueue.get(i).isCompatible(tec.getRobot(IDsIdleRobots.get(j)))) { - flagAllocateTask = true; - } - } - //the task cannot be assigned to any robot -> add a dummy robot and task - if (!flagAllocateTask) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - } - } - } - } - - - /** - * Transform a 1D array of MPVariable into a 3D MATRIX - * @param numRobot -> Number of robots - * @param numTasks -> Number of tasks - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblem}, {@link #buildOptimizationProblemWithB} or {@link #buildOptimizationProblemWithBNormalized} - * @return 3D Matrix of Decision Variable of the input problem - */ - private MPVariable [][][] tranformArray(MPSolver optimizationProblem) { - //Take the vector of Decision Variable from the Optimization Problem - MPVariable [] array1D = optimizationProblem.variables(); - MPVariable [][][] decisionVariable = new MPVariable [numRobotAug][numTaskAug][maxNumPaths]; - //Store them in a 2D Matrix - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - for (int s = 0; s < maxNumPaths; s++) { - decisionVariable[i][j][s] = array1D[i*numTaskAug*maxNumPaths+j*maxNumPaths+s]; - } - - } - } - return decisionVariable; - } - - /** - * Impose a constraint on the optimization problem on previous optimal solution in order to not consider more it - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblem},{@link #buildOptimizationProblemWithB} or {@link #buildOptimizationProblemWithBNormalized} in which a solution is found - * @param assignmentMatrix -> The Assignment Matrix of the actual optimal solution - * @return Optimization Problem updated with the new constraint on previous optimal solution found - */ - private MPSolver constraintOnPreviousSolution(MPSolver optimizationProblem, double [][][] assignmentMatrix) { - //Take decision Variable from Optimization Problem - MPVariable [][][] DecisionVariable = tranformArray(optimizationProblem); - //Initialize a Constraint - //MPConstraint c2 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY,1); - MPConstraint c2 = optimizationProblem.makeConstraint(0,numRobotAug-1); - //Define the actual optimal solution as a Constraint in order to not consider more it - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - for(int s = 0;s < maxNumPaths; s++) { - if (assignmentMatrix[i][j][s] >0) { - c2.setCoefficient(DecisionVariable[i][j][s],1); - } - } - } - } - //Return the updated Optimization Problem - return optimizationProblem; - } - - - /** - * Impose a constraint on the optimization problem on previous optimal solution cost in order to not consider more solution that has a cost higher - * than this. In this manner is possible to avoid some cases. - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblem} in which a solution is found - * @param assignmentMatrix -> The Assignment Matrix of the actual optimal solution - * @return Optimization Problem updated with the new constraint on optimal solution cost - */ - private MPSolver constraintOnCostSolution(MPSolver optimizationProblem,double objectiveValue) { - //Take the vector of Decision Variable from the input solver - MPVariable [][][] decisionVariable = tranformArray(optimizationProblem); - //Add tolerance - objectiveValue = objectiveValue + 0.0005; - //Initialize a Constraint - MPConstraint c3 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY,objectiveValue); - //Define a constraint for which the next optimal solutions considering only B must have a cost less than objectiveValue - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - for(int s = 0;s < maxNumPaths; s++) { - c3.setCoefficient(decisionVariable[i][j][s],costValuesMatrix[i][j][s]); - } - } - } - //Return the updated Optimization Problem - return optimizationProblem; - } - - - /** - * Store the solution of a optimization problem in a Matrix - * @param numRobot -> Number of robots - * @param numTasks -> Number of tasks - * @param optimizationProblem -> A solved optimization problem - * @return Assignment matrix for the optimization problem given as input - */ - - private double [][][] saveAssignmentMatrix(int numRobot,int numTasks,MPSolver optimizationProblem){ - //Take the decision variable from the optimization problem - MPVariable [][][] decisionVariable = tranformArray(optimizationProblem); - double [][][] assignmentMatrix = new double [numRobot][numTasks][maxNumPaths]; - //Store decision variable values in a Matrix - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTasks; j++) { - for(int s = 0;s < maxNumPaths; s++) { - assignmentMatrix[i][j][s] = decisionVariable[i][j][s].solutionValue(); - } - } - } - return assignmentMatrix; - } - - - /** - * Evaluate the cost associated to the path length for the a couple of robot and task. - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param robot -> The Robot ID - * @param task -> The Task ID - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The cost associated to the path length for the couple of robot and task given as input - */ - private double evaluatePathLength(int robotID , int taskID, int path, AbstractTrajectoryEnvelopeCoordinator tec){ - //Evaluate the path length for the actual couple of task and ID - //Initialize the path length to infinity - double pathLength = MaxPathLength; - //take index positon of robotID in Robot set - int robotindex = IDsIdleRobots.indexOf(robotID); - // Only for real robots and tasks - if (IDsIdleRobots.contains(robotID) && IDsRealTasks.contains(taskID)) { - //Take the state for the i-th Robot - - RobotReport rr = tec.getRobotReport(IDsIdleRobots.get(robotindex)); - if (rr == null) { - metaCSPLogger.severe("RobotReport not found for Robot" + robotID + "."); - throw new Error("RobotReport not found for Robot" + robotID + "."); - } - //Evaluate the path from the Robot Starting Pose to Task End Pose - int taskIndex = IDsRealTasks.indexOf(taskID); - AbstractMotionPlanner rsp = tec.getMotionPlanner(robotID).getCopy(false); - - rsp.setStart(rr.getPose()); - rsp.setGoals(taskQueue.get(taskIndex).getStartPose(),taskQueue.get(taskIndex).getGoalPose()); - rsp.setFootprint(tec.getFootprint(robotID)); - - if (!rsp.plan()) { - System.out.println("Robot" + robotID +" cannot reach the Target End of Task " + taskID); - //the path to reach target end not exits - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, null); - //Infinity cost is returned - - return pathLength; - - } - - //If the path exists - //Take the Pose Steering representing the path - PoseSteering[] pss = rsp.getPath(); - - //Add the path to the FleetMaster Interface -> this is necessary for F function - addPath(robotID, pss.hashCode(), pss, null, tec.getFootprint(robotID)); - //Save the path to Task in the path set - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, pss); - //Take the Path Length - Mission m1 = new Mission(robotID,pss); - Missions.enqueueMission(m1); - pathLength = Missions.getPathLength(pss); - } else { //There also virtual robot and task are considered - //There are considered real robot and dummy task - if (numRobot >= numTask && IDsIdleRobots.contains(robotID)){ //dummy task -> The Robot receive the task to stay in starting position - //The second condition is used in the special case in which we have that one robot cannot be - //assigned to any tasks due to its type, so we must add a dummy robot and a dummy task, but we - //Create the task to stay in robot starting position - PoseSteering[] dummyTask = new PoseSteering[1]; - //Take the state for the i-th Robot - RobotReport rr = tec.getRobotReport(IDsIdleRobots.get(robotindex)); - if (rr == null) { - metaCSPLogger.severe("RobotReport not found for Robot" + robotID + "."); - throw new Error("RobotReport not found for Robot" + robotID + "."); - } - //take the starting position of the robot - dummyTask[0] = new PoseSteering(rr.getPose(),0); - //Add the path to the FleetMaster Interface -> so it can be considered as an obstacle from - //the motion planner - addPath(robotID, dummyTask.hashCode(), dummyTask, null, tec.getFootprint(robotID)); - //Save the path to Dummy Task - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, dummyTask); - //Consider a minimal pathLength - pathLength = 1; - Mission m1 = new Mission(robotID,dummyTask); - Missions.enqueueMission(m1); - - return pathLength; - } - else { //There are considered dummy robot and real task - //dummy robot -> Consider a only virtual Robot - PoseSteering[] dummyRobot = new PoseSteering[1]; - dummyRobot[0] = new PoseSteering(taskQueue.get(0).getGoalPose(),0); - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, dummyRobot); - pathLength = 1; - Mission m1 = new Mission(robotID,dummyRobot); - Missions.enqueueMission(m1); - return pathLength; - } - } - - return pathLength; - } - - /** - * Evaluate the PAll matrix with a precomputed Scenario, that is a matrix that contains all path for each possible combination of robot - * and task - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The PAll matrix - */ - private double [][][] evaluatePAllWithScenario(AbstractTrajectoryEnvelopeCoordinator tec){ - - //Evaluate the path length for the actual couple of task and ID - //Initialize the sum of max paths lengths and time to do it for each robot - //This cost are used then for normalizing cost - double sumPathsLength = 0; - double sumArrivalTime = 0; - String ppCostObjectiveFunction = "Test"+2+"/Sys/prova-T" + 1 +".txt"; - PrintStream fileStream = null; - try { - - - fileStream = new PrintStream((new FileOutputStream(ppCostObjectiveFunction,true))); - - - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - Missions.loadScenario(scenario); - - double [][][] PAll = new double[numRobotAug][numTaskAug][maxNumPaths]; - for (int robotID : IDsAllRobots) { - int robotindex = IDsAllRobots.indexOf(robotID); - double maxPathLength = 1; - for (int taskID : IDsAllTasks ) { - int taskIndex = IDsAllTasks.indexOf(taskID); - double pathLength = MaxPathLength; - //Evaluate path Length - boolean typesAreEqual = false; - if (taskIndex < numTask && robotindex < numRobot ) { - typesAreEqual = taskQueue.get(taskIndex).isCompatible(tec.getRobot(robotID)); - - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - for(int path = 0;path < maxNumPaths; path++) { - if(typesAreEqual) { - if(IDsIdleRobots.contains(robotID)) { - - }else { - - } - //int cont = previousAssignment; - int cont= 0; - - - //if(IDsIdleRobots.contains(robotID)&& taskIndex < taskQueue.size()) { - if(IDsIdleRobots.contains(robotID)&& taskIndex < taskQueue.size()) { - fileStream.println("Missions>> " + Missions.getMissions(robotID).size()); - fileStream.println("Missions taken >> " + cont); - - while(cont < Missions.getMissions(robotID).size() ) { - //while(Missions.getMissions(robotID).size() != 0 ) { - fileStream.println("Boolean >> " + ArrayUtils.isEquals(Missions.getMission(robotID, cont).getToPose().toString(),taskQueue.get(taskIndex).getGoalPose().toString()) + "ID>>" + robotID+ "task" + taskID); - fileStream.println("Missions End >> " + Missions.getMission(robotID, cont).getToPose() + "ID>>" + robotID); - fileStream.println("Missions End >> " + taskQueue.get(taskIndex).getGoalPose() + "ID>>" + robotID); - fileStream.println("Missions start >> " + Missions.getMission(robotID, cont).getFromPose()); - fileStream.println("Missions start >> " + tec.getRobotReport(robotID).getPose()); - boolean flag1 = ArrayUtils.isEquals(Missions.getMission(robotID, cont).getToPose().getX(),taskQueue.get(taskIndex).getGoalPose().getX()); - boolean flag2 = ArrayUtils.isEquals(Missions.getMission(robotID, cont).getToPose().getY(),taskQueue.get(taskIndex).getGoalPose().getY()); - boolean flag3 = ArrayUtils.isEquals(Missions.getMission(robotID, cont).getFromPose().getX(),tec.getRobotReport(robotID).getPose().getX()); - boolean flag4 = ArrayUtils.isEquals(Missions.getMission(robotID, cont).getFromPose().getY(),tec.getRobotReport(robotID).getPose().getY()); - //if( ArrayUtils.isEquals(Missions.getMission(robotID, cont).getToPose().toString(),taskQueue.get(taskIndex).getGoalPose().toString())) { - fileStream.println("Boolean >> " + flag1+ " " + " " + flag2 + " " + flag3 + " " + flag4); - if( flag1 && flag2 && flag3 && flag4) { - fileStream.println("Missions End >> " + Missions.getMission(robotID, cont).getToPose() + "ID>>" + robotID); - PoseSteering[] pss = Missions.getMission(robotID, cont).getPath(); - addPath(robotID, pss.hashCode(), pss, null, tec.getFootprint(robotID)); - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, pss); - pathLength = Missions.getPathLength(pss); - //Missions.removeMissions(Missions.getMission(robotID, cont)); - - } - cont +=1; - - - } - }else { - if (numRobot >= numTask && IDsIdleRobots.contains(robotID)){ - PoseSteering[] pss = new PoseSteering[] {new PoseSteering(tec.getRobotReport(robotID).getPose(),0)}; - pathLength =1 ; - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, pss); - //Missions.removeMissions(Missions.getMission(robotID, cont)); - cont += 1; - - - }else { - PoseSteering[] pss = new PoseSteering[] {new PoseSteering(taskQueue.get(0).getGoalPose(),0)}; - pathLength =1 ; - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, pss); - } - } - - - - - if ( pathLength > maxPathLength && pathLength != MaxPathLength) { - maxPathLength = pathLength; - } - }else { - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, null); - } - PAll[robotindex][taskIndex][path] = pathLength; - }//For path - }//For task - - //Sum the max path length for each robot - sumPathsLength += maxPathLength; - //Sum the arrival time for the max path length - sumArrivalTime += computeArrivalTime(maxPathLength,this.minMaxVel,this.minMaxAcc); - } - double [][][] PAllAug = checkTargetGoals(PAll); - previousAssignment += numTaskAug; - //Save the sum of max paths length to normalize path length cost - this.sumMaxPathsLength = sumPathsLength; - //Save the sum of arrival time considering max paths length to normalize delay cost - this.sumArrivalTime = sumArrivalTime; - - //Return the cost of path length - - return PAllAug; - } - - /** - * Evaluate the PAll matrix, that is a matrix that contains all path for each possible combination of robot - * and task - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The PAll matrix - */ - /** - * Evaluate the PAll matrix, that is a matrix that contains all path for each possible combination of robot - * and task - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The PAll matrix - */ - private double [][][] evaluatePAll(AbstractTrajectoryEnvelopeCoordinator tec){ - - - - - //Evaluate the path length for the actual couple of task and ID - //Initialize the sum of max paths lengths and time to do it for each robot - //This cost are used then for normalizing cost - double sumPathsLength = 0; - double sumArrivalTime = 0; - double [][][] PAll = new double[numRobotAug][numTaskAug][maxNumPaths]; - String ppCostObjectiveFunction = "Test"+2+"/Sys/prova2-T" + 1 +".txt"; - PrintStream fileStream = null; - try { - - - fileStream = new PrintStream((new FileOutputStream(ppCostObjectiveFunction,true))); - - - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - long timeInitial = Calendar.getInstance().getTimeInMillis(); - if(scenario != null) { - double [][][] PAllScenario = evaluatePAllWithScenario(tec); - return PAllScenario; - } - for (int robotID : IDsAllRobots) { - double maxPathLength = 1; - int robotIndex = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int taskIndex = IDsAllTasks.indexOf(taskID); - - //Evaluate path Length - boolean typesAreEqual = false; - if (IDsIdleRobots.contains(robotID) && IDsRealTasks.contains(taskID) ) { - typesAreEqual = taskQueue.get(taskIndex).isCompatible(tec.getRobot(robotID)); - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - for(int path = 0;path < maxNumPaths; path++) { - final int pathID = path; - if(typesAreEqual) { // only if robot and typoe have the same types - - - new Thread("Robot" + robotID) { - public void run() { - evaluatePathLength(robotID,taskID,pathID,tec); - } - }.start(); - //Take time to evaluate the path - - } - else { - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, null); - } - } - }//For Task - - } - boolean allResultsReady = false; - while (!allResultsReady) { - allResultsReady = true; - - for (int robotID : IDsIdleRobots) { - for (int task : IDsAllTasks) { - for(int path=0;path < maxNumPaths ; path ++) { - if (!pathsToTargetGoal.containsKey(robotID*numTaskAug*maxNumPaths+task*maxNumPaths+path) ) { - allResultsReady = false; - } - } - - } - try { Thread.sleep(500); } - catch (InterruptedException e) { - e.printStackTrace(); - } - } - - } - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoEvaluatePaths = timeRequiretoEvaluatePaths + timeRequired; - long timeInitial2 = Calendar.getInstance().getTimeInMillis(); - for (int robotID : IDsAllRobots) { - int robotindex = IDsAllRobots.indexOf(robotID); - double maxPathLength = 1; - for (int taskID : IDsAllTasks ) { - int taskIndex = IDsAllTasks.indexOf(taskID); - double pathLength = MaxPathLength; - //Evaluate path Length - boolean typesAreEqual = false; - if (taskIndex < numTask && robotindex < numRobot ) { - typesAreEqual = taskQueue.get(taskIndex).isCompatible(tec.getRobot(robotID)); - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - for(int path = 0;path < maxNumPaths; path++) { - if(typesAreEqual) { - if(pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path)==null) { - pathLength = MaxPathLength; - }else { - pathLength=Missions.getPathLength(pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path)); - } - - if ( pathLength > maxPathLength && pathLength != MaxPathLength) { - maxPathLength = pathLength; - } - } - - PAll[robotindex][taskIndex][path] = pathLength; - }//For path - }//For task - //Sum the max path length for each robot - sumPathsLength += maxPathLength; - //Sum the arrival time for the max path length - sumArrivalTime += computeArrivalTime(maxPathLength,this.minMaxVel,this.minMaxAcc); - } - //Take the time to fill in the PAll Matrix - long timeFinal2 = Calendar.getInstance().getTimeInMillis(); - long timeRequired2 = timeFinal2- timeInitial2; - timeRequiretofillInPall = timeRequiretofillInPall + timeRequired2; - double [][][] PAllAug = checkTargetGoals(PAll); - //Save the sum of max paths length to normalize path length cost - this.sumMaxPathsLength = sumPathsLength; - //Save the sum of arrival time considering max paths length to normalize delay cost - this.sumArrivalTime = sumArrivalTime; - //Return the cost of path length - Missions.saveScenario("ProvaScenario"+numAllocation); - if(saveFutureAllocation==true) { - numAllocation += 1; - } - - - for (int robotID : IDsIdleRobots) { - int cont= 0; - fileStream.println("Missions>> " + Missions.getMissions(robotID).size()); - for (int taskID : IDsAllTasks ) { - fileStream.println("Missions online>> " + Missions.getMissions(robotID).size()); - if(cont < Missions.getMissions(robotID).size()) { - Mission m1 = Missions.getMission(robotID, cont); - Missions.removeMissions(m1); - fileStream.println("Missions taken >> " + cont); - //cont +=1; - } - } - fileStream.println("Missions again >> " + Missions.getMissions(robotID).size()); - - } - - return PAll; - } - /** - * Evaluate the cost associated to time delay on completion of a task for a specific robot, due to interference with other robot - * and precedence constraints. The cost is evaluated considering the intersection between the path of robot i-th - * with the paths of other robots, considering the actual Assignment, but also paths related to already driving robot - * are considered. - * @param robot -> The i-th Robot - * @param task -> The j-th Task - * @param pathID -> The s-th path - * @param assignmentMatrix -> The Assignment Matrix related to a solution of the optimization problem - * @param tec -> an Abstract Trajectory Envelope Coordinator - * @return The cost associated to the delay on completion of task j for robot i due to interference with other robot - */ - private double evaluatePathDelay(int robotID ,int taskID,int pathID,double [][][] assignmentMatrix,AbstractTrajectoryEnvelopeCoordinator tec){ - CriticalSection[][][][] cssMatrix = new CriticalSection [IDsIdleRobots.size()][IDsRealTasks.size()][maxNumPaths][1]; - PrintStream fileStream1 = null; - PrintStream fileStream2 = null; - PrintStream fileStream3 = null; - - - int a = tec.getTestNumber(); - int c = tec.getFolderNumber(); - - - - String ppDelayTime = "Test"+ c +"/Sys/DelayTime-T" + a +".txt"; - - try { - fileStream1 = new PrintStream(new FileOutputStream("CriticalSections.txt",true)); - fileStream2 = new PrintStream(new FileOutputStream("PathDelay.txt",true)); - fileStream3 = new PrintStream(new FileOutputStream(ppDelayTime,true)); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - long timeInitial2 = 0; - - int robotIndex = IDsAllRobots.indexOf(robotID); - int taskIndex = IDsAllTasks.indexOf(taskID); - //Evaluate the delay time on completion time for the actual couple of task and ID - //Initialize the time delay - double delay = 0; - //Considering the Actual Assignment - if (assignmentMatrix[robotIndex][taskIndex][pathID]>0) { - // Only for real robots and tasks - if (IDsIdleRobots.contains(robotID) && IDsRealTasks.contains(taskID)) { - //Take the Pose steering relate to i-th robot and j-th task from path set - //PoseSteering[] pss1 = pathsToTargetGoalTotal.get((robot-1)*numTaskAug*maxNumPaths + task*maxNumPaths +pathID); - PoseSteering[] pss1 = pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID); - if(pss1 == null) { - return delay; - } - //Initialize Array of delays for the two robots - TreeSet te1TCDelays = new TreeSet() ; - TreeSet te2TCDelays = new TreeSet() ; - //Compute the spatial Envelope for the i-th Robot - - SpatialEnvelope se1 = TrajectoryEnvelope.createSpatialEnvelope(pss1,tec.getFootprint(robotID)); - //Evaluate other path depending from the Assignment Matrix - for(int secondRobotID : IDsIdleRobots) { - int secondRobotIndex = IDsIdleRobots.indexOf(secondRobotID); - for(int secondTaskID: IDsRealTasks) { - int secondTaskIndex = IDsRealTasks.indexOf(secondTaskID); - for(int s = 0;s < maxNumPaths; s++) { - if (assignmentMatrix [secondRobotIndex][secondTaskIndex][s] > 0 && secondRobotID != robotID && secondTaskID != taskID) { - //Take the path of this second robot from path set - - PoseSteering[] pss2 = pathsToTargetGoal.get(secondRobotID*numTaskAug*maxNumPaths + secondTaskID*maxNumPaths+s); - if (pss2 != null) {//is == null if robotType is different to Task type - - - //Evaluate the Spatial Envelope of this second Robot - SpatialEnvelope se2 = TrajectoryEnvelope.createSpatialEnvelope(pss2,tec.getFootprint(secondRobotID)); - long timeInitial = Calendar.getInstance().getTimeInMillis(); - //Compute the Critical Section between this 2 robot - CriticalSection [] css = new CriticalSection[1]; - - /* - - if(criticalSections.containsKey(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID) ) { - css= criticalSections.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID)[secondRobotIndex][secondTaskIndex][s]; - if(css.length == 1) { - css = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, se2,true, Math.min(tec.getFootprintPolygon(robotID).getArea(),tec.getFootprintPolygon(secondRobotID).getArea())); - - } - }else { - css = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, se2,true, Math.min(tec.getFootprintPolygon(robotID).getArea(),tec.getFootprintPolygon(secondRobotID).getArea())); - cssMatrix[secondRobotIndex][secondTaskIndex][s] = css; - criticalSections.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID, cssMatrix); - //Evaluate the time to compute critical Section - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoComputeCriticalSection = timeRequiretoComputeCriticalSection + timeRequired; - fileStream1.println(timeRequired+""); - - } - - */ - - - css = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, se2,true, Math.min(tec.getFootprintPolygon(robotID).getArea(),tec.getFootprintPolygon(secondRobotID).getArea())); - cssMatrix[secondRobotIndex][secondTaskIndex][s] = css; - criticalSections.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID, cssMatrix); - //Evaluate the time to compute critical Section - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoComputeCriticalSection = timeRequiretoComputeCriticalSection + timeRequired; - fileStream1.println(timeRequired+""); - - - timeInitial2 = Calendar.getInstance().getTimeInMillis(); - //Compute the delay due to precedence constraint in Critical Section - for (int g = 0; g < css.length; g++) { - Pair a1 = estimateTimeToCompletionDelays(pss1.hashCode(),pss1,te1TCDelays,pss2.hashCode(),pss2,te2TCDelays, css[g]); - double delayCriticalSection = Math.min(a1.getFirst(), a1.getSecond()); - fileStream3.println(a1.getFirst() + " " + a1.getSecond() + " " + robotID + " " + secondRobotID+ " "); - if(delayCriticalSection < 0 ) { - delay += 0; - }else if(delayCriticalSection == Double.POSITIVE_INFINITY) { - delay += 10000; - }else { - delay += delayCriticalSection; - } - } - - - //Take the paths of driving robots from coordinator - pathsDrivingRobot = tec.getDrivingEnvelope(); - //Evaluate the delay time due to already driving robots - for(int k = 0; k < pathsDrivingRobot.size(); k++) { - CriticalSection [] cssDrivingRobot = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, pathsDrivingRobot.get(k),true, Math.min(tec.getFootprintPolygon(robotID).getArea(),tec.getFootprintPolygon(secondRobotID).getArea())); - for (int b = 0; b < cssDrivingRobot.length; b++) { - Pair a1 = estimateTimeToCompletionDelays(pss1.hashCode(),pss1,te1TCDelays,pathsDrivingRobot.get(k).getPath().hashCode(),pathsDrivingRobot.get(k).getPath(),te2TCDelays, cssDrivingRobot[b]); - double delayCriticalSection = Math.min(a1.getFirst(), a1.getSecond()); - if(delayCriticalSection < 0 ) { - delay += 0; - }else if(delayCriticalSection == Double.POSITIVE_INFINITY) { - delay += 10000; - }else { - delay += delayCriticalSection; - } - } - } - - - long timeFinal2 = Calendar.getInstance().getTimeInMillis(); - long timeRequired2 = timeFinal2- timeInitial2; - timeRequiretoComputePathsDelay = timeRequiretoComputePathsDelay + timeRequired2; - fileStream2.println(timeRequired2+""); - } - } - } - } - } - //fileStream3.println(" "); - - } else { //There also virtual robot and task are considered - //the delay associated to dummy robot and task is considerd 0 - return delay; - } - } - - //return the delay for the i-th robot and the j-th task due to interference with other robots - return delay; - } - - - - /** - * Compute the arrival time to a task for a specified robot - * @param pathLength -> The max path for each robot - * @return The time to drive the path - */ - private double computeArrivalTime(double pathLength,double vel,double acc){ - //Compute the arrival time of this path, considering a robot alone with a velocity trapezoidal model - double Pmax = Math.pow(vel, 2)/(2*acc); - double arrivalTime = 1; - if(pathLength > Pmax) { - //Use trapezoidal profile - arrivalTime = pathLength/vel + vel/acc; - }else { - arrivalTime = (2*pathLength)/vel; - } - - //Return the arrival time - return arrivalTime; - } - - /** - * Compute the arrival for all robots in fleet to all possible tasks - * @param pathLength -> The max path for each robot - * @return The time to drive the path - */ - private double [][][] computeArrivalTimeFleet(double[][][]PAll,AbstractTrajectoryEnvelopeCoordinator tec){ - //Compute the arrival time of this path, considering a robot alone with a velocity trapezoidal model - double [][][] arrivalTimeMatrix = new double [numRobotAug][numTaskAug][maxNumPaths]; - for (int robotID : IDsIdleRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsRealTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int path = 0;path < maxNumPaths; path++) { - double vel = tec.getRobot(robotID).getForwardModel().getVel(); - double acc = tec.getRobot(robotID).getForwardModel().getAcc(); - double arrivalTime = computeArrivalTime(PAll[i][j][path],vel,acc); - arrivalTimeMatrix[i][j][path] = arrivalTime; - } - - } - } - - //Return the arrival time - return arrivalTimeMatrix; - } - - - /** - * Evaluate the tardiness in completion of a task for a single robot . The tardiness is the defined as the further time required to complete a task - * after the deadline - * @param pathLength -> path length - * @param task -> the task j-th - * @return - */ - - private double computeTardiness(int robotID,int taskID,double pathLength, AbstractTrajectoryEnvelopeCoordinator tec) { - double tardiness = 0; - if(IDsRealTasks.contains(taskID)) { - int taskIndex = IDsRealTasks.indexOf(taskID); - if (taskQueue.get(taskIndex).isDeadlineSpecified()) { // Compute tardiness only if specified in task constructor - double deadline = taskQueue.get(taskIndex).getDeadline(); //Expressed in seconds - double vel = tec.getRobot(robotID).getForwardModel().getVel(); - double acc = tec.getRobot(robotID).getForwardModel().getVel(); - double completionTime = computeArrivalTime(pathLength,vel,acc) + taskQueue.get(taskIndex).getOperationTime(); - tardiness = Math.max(0, (completionTime-deadline)); - } - } - return tardiness; - } - - - /** - * Evaluate the tardiness in completion of a task . The tardiness is the defined as the further time required to complete a task - * after the deadline - * @param pathLength -> path length - * @param task -> the task j-th - * @return - */ - - private double[][][] computeTardinessFleet(double [][][]PAll,AbstractTrajectoryEnvelopeCoordinator tec) { - double tardiness = 0; - - double [][][] tardinessMatrix = new double [numRobotAug][numTaskAug][maxNumPaths]; - for (int robotID : IDsIdleRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsRealTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int path = 0;path < maxNumPaths; path++) { - if (taskQueue.get(j).isDeadlineSpecified()) { // Compute tardiness only if specified in task constructor - double deadline = taskQueue.get(j).getDeadline(); //Expressed in seconds - double vel = tec.getRobot(robotID).getForwardModel().getVel(); - double acc = tec.getRobot(robotID).getForwardModel().getVel(); - double completionTime = computeArrivalTime(PAll[i][j][path],vel,acc) + taskQueue.get(j).getOperationTime(); - tardiness = Math.max(0, (completionTime-deadline)); - tardinessMatrix[i][j][path] = tardiness; - sumTardiness += tardiness; - } - } - - } - } - return tardinessMatrix; - } - - /** - * Evaluate the overall B function, that is the function that consider interference free costs - * Costs considered: - * 1) Path Length - * 2) Tardiness - * Each cost is already normalized; - * @param PAll - * @return - */ - private double [][][] evaluateBFunction(double [][][]PAll,AbstractTrajectoryEnvelopeCoordinator tec){ - double [][][] tardinessMatrix = computeTardinessFleet(PAll,tec); - double [][][] BFunction = new double [numRobotAug][numTaskAug][maxNumPaths]; - costValuesMatrix = new double [numRobotAug][numTaskAug][maxNumPaths]; - if(linearWeight == 1) { - double [][][] arrivalTimeMatrix = computeArrivalTimeFleet(PAll,tec); - for (int i = 0 ; i < numRobotAug; i++) { - for (int j = 0 ; j < numTaskAug; j++) { - for(int path = 0;path < maxNumPaths; path++) { - BFunction[i][j][path] = pathLengthWeight*PAll[i][j][path]/sumMaxPathsLength+ tardinessWeight*tardinessMatrix[i][j][path]/sumTardiness + arrivalTimeWeight*arrivalTimeMatrix[i][j][path]/sumArrivalTime; - costValuesMatrix[i][j][path] = BFunction[i][j][path]; - //costValuesMatrix[i][j][path] = PAll[i][j][path]/sumMaxPathsLength+ tardinessMatrix[i][j][path]/sumTardiness + arrivalTimeMatrix[i][j][path]/sumArrivalTime; - } - - } - } - } - else { - for (int i = 0 ; i < numRobotAug; i++) { - for (int j = 0 ; j < numTaskAug; j++) { - for(int path = 0;path < maxNumPaths; path++) { - BFunction[i][j][path] = pathLengthWeight*PAll[i][j][path]/sumMaxPathsLength+ tardinessWeight*tardinessMatrix[i][j][path]/sumTardiness; - //costValuesMatrix[i][j][path] = PAll[i][j][path]/sumMaxPathsLength+ tardinessMatrix[i][j][path]/sumTardiness; - costValuesMatrix[i][j][path] = BFunction[i][j][path] ; - } - - - - } - } - } - - return BFunction; - } - - /** - * Evaluates the number of all feasible solutions for an optimization problem, with is defined with {@link #buildOptimizationProblem} - * @param numRobot -> Number of Robot of the optimization problem - * @param numTasks -> Number of Tasks of the optimization problem - * @return The number of all feasible solutions for the Optimization Problem - */ - - public int numberFeasibleSolution(int numRobot,int numTasks){ - //Create an optimization problem - MPSolver optimizationProblemCopy = buildOptimizationProblem(numRobot,numTasks); - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblemCopy.solve(); - int numberFeasibleSolution = 0; - while(resultStatus != MPSolver.ResultStatus.INFEASIBLE ) { - //Solve the optimization Problem - resultStatus = optimizationProblemCopy.solve(); - //If The solution is feasible increment the number of feasible solution - if (resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - numberFeasibleSolution = numberFeasibleSolution+1; - } - double [][][] assignmentMatrix = saveAssignmentMatrix(numRobotAug,numTaskAug,optimizationProblemCopy); - //Add the constraint to actual solution -> in order to consider this solution as already found - optimizationProblemCopy = constraintOnPreviousSolution(optimizationProblemCopy,assignmentMatrix); - } - //Return the Total number of feasible solution - optimizationProblemCopy.clear(); - return numberFeasibleSolution; - } - - - /** - * Evaluates all possible feasible solutions for an optimization problem,with is defined with {@link #buildOptimizationProblem}. A feasible solution is a solution that verify constraints - * @param numRobot -> Number of Robots - * @param numTasks -> Number of Tasks - * @return A set containing all feasible solutions - */ - public double [][][][] evaluateFeasibleSolution(int numRobot,int numTasks){ - //Define the optimization problem - MPSolver optimizationProblemCopy = buildOptimizationProblem(numRobot,numTasks); - //Evaluate the number of all feasible solution for the optimization problem - int feasibleSolutions = numberFeasibleSolution(numRobot,numTasks); - //Initialize a set to store all feasible solution - double [][][][] AssignmentMatrixOptimalSolutions = new double [feasibleSolutions][numRobot][numTasks][maxNumPaths]; - /////////////////////////////////////// - for(int k=0; k < feasibleSolutions; k++) { - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblemCopy.solve(); - //Transform the Assignment Vector to Matrix - double [][][] AssignmentMatrix = saveAssignmentMatrix(numRobot,numTasks,optimizationProblemCopy); - //Store the optimal solution - AssignmentMatrixOptimalSolutions[k]=AssignmentMatrix; - //Add the constraint to actual solution in order to consider this solution as already found - optimizationProblemCopy = constraintOnPreviousSolution(optimizationProblemCopy,AssignmentMatrix); - } - //Return the set of all Feasible solutions - return AssignmentMatrixOptimalSolutions; - } - - - - /** - * Builds the optimization problem. Define a decision variable X_ij as a binary variable in which i indicate - * the robot id, j the tasks. Also constraints are defined: - * the constraints considered are : - * 1) Each Task can be assign only to a robot; - * 2) Each Robot can perform only a task at time; - * @param numRobot -> Number of Robots - * @param numTasksAug -> Number of Tasks. - * @return A constrained optimization problem without the objective function - */ - private MPSolver buildOptimizationProblem(int numRobotAug,int numTasksAug) { - //Initialize a linear solver - - MPSolver optimizationProblem = new MPSolver( - "TaskAssignment", MPSolver.OptimizationProblemType.CBC_MIXED_INTEGER_PROGRAMMING); - //START DECISION VARIABLE VARIABLE - MPVariable [][][] decisionVariable = new MPVariable[numRobotAug][numTasksAug][maxNumPaths]; - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTasksAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - decisionVariable[i][j][s] = optimizationProblem.makeBoolVar("x"+"["+i+","+j+","+s+"]"); - } - - } - } - //END DECISION VARIABLE - ////////////////////////// - // START CONSTRAINTS - //Each Robot can be assign only to a Task - for (int i = 0; i < numRobotAug; i++) { - //Initialize the constraint - //MPConstraint c0 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY, 1); - MPConstraint c0 = optimizationProblem.makeConstraint(1, 1); - for (int j = 0; j < numTasksAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j][s], 1); - } - - } - } - - //Each task can be performed only by a robot - for (int j = 0; j < numTasksAug; j++) { - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(1, 1); - for (int i = 0; i < numRobotAug; i++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j][s], 1); - } - } - } - - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - if (i < numRobot) { //Considering only real Robot - PoseSteering[] pss = pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s); - if(pss==null) { - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j][s],1); - } - } - } - } - } - - //END CONSTRAINTS - //In case of having more task than robots, the task with a closest deadline are set with a higher priority - if(taskQueue.size() > IDsIdleRobots.size()) { - checkOnTaskDeadline(); - //Each task can be performed only by a robot - for (int j = 0; j < taskQueue.size(); j++) { - //Initialize the constraint - if(taskQueue.get(j).isPriority()) { - MPConstraint c3 = optimizationProblem.makeConstraint(1, 1); - for (int i = 0; i < IDsIdleRobots.size(); i++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c3.setCoefficient(decisionVariable[i][j][s], 1); - } - } - } - } - } - ///////////////////////////////////////////////// - return optimizationProblem; - } - - - - /** - * Builds the optimization problem complete with Objective Function. Define a decision variable X_ij as a binary variable in which i indicate - * the robot id, j the tasks. Also constraints are defined: - * the constraints considered are : - * 1) Each Task can be assign only to a robot; - * 2) Each Robot can perform only a task at time. - * The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m) - * with n = number of robot and m = number of tasks. - * Only the B function is considered in this case, and each cost is normalized with the max path length considering - * all missions. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return A constrained optimization problem with the objective function and each cost is normalized - */ - public MPSolver buildOptimizationProblemWithBNormalized(AbstractTrajectoryEnvelopeCoordinator tec) { - this.initialTime = Calendar.getInstance().getTimeInMillis(); - - - //Perform a check in order to avoid blocking - //checkOnBlocking(tec); - //Take the number of tasks - numTask = taskQueue.size(); - //Get free robots and their IDs - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - getAllRobotIDs(); - getAllTaskIDs(); - double[][][] PAll = evaluatePAll(tec); - double[][][] BFunction = evaluateBFunction(PAll,tec); - //Build the solver and an objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobotAug,numTaskAug); - - MPVariable [][][] decisionVariable = tranformArray(optimizationProblem); - ///////////////////////////////// - //START OBJECTIVE FUNCTION - MPObjective objective = optimizationProblem.objective(); - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - double pathLength = BFunction[i][j][s]; - if ( pathLength != MaxPathLength) { - //Set the coefficient of the objective function with the normalized path length - objective.setCoefficient(decisionVariable[i][j][s], pathLength); - }else { // if the path does not exists or the robot type is different from the task type - //the path to reach the task not exists - //the decision variable is set to 0 -> this allocation is not valid - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j][s],1); - } - } - } - } - //Define the problem as a minimization problem - objective.setMinimization(); - //END OBJECTIVE FUNCTION - return optimizationProblem; - } - - - /** - * Solve the optimization problem given as input considering both B and F Functions. The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m). - * with n = number of robot and m = number of tasks. The solver first finds the optimal solution considering only B function and then - * for this each solution (that is an assignment) evaluates the cost of F function. Then a new optimal solution considering only B is - * computed and it is consider only if the cost of this new assignment considering only B is less than the min cost of previous assignments - * considering both F and B function - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblemWithB} - * @param tec -> an Abstract Trajectory Envelope Coordinator - - * @return An Optimal Assignment that minimize the objective function - */ - - public double [][][] solveOptimizationProblem(MPSolver optimizationProblem,AbstractTrajectoryEnvelopeCoordinator tec){ - - - PrintStream fileStream = null; - PrintStream fileStream1 = null; - PrintStream fileStream2 = null; - PrintStream fileStream3 = null; - PrintStream fileStream4 = null; - PrintStream fileStream5 = null; - PrintStream fileStream6 = null; - PrintStream fileStream7 = null; - PrintStream fileStream8 = null; - PrintStream fileStream9 = null; - PrintStream fileStream10 = null; - PrintStream fileStream11 = null; - - int a = tec.getTestNumber(); - int b = tec.getFolderNumber(); - - /* - String ppRequiredTime = "Test1/Sys/RequiredTime-T" + a +".txt"; - String ppCriticalSections = "Test1/Sys/CriticalSections-T" + a +".txt"; - String ppPathDelay = "Test1/Sys/PathDelay-T" + a +".txt"; - String ppCostOptimalSolution = "Test1/Sys/CostOptimalSolution-T" + a +".txt"; - String ppAssignMatrix = "Test1/Sys/AssignMatrix-T" + a +".txt"; - String ppTotalTime = "Test1/Sys/TotalTime-T" + a +".txt"; - String ppOptimizationProblem = "Test1/Sys/OptimizationProblem-T" + a +".txt"; - String ppAnalysisProblem = "Test1/Sys/AnalysisProblem-T" + a +".txt"; - String ppAssignMatrix2 = "Test1/Sys/AssignMatrix2-T" + a +".txt"; - String ppCostObjectiveFunction = "Test1/Sys/CostObjectiveFunction-T" + a +".txt"; - - */ - - String ppRequiredTime = "Test"+b+"/Sys/RequiredTime-T" + a +".txt"; - String ppCriticalSections = "Test"+b+"/Sys/CriticalSections-T" + a +".txt"; - String ppPathDelay = "Test"+b+"/Sys/PathDelay-T" + a +".txt"; - String ppCostOptimalSolution = "Test"+b+"/Sys/CostOptimalSolution-T" + a +".txt"; - String ppAssignMatrix = "Test"+b+"/Sys/AssignMatrix-T" + a +".txt"; - String ppTotalTime = "Test"+b+"/Sys/TotalTime-T" + a +".txt"; - String ppOptimizationProblem = "Test"+b+"/Sys/OptimizationProblem-T" + a +".txt"; - String ppAnalysisProblem = "Test"+b+"/Sys/AnalysisProblem-T" + a +".txt"; - String ppAssignMatrix2 = "Test"+b+"/Sys/AssignMatrix2-T" + a +".txt"; - String ppCostObjectiveFunction = "Test"+b+"/Sys/CostObjectiveFunction-T" + a +".txt"; - String ppCostObjectiveFunctionOptimal = "Test"+b+"/Sys/CostObjectiveFunctionOptimal-T" + a +".txt"; - - String ppDelayTime = "Test"+ b +"/Sys/DelayTime-T" + a +".txt"; - - try { - - /* - fileStream = new PrintStream((new FileOutputStream("Test1/Sys/RequiredTime.txt",true))); - fileStream1 = new PrintStream(new File("Test1/Sys/CriticalSections.txt")); - PrintStream fileStream2 = new PrintStream(new File("Test1/Sys/PathDelay.txt")); - fileStream3 = new PrintStream(new File("Test1/Sys/CostOptimalSolution.txt")); - fileStream4 = new PrintStream(new FileOutputStream("Test1/Sys/AssignMatrix.txt",true)); - fileStream5 = new PrintStream(new FileOutputStream("Test1/Sys/TotalTime.txt",true)); - - //fileStream5 = new PrintStream(new File("TotalTime.txt")); - fileStream6 = new PrintStream(new File("Test1/Sys/OptimizationProblem.txt")); - fileStream7 = new PrintStream(new FileOutputStream("Test1/Sys/AnalysisProblem.txt",true)); - fileStream8 = new PrintStream(new FileOutputStream("Test1/Sys/AssignMatrix2.txt",true)); - fileStream9 = new PrintStream(new FileOutputStream("Test1/Sys/CostObjectiveFunction.txt",true)); - */ - - fileStream = new PrintStream((new FileOutputStream(ppRequiredTime,true))); - fileStream1 = new PrintStream(new FileOutputStream(ppCriticalSections,true)); - fileStream2 = new PrintStream(new File(ppPathDelay)); - fileStream3 = new PrintStream(new File(ppCostOptimalSolution)); - fileStream4 = new PrintStream(new FileOutputStream(ppAssignMatrix,true)); - fileStream5 = new PrintStream(new FileOutputStream(ppTotalTime,true)); - - //fileStream5 = new PrintStream(new File("TotalTime.txt")); - fileStream6 = new PrintStream(new File(ppOptimizationProblem)); - fileStream7 = new PrintStream(new FileOutputStream(ppAnalysisProblem,true)); - fileStream8 = new PrintStream(new FileOutputStream(ppAssignMatrix2,true)); - fileStream9 = new PrintStream(new FileOutputStream(ppCostObjectiveFunction,true)); - fileStream10 = new PrintStream(new FileOutputStream(ppCostObjectiveFunctionOptimal,true)); - fileStream11 = new PrintStream(new File(ppDelayTime)); - - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - long timeProva2= 0; - long timeProvaFinal2= 0; - - //Initialize the optimal assignment and the cost associated to it - double [][][] optimalAssignmentMatrix = new double[numRobotAug][numTaskAug][maxNumPaths]; - double objectiveOptimalValue = 100000000; - double costBOptimal = 0; - double costFOptimal = 0; - - fileStream6.println(optimizationProblem.numConstraints()+ ""); - //Solve the optimization problem - fileStream1.println(sumArrivalTime+""); - fileStream1.println(""); - if(ScenarioAllocation != null) { - fileStream7.println("Tasks>> "+ IDsRealTasks.size() + " Robot>> " + IDsIdleRobots.size() ); - double costofAssignmentFake = 0; - double costBFake = 0; - double costFFake = 0; - double costFFake2 = 0; - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - //fileStream7.println("lenMAtrix>> "+costValuesMatrix.length + "lenmatrix2>>" + costValuesMatrix[0].length); - //fileStream7.println("robotID>> "+ robotID+ " taskID>> " + taskID ); - //fileStream7.println("len>> "+ IDsAllRobots.size()+ " len2>> " + IDsAllTasks.size() ); - if ( ScenarioAllocation[i][j][s] > 0) { - if (linearWeight != 1) { - //Evaluate cost of F function only if alpha is not equal to 1 - double costB = optimizationProblem.objective().getCoefficient(optimizationProblem.variables()[i*numTaskAug*maxNumPaths+j*maxNumPaths+s]); - costBFake = linearWeight*costB + costBFake; - costFFake = evaluatePathDelay(robotID,taskID,s,ScenarioAllocation,tec)/sumArrivalTime; - costFFake2 = (1-linearWeight)*costFFake + costFFake2; - costofAssignmentFake = linearWeight*costB + (1-linearWeight)*costFFake + costofAssignmentFake ; - //fileStream1.println(costFFake+""); - } - else { - double costB = optimizationProblem.objective().getCoefficient(optimizationProblem.variables()[i*numTaskAug*maxNumPaths+j*maxNumPaths+s]); - costofAssignmentFake = Math.pow(linearWeight*costB, 2) + costofAssignmentFake ; - - - } - - - } - } - - } - } - fileStream10.println(costBFake + " " + costFFake2 ); - fileStream1.println(costFFake2+" TOTAL"); - fileStream1.println(" "); - fileStream9.println(costofAssignmentFake); - return this.ScenarioAllocation; - } - - - - fileStream7.println("Tasks>> "+ IDsRealTasks.size() + " Robot>> " + IDsIdleRobots.size() ); - MPSolver.ResultStatus resultStatus = optimizationProblem.solve(); - long timeOffsetInitial = Calendar.getInstance().getTimeInMillis(); - long timeOffset = 0; - int cont = 1; - while(resultStatus != MPSolver.ResultStatus.INFEASIBLE && timeOffset < timeOut) { - //Evaluate an optimal assignment that minimize only the B function - timeProva2 = Calendar.getInstance().getTimeInMillis(); - resultStatus = optimizationProblem.solve(); - timeProvaFinal2 = Calendar.getInstance().getTimeInMillis(); - long timeRequiredProva = timeProvaFinal2- timeProva2; - //Evaluate the Assignment Matrix - double [][][] AssignmentMatrix = saveAssignmentMatrix(numRobotAug,numTaskAug,optimizationProblem); - //Initialize cost of objective value - double costofAssignment = 0; - double costofAssignmentForConstraint = 0; - double costF = 0; - //Evaluate the cost of F Function for this Assignment - timeRequiretoComputeCriticalSection = 0; - timeRequiretoComputePathsDelay = 0; - double costFFake3 = 0; - double costBFake3 = 0; - //Take time to understand how much time require this function - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - if ( AssignmentMatrix[i][j][s] > 0) { - - if (linearWeight != 1) { - //Evaluate cost of F function only if alpha is not equal to 1 - double costB = optimizationProblem.objective().getCoefficient(optimizationProblem.variables()[i*numTaskAug*maxNumPaths+j*maxNumPaths+s]); - //fileStream11.println(" "); - costF = evaluatePathDelay(robotID,taskID,s,AssignmentMatrix,tec)/sumArrivalTime; - costofAssignment = linearWeight*costB + (1-linearWeight)*costF + costofAssignment ; - costofAssignmentForConstraint = costValuesMatrix[i][j][s] + costF + costofAssignmentForConstraint; - fileStream2.println(costF+" " + costB+ " "); - costFFake3 = (1-linearWeight)*costF + costFFake3; - costBFake3 = linearWeight*costB + costBFake3; - - - } - else { - double costB = optimizationProblem.objective().getCoefficient(optimizationProblem.variables()[i*numTaskAug*maxNumPaths+j*maxNumPaths+s]); - costofAssignment = Math.pow(linearWeight*costB, 2) + costofAssignment ; - costofAssignmentForConstraint = costValuesMatrix[i][j][s] + costofAssignmentForConstraint; - } - - - } - } - - } - } - fileStream2.println(costFFake3 +" TOTAL F " + costBFake3 + " TOTAL B "); - fileStream2.println(" "); - //fileStream11.println("-------- New Assignment --------------"); - fileStream3.println(optimizationProblem.objective().value()+""); - fileStream3.println((costofAssignmentForConstraint-optimizationProblem.objective().value())+""); - fileStream3.println(costofAssignmentForConstraint+"CostConstraint"); - //fileStream3.println(optimizationProblem.objective().value()+" Cost B"); - //fileStream3.println((costofAssignmentForConstraint-optimizationProblem.objective().value())+" Cost F"); - //fileStream3.println(costofAssignmentForConstraint+" Cost Assignment"); - fileStream.println(timeRequiretoEvaluatePaths+""); - fileStream.println(timeRequiretofillInPall+""); - fileStream.println(timeRequiretoComputeCriticalSection+""); - fileStream.println(timeRequiretoComputePathsDelay+""); - fileStream.println(timeRequiredProva+""); - - fileStream4.println("-------------------"); - fileStream4.println(costofAssignment+""); - fileStream4.println("-------------------"); - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j > "+costValuesMatrix.length + "lenmatrix2>>" + costValuesMatrix[0].length); - //fileStream7.println("robotID>> "+ robotID+ " taskID>> " + taskID ); - //fileStream7.println("len>> "+ IDsAllRobots.size()+ " len2>> " + IDsAllTasks.size() ); - if ( optimalAssignmentMatrix[i][j][s] > 0) { - if (linearWeight != 1) { - //Evaluate cost of F function only if alpha is not equal to 1 - double costB = optimizationProblem.objective().getCoefficient(optimizationProblem.variables()[i*numTaskAug*maxNumPaths+j*maxNumPaths+s]); - costBFake = linearWeight*costB + costBFake; - costFFake = evaluatePathDelay(robotID,taskID,s,optimalAssignmentMatrix,tec)/sumArrivalTime; - costFFake2 = (1-linearWeight)*costFFake + costFFake2; - costofAssignmentFake = linearWeight*costB + (1-linearWeight)*costFFake + costofAssignmentFake ; - fileStream1.println(costFFake+""); - } - else { - double costB = optimizationProblem.objective().getCoefficient(optimizationProblem.variables()[i*numTaskAug*maxNumPaths+j*maxNumPaths+s]); - costofAssignmentFake = Math.pow(linearWeight*costB, 2) + costofAssignmentFake ; - - } - - - } - } - - } - } - fileStream10.println(costBFake + " " + costFFake2 ); - fileStream1.println(costofAssignmentFake +" TOTAL"); - fileStream1.println(" "); - fileStream9.println(costofAssignmentFake); - ////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////// - - - fileStream6.println(cont+ ""); - fileStream8.println(""); - //Return the Optimal Assignment Matrix - String ppMatrixOptimal = "Test"+b+"/Sys/MatrixOptimal-T" + a +".txt"; - writeMatrix(ppMatrixOptimal,optimalAssignmentMatrix); - this.ScenarioAllocation = null; - this.scenario = null; - readyForNextAssignment = false; - return optimalAssignmentMatrix; - } - - - /** - * Solve the optimization problem given as input considering both B and F Functions with a Local Search Algorithm. The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m). - * with n = number of robot and m = number of tasks. Starting from a feasible solution, all his neighbors are computed; then the new solution - * is the optimal solutions considering the neighbors. The algorithm go on until the specified number of iterations - * @param tec -> an Abstract Trajectory Envelope Coordinator - * @param iteration -> number of iteration to consider ( if -1 all feasible solution are considered) - * @return An Optimal Assignment that minimize the objective function - */ - - public double [][][] solveOptimizationProblemLocalSearch(AbstractTrajectoryEnvelopeCoordinator tec,int iteration){ - long initialTime = Calendar.getInstance().getTimeInMillis(); - PrintStream fileStream = null; - PrintStream fileStream1 = null; - PrintStream fileStream3 = null; - PrintStream fileStream4 = null; - PrintStream fileStream5 = null; - PrintStream fileStream8 = null; - PrintStream fileStream7 = null; - PrintStream fileStream9 = null; - - int a = tec.getTestNumber(); - int b = tec.getFolderNumber(); - String ppRequiredTime = "Test"+b+"/GA/RequiredTime-T" + a +".txt"; - String ppCriticalSections = "Test"+b+"/GA/CriticalSections-T" + a +".txt"; - String ppPathDelay = "Test"+b+"/GA/PathDelay-T" + a +".txt"; - String ppCostOptimalSolution = "Test"+b+"/GA/CostOptimalSolution-T" + a +".txt"; - String ppAssignMatrix = "Test"+b+"/GA/AssignMatrix-T" + a +".txt"; - String ppTotalTime = "Test"+b+"/GA/TotalTime-T" + a +".txt"; - String ppAnalysisProblem = "Test"+b+"/GA/AnalysisProblem-T" + a +".txt"; - String ppAssignMatrix2 = "Test"+b+"/GA/AssignMatrix2-T" + a +".txt"; - String ppCostObjectiveFunction = "Test"+b+"/GA/CostObjectiveFunction-T" + a +".txt"; - - try { - /* - fileStream = new PrintStream((new FileOutputStream("Test1/GA/RequiredTime.txt",true))); - fileStream1 = new PrintStream(new File("Test1/GA/CriticalSections.txt")); - PrintStream fileStream2 = new PrintStream(new File("Test1/GA/PathDelay.txt")); - fileStream3 = new PrintStream(new File("Test1/GA/CostOptimalSolution.txt")); - fileStream4 = new PrintStream(new FileOutputStream("Test1/GA/AssignMatrix.txt",true)); - fileStream5 = new PrintStream(new FileOutputStream("Test1/GA/TotalTime.txt",true)); - fileStream8 = new PrintStream(new FileOutputStream("Test1/GA/AssignMatrix2.txt",true)); - fileStream7 = new PrintStream(new FileOutputStream("Test1/GA/AnalysisProblem.txt",true)); - fileStream9 = new PrintStream(new FileOutputStream("Test1/GA/CostObjectiveFunction.txt",true)); - */ - fileStream = new PrintStream((new FileOutputStream(ppRequiredTime,true))); - fileStream1 = new PrintStream(new File(ppCriticalSections)); - PrintStream fileStream2 = new PrintStream(new File(ppPathDelay)); - fileStream3 = new PrintStream(new File(ppCostOptimalSolution)); - fileStream4 = new PrintStream(new FileOutputStream(ppAssignMatrix,true)); - fileStream5 = new PrintStream(new FileOutputStream(ppTotalTime,true)); - - //fileStream5 = new PrintStream(new File("TotalTime.txt")); - fileStream7 = new PrintStream(new FileOutputStream(ppAnalysisProblem,true)); - fileStream8 = new PrintStream(new FileOutputStream(ppAssignMatrix2,true)); - fileStream9 = new PrintStream(new FileOutputStream(ppCostObjectiveFunction,true)); - - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - ArrayList IDsAllRobotsNew = new ArrayList (); - numTask = taskQueue.size(); - //Get free robots - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - getAllRobotIDs(); - getAllTaskIDs(); - //Consider possibility to have dummy Robot or Tasks - double [][][] PAll = evaluatePAll(tec); - double[][][] BFunction = evaluateBFunction(PAll,tec); - - //Build the optimization Problem without the objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobotAug,numTaskAug); - MPObjective objective = optimizationProblem.objective(); - objective.setMinimization(); - //Initialize the optimal assignment and the cost associated to it - double [][][] optimalAssignmentMatrix = new double[numRobotAug][numTaskAug][maxNumPaths]; - double objectiveOptimalValue = Double.POSITIVE_INFINITY; - double costBOptimal = 0; - double costFOptimal = 0; - //Solve the optimization problem - optimizationProblem.solve(); - double [][][] AssignmentMatrix = saveAssignmentMatrix(numRobotAug,numTaskAug,optimizationProblem); - int prova1 = 0; - int prova2 = 0; - - - int numIteration = 1; - for(int fat= 1 ; fat <= numRobotAug; fat++) { - numIteration *= fat; - } - if(iteration > numIteration || iteration == -1) { - iteration = numIteration; - }else { - numIteration = iteration; - } - int index1i = 0; - int index1j = 0; - int index1s = 0; - int index2i = 0; - int index2j = 0; - int index2s = 0; - double [][][] newAssignmentMatrix = new double[numRobotAug][numTaskAug][maxNumPaths]; - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j IDsRandomRobots = new ArrayList (); - ArrayList IDsRandomRobots2 = new ArrayList (); - IDsRandomRobots.addAll(IDsAllRobots); - IDsAllRobotsNew.addAll(IDsAllRobots); - - //IDsRandomRobots2.addAll(IDsAllRobots); - int ind = (int) Math.floor(Math.random()*IDsRandomRobots.size()); - prova1 = IDsRandomRobots.get(ind); - - //Take the robots of the same types of robot 1 - for(int m: IDsAllRobots) { - if(IDsIdleRobots.contains(m) && tec.getRobot(prova1)!=null){ - tec.getRobot(m).getRobotType(); - tec.getRobot(prova1).getRobotType(); - if(tec.getRobot(m).getRobotType() == tec.getRobot(prova1).getRobotType() ) { - IDsRandomRobots2.add(m); - - } - }else {//virtual robots - IDsRandomRobots2.add(m); - } - } - int index = IDsRandomRobots2.indexOf(prova1); - IDsRandomRobots2.remove(index); - fileStream4.println("Remove-------------------"+ prova1 + "--index--"+ index); - if(ScenarioAllocation!= null) { - fileStream7.println("Tasks>> "+ IDsRealTasks.size() + " Robot>> " + IDsIdleRobots.size() ); - return this.ScenarioAllocation; - } - - prova1 = IDsRandomRobots.get(ind); - IDsRandomRobots.remove(ind); - - long timeOffsetInitial = Calendar.getInstance().getTimeInMillis(); - long timeOffset = 0; - - fileStream7.println("Tasks>> "+ IDsRealTasks.size() + " Robot>> " + IDsIdleRobots.size() ); - //IDsRandomRobots.remove(ind); - for(int k=0; k < iteration ;k++){ - //Evaluate an optimal assignment that minimize only the B function - //Initialize cost of objective value - if(timeOffset < timeOut) { - double costofAssignment = 0; - double costofAssignmentForConstraint = 0; - double costBFunction = 0; - double costFFunction = 0; - if(k>0) { - fileStream4.println("--"+ numIteration); - fileStream4.println("--"+ k); - if(IDsRandomRobots2.size() == 0) { - //prova1 += 1; - if(IDsRandomRobots.size()==0) { - IDsRandomRobots.addAll(IDsAllRobots); - } - - if(IDsRandomRobots.size()>0) { - ind = (int) Math.floor(Math.random()*IDsRandomRobots.size()); - prova1 = IDsRandomRobots.get(ind); - IDsRandomRobots.remove(ind); - }else { - prova1 =0; - } - //IDsRandomRobots2.addAll(IDsAllRobots); - - for(int m: IDsAllRobots) { - if(IDsIdleRobots.contains(m) && tec.getRobot(prova1)!=null){ - - if(tec.getRobot(m).getRobotType() == tec.getRobot(prova1).getRobotType()) { - IDsRandomRobots2.add(m); - - } - - - }else {//virtual robots - IDsRandomRobots2.add(m); - } - } - index = IDsRandomRobots2.indexOf(prova1); - IDsRandomRobots2.remove(index); - - - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j 0) { - prova2 = IDsRandomRobots2.get(ind2); - IDsRandomRobots2.remove(ind2); - } - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j 0) { - fileStream4.println("-------------------"); - fileStream4.println("Solution not already found" + solutionAlreadyFound); - fileStream4.println("Switch" + prova1 + "--" + prova2 ); - fileStream4.println("-------------------"); - } - //Evaluate the cost of F Function for this Assignment - timeRequiretoComputeCriticalSection = 0; - timeRequiretoComputePathsDelay = 0; - //Take time to understand how much time require this function - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - if ( newAssignmentMatrix[i][j][s] > 0) { - if (linearWeight != 1) { - //Evaluate cost of F function only if alpha is not equal to 1 - costBFunction = BFunction[i][j][s]; - costFFunction = evaluatePathDelay(robotID,taskID,s,newAssignmentMatrix,tec)/sumArrivalTime; - costofAssignment = linearWeight*costBFunction + (1-linearWeight)*costFFunction + costofAssignment ; - costofAssignmentForConstraint = costValuesMatrix[i][j][s] + costFFunction + costofAssignmentForConstraint; - } - else { - costBFunction= BFunction[i][j][s]; - costofAssignment = Math.pow(linearWeight*costBFunction, 2) + costofAssignment ; - costofAssignmentForConstraint = costValuesMatrix[i][j][s] + costofAssignmentForConstraint; - } - - } - matrixSolutionFound[k][i][j][s] = newAssignmentMatrix[i][j][s]; - } - - } - } - costSolutions[k] = costofAssignment; - }else { - fileStream4.println("-------------------"); - fileStream4.println("Solution already found" + solutionAlreadyFound + "--" + indexSolutionFound); - fileStream4.println("Switch was" + prova1 + "--" + prova2 ); - fileStream4.println("-------------------"); - costofAssignment = costSolutions[indexSolutionFound]; - } - - fileStream4.println("-------------------"); - fileStream4.println(costofAssignment+""); - fileStream4.println("-------------------"); - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j TrajectoryEnvelopeCoordinatorSimulation - * @param alpha -> the linear parameter used to weights B and F function expressed in percent( 1-> 100%). The objective function is - * considered as B*alpha + (1 - alpha)*F - * @return The Optimal Assignment that minimize the objective function - */ - - public double [][][] solveOptimizationProblemExactAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec,double alpha){ - long initialTime = Calendar.getInstance().getTimeInMillis(); - - - PrintStream fileStream = null; - PrintStream fileStream1 = null; - PrintStream fileStream3 = null; - PrintStream fileStream4 = null; - PrintStream fileStream5 = null; - PrintStream fileStream8 = null; - PrintStream fileStream7 = null; - PrintStream fileStream9 = null; - - - int a = tec.getTestNumber(); - int b = tec.getFolderNumber(); - String ppRequiredTime = "Test"+b+"/G/RequiredTime-T" + a +".txt"; - String ppCriticalSections = "Test"+b+"/G/CriticalSections-T" + a +".txt"; - String ppPathDelay = "Test"+b+"/G/PathDelay-T" + a +".txt"; - String ppCostOptimalSolution = "Test"+b+"/G/CostOptimalSolution-T" + a +".txt"; - String ppAssignMatrix = "Test"+b+"/G/AssignMatrix-T" + a +".txt"; - String ppTotalTime = "Test"+b+"/G/TotalTime-T" + a +".txt"; - String ppAnalysisProblem = "Test"+b+"/G/AnalysisProblem-T" + a +".txt"; - String ppAssignMatrix2 = "Test"+b+"/G/AssignMatrix2-T" + a +".txt"; - String ppCostObjectiveFunction = "Test"+b+"/G/CostObjectiveFunction-T" + a +".txt"; - - try { - /* - fileStream = new PrintStream((new FileOutputStream("Test1/GA/RequiredTime.txt",true))); - fileStream1 = new PrintStream(new File("Test1/GA/CriticalSections.txt")); - PrintStream fileStream2 = new PrintStream(new File("Test1/GA/PathDelay.txt")); - fileStream3 = new PrintStream(new File("Test1/GA/CostOptimalSolution.txt")); - fileStream4 = new PrintStream(new FileOutputStream("Test1/GA/AssignMatrix.txt",true)); - fileStream5 = new PrintStream(new FileOutputStream("Test1/GA/TotalTime.txt",true)); - fileStream8 = new PrintStream(new FileOutputStream("Test1/GA/AssignMatrix2.txt",true)); - fileStream7 = new PrintStream(new FileOutputStream("Test1/GA/AnalysisProblem.txt",true)); - fileStream9 = new PrintStream(new FileOutputStream("Test1/GA/CostObjectiveFunction.txt",true)); - */ - fileStream = new PrintStream((new FileOutputStream(ppRequiredTime,true))); - fileStream1 = new PrintStream(new File(ppCriticalSections)); - PrintStream fileStream2 = new PrintStream(new File(ppPathDelay)); - fileStream3 = new PrintStream(new File(ppCostOptimalSolution)); - fileStream4 = new PrintStream(new FileOutputStream(ppAssignMatrix,true)); - fileStream5 = new PrintStream(new FileOutputStream(ppTotalTime,true)); - - //fileStream5 = new PrintStream(new File("TotalTime.txt")); - fileStream7 = new PrintStream(new FileOutputStream(ppAnalysisProblem,true)); - fileStream8 = new PrintStream(new FileOutputStream(ppAssignMatrix2,true)); - fileStream9 = new PrintStream(new FileOutputStream(ppCostObjectiveFunction,true)); - - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - - - - numTask = taskQueue.size(); - //Get free robots - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - getAllRobotIDs(); - getAllTaskIDs(); - //Consider possibility to have dummy Robot or Tasks - double [][][] PAll = evaluatePAll(tec); - double[][][] BFunction = evaluateBFunction(PAll,tec); - //Build the optimization Problem without the objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobotAug,numTaskAug); - MPObjective objective = optimizationProblem.objective(); - objective.setMinimization(); - //Initialize the optimal assignment and the cost associated to it - double [][][] optimalAssignmentMatrix = new double[numRobotAug][numTaskAug][maxNumPaths]; - double objectiveOptimalValue = 100000000; - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblem.solve(); - while(resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - //Evaluate a feasible assignment - resultStatus = optimizationProblem.solve(); - //Evaluate the Assignment Matrix - double [][][] AssignmentMatrix = saveAssignmentMatrix(numRobotAug,numTaskAug,optimizationProblem); - //Initialize cost of objective value - double objectiveFunctionValue = 0; - double costBFunction = 0; - double costFFunction = 0; - //Evaluate the cost for this Assignment - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - if (AssignmentMatrix[i][j][s] >0) { - costBFunction = costBFunction + BFunction[i][j][s]; - //System.out.println("COSTB " + BFunction[i][j][s] + "robotID>> " + robotID + " taskID>> " + taskID); - - if (alpha != 1) { - costFFunction = costFFunction + evaluatePathDelay(robotID,taskID,s,AssignmentMatrix,tec)/sumArrivalTime; - - } - } - //System.out.println("COSTB " + AssignmentMatrix[i][j][s] + "robotID>> " + robotID + " taskID>> " + taskID); - } - - } - } - fileStream.println(timeRequiretoEvaluatePaths+""); - fileStream.println(timeRequiretofillInPall+""); - fileStream.println(timeRequiretoComputeCriticalSection+""); - fileStream.println(timeRequiretoComputePathsDelay+""); - - objectiveFunctionValue = alpha * costBFunction + (1-alpha)*costFFunction; - - //Compare actual solution and optimal solution finds so far - if (objectiveFunctionValue < objectiveOptimalValue && resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - objectiveOptimalValue = objectiveFunctionValue; - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j TrajectoryEnvelopeCoordinatorSimulation - * considered as B*alpha + (1 - alpha)*F - * @return The Optimal Assignment that minimize the objective function - */ - - public double [][][] solveOptimizationProblemGreedyAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec){ - long initialTime = Calendar.getInstance().getTimeInMillis(); - - PrintStream fileStream = null; - PrintStream fileStream1 = null; - PrintStream fileStream3 = null; - PrintStream fileStream4 = null; - PrintStream fileStream5 = null; - PrintStream fileStream8 = null; - PrintStream fileStream7 = null; - PrintStream fileStream9 = null; - - int a = tec.getTestNumber(); - int b = tec.getFolderNumber(); - String ppRequiredTime = "Test"+b+"/G/RequiredTime-T" + a +".txt"; - String ppCriticalSections = "Test"+b+"/G/CriticalSections-T" + a +".txt"; - String ppPathDelay = "Test"+b+"/G/PathDelay-T" + a +".txt"; - String ppCostOptimalSolution = "Test"+b+"/G/CostOptimalSolution-T" + a +".txt"; - String ppAssignMatrix = "Test"+b+"/G/AssignMatrix-T" + a +".txt"; - String ppTotalTime = "Test"+b+"/G/TotalTime-T" + a +".txt"; - String ppAnalysisProblem = "Test"+b+"/G/AnalysisProblem-T" + a +".txt"; - String ppAssignMatrix2 = "Test"+b+"/G/AssignMatrix2-T" + a +".txt"; - String ppCostObjectiveFunction = "Test"+b+"/G/CostObjectiveFunction-T" + a +".txt"; - - try { - /* - fileStream = new PrintStream((new FileOutputStream("Test1/GA/RequiredTime.txt",true))); - fileStream1 = new PrintStream(new File("Test1/GA/CriticalSections.txt")); - PrintStream fileStream2 = new PrintStream(new File("Test1/GA/PathDelay.txt")); - fileStream3 = new PrintStream(new File("Test1/GA/CostOptimalSolution.txt")); - fileStream4 = new PrintStream(new FileOutputStream("Test1/GA/AssignMatrix.txt",true)); - fileStream5 = new PrintStream(new FileOutputStream("Test1/GA/TotalTime.txt",true)); - fileStream8 = new PrintStream(new FileOutputStream("Test1/GA/AssignMatrix2.txt",true)); - fileStream7 = new PrintStream(new FileOutputStream("Test1/GA/AnalysisProblem.txt",true)); - fileStream9 = new PrintStream(new FileOutputStream("Test1/GA/CostObjectiveFunction.txt",true)); - */ - fileStream = new PrintStream((new FileOutputStream(ppRequiredTime,true))); - fileStream1 = new PrintStream(new File(ppCriticalSections)); - PrintStream fileStream2 = new PrintStream(new File(ppPathDelay)); - fileStream3 = new PrintStream(new File(ppCostOptimalSolution)); - fileStream4 = new PrintStream(new FileOutputStream(ppAssignMatrix,true)); - fileStream5 = new PrintStream(new FileOutputStream(ppTotalTime,true)); - - //fileStream5 = new PrintStream(new File("TotalTime.txt")); - fileStream7 = new PrintStream(new FileOutputStream(ppAnalysisProblem,true)); - fileStream8 = new PrintStream(new FileOutputStream(ppAssignMatrix2,true)); - fileStream9 = new PrintStream(new FileOutputStream(ppCostObjectiveFunction,true)); - - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - - numTask = taskQueue.size(); - //Get free robots - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - getAllRobotIDs(); - getAllTaskIDs(); - int iOtt = 0; - int jOtt = 0; - int sOtt = 0; - double [][][] optimalAssignmentMatrix = new double[numRobotAug][numTaskAug][maxNumPaths]; - boolean [] TasksMissionsAllocates = new boolean [numTaskAug]; - //Initialize a boolean vector related to task set in order to consider already allocate task - fileStream7.println("Tasks>> "+ IDsRealTasks.size() + " Robot>> " + IDsIdleRobots.size() ); - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - - //Initialize optimal indexes - double costBFunction = 0; - double pathLength = 0; - double costArrivalTime = 0; - double costTardiness = 0; - double OptimalValueBFunction = 100000000; - boolean typesAreEqual = false; - for (int taskID : IDsAllTasks ) { - - int j = IDsAllTasks.indexOf(taskID); - if (j < numTask && i < numRobot ) { - typesAreEqual = taskQueue.get(j).isCompatible(tec.getRobot(robotID)); - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - for(int s = 0; s < maxNumPaths; s++) { - if (typesAreEqual && TasksMissionsAllocates[j] != true ) { - - long timeInitial = Calendar.getInstance().getTimeInMillis(); - pathLength = evaluatePathLength(robotID,taskID,s,tec); - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoEvaluatePaths = timeRequiretoEvaluatePaths + timeRequired; - costArrivalTime = computeArrivalTime(pathLength,tec.getForwardModel(robotID).getVel(),tec.getForwardModel(robotID).getAcc()); - costTardiness = computeTardiness(robotID,taskID,pathLength,tec); - costBFunction = pathLengthWeight*pathLength+ tardinessWeight*costTardiness + arrivalTimeWeight*costArrivalTime; - if (costBFunction < OptimalValueBFunction ) { - OptimalValueBFunction = costBFunction; - iOtt = i; - jOtt= j; - sOtt = s; - } - } - else { - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s, null); - } - } - } - optimalAssignmentMatrix[iOtt][jOtt][sOtt] = 1; - TasksMissionsAllocates[jOtt] = true; - //the task is already assigned - } - String ppMatrixOptimal = "Test"+b+"/G/MatrixOptimal-T" + a +".txt"; - writeMatrix(ppMatrixOptimal,optimalAssignmentMatrix); - fileStream.println(timeRequiretoEvaluatePaths+""); - fileStream.println(timeRequiretofillInPall+""); - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - if(optimalAssignmentMatrix[i][j][s] == 1) { - fileStream8.println(optimalAssignmentMatrix[i][j][s]+"--" + robotID + "--"+ taskID +"--" +(s+1)); - } - } - } - } - - fileStream8.println(""); - //Return the Optimal Assignment Matrix - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- initialTime; - fileStream5.println(timeRequired+""); - return optimalAssignmentMatrix; - } - - - /** - * Perform the task Assignment defining the mission for each robot - * @param AssignmentMatrix -> An Assignment Matrix of the optimization problem - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return An updated Trajectory Envelope Coordinator Simulation in which the mission for each - * robot is defined - */ - public boolean TaskAllocation(double [][][] AssignmentMatrix,AbstractTrajectoryEnvelopeCoordinator tec){ - System.out.println("Number of Robot : " + numRobot); - System.out.println("Number of Task : " + numTask); - System.out.println("Number of dummy Robot : " + dummyRobot); - System.out.println("Number of dummy Task : " + dummyTask); - System.out.println("Total Number of Robot : " + numRobotAug); - System.out.println("Total Number of Task : " + numTaskAug); - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - if (AssignmentMatrix[i][j][s] > 0) { - if (i < numRobot) { //Considering only real Robot - PoseSteering[] pss = pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s); - //PoseSteering[] pss = new PoseSteering[1]; - //pss[0] = new PoseSteering(tec.getRobotReport(robotID).getPose(),0); - //For Dispatch mission - if (j < numTask && pss != null) { - removePath(pss.hashCode()); - taskQueue.get(j).assignRobot(robotID); - taskQueue.get(j).setPaths(pss); - Mission[] robotMissions = taskQueue.get(j).getMissions(); - viz.displayTask(taskQueue.get(j).getStartPose(), taskQueue.get(j).getGoalPose(),taskID, "red"); - //tec.addMissions(new Mission(IDsIdleRobots[i],pss)); - System.out.println("Task # "+ taskID + " is Assigned"); - tec.setTaskAssigned(robotID,taskID); - tec.addMissions(robotMissions); - }else { - System.out.println("Virtual Task # "+ taskID + " is Assigned to a real robot"); - } - }else{ - - - - System.out.println("Task # "+ taskID + " is not Assigned to a real robot"); - - } - } - //Remove path from the path set - pathsToTargetGoal.remove(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s); - } - - } - } - - //Remove Assigned Tasks from the set - int i = 0; - int cont = 0; - while (i < Math.min(numRobot, numTask)) { - if (taskQueue.size() == 0 || taskQueue.size() <= i) { - break; - } - if (taskQueue.get(i).isTaskAssigned()){ - taskQueue.remove(i); - System.out.println("Task # "+ (cont+1) + " is removed "); - }else { - i = i+1; - } - cont +=1; - - } - System.out.println("Remaining task: "+ taskQueue.size()); - IDsAllRobots.removeAll(IDsAllRobots); - IDsAllTasks.removeAll(IDsAllTasks); - IDsRealTasks.removeAll(IDsRealTasks); - IDsIdleRobots.removeAll(IDsIdleRobots); - ScenarioAllocation = null; - - return true; - }//End Task Assignment Function - - - Callback cb = new Callback() { - private long lastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - public void performOperation() { - long timeNow = Calendar.getInstance().getTimeInMillis(); - if (timeNow-lastUpdate > 1000) { - lastUpdate = timeNow; - } - } - }; - - /** - * Start the Task Allocation Algorithm - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param alpha -> the weight of B and F function in objective function. It is considered as - * B*alpha + (1-alpha)*F - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - public void startTaskAssignment(AbstractTrajectoryEnvelopeCoordinator tec) { - //Create meta solver and solver - coordinator = tec; - numRobot = coordinator.getIdleRobots().size(); - //Start a thread that checks and enforces dependencies at every clock tick - this.setupInferenceCallback(); - - } - - - protected void setupInferenceCallback() { - - Thread TaskAssignmentThread = new Thread("Task Assignment") { - private long threadLastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - - public void run() { - while (true) { - System.out.println("Thread Running"); - - if (!taskQueue.isEmpty() && coordinator.getIdleRobots().size() > 2) { - MPSolver solverOnline = buildOptimizationProblemWithBNormalized(coordinator); - double [][][] assignmentMatrix = solveOptimizationProblem(solverOnline,coordinator); - for (int i = 0; i < assignmentMatrix.length; i++) { - int robotID = IDsAllRobots.get((i)); - for (int j = 0; j < assignmentMatrix[0].length; j++) { - int taskID = IDsAllTasks.get((j)); - for(int s = 0; s < maxNumPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if (assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " + robotID +" is assigned to Task "+ taskID +" throw Path " + (s+1)); - } - } - - } - } - TaskAllocation(assignmentMatrix,coordinator); - System.out.print("Task to be completed "+ taskQueue.size()); - solverOnline.clear(); - if(taskPosponedQueue.size() !=0) { - taskQueue.addAll(taskPosponedQueue); - taskPosponedQueue.removeAll(taskPosponedQueue); - } - } - - //Sleep a little... - if (CONTROL_PERIOD_Task > 0) { - try { - System.out.println("Thread Sleeping"); - Thread.sleep(CONTROL_PERIOD_Task); } //Thread.sleep(Math.max(0, CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate)); } - catch (InterruptedException e) { e.printStackTrace(); } - } - - long threadCurrentUpdate = Calendar.getInstance().getTimeInMillis(); - EFFECTIVE_CONTROL_PERIOD_task = (int)(threadCurrentUpdate-threadLastUpdate); - threadLastUpdate = threadCurrentUpdate; - - if (cb != null) cb.performOperation(); - - } - } - }; - TaskAssignmentThread.setPriority(Thread.MAX_PRIORITY); - TaskAssignmentThread.start(); - } - - - - - /** - * Start the Task Allocation Algorithm with Greedy Algorithm - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param alpha -> the weight of B and F function in objective function. It is considered as - * B*alpha + (1-alpha)*F - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - public void startTaskAssignmentLocalSearchAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec,int numIteration) { - //Create meta solver and solver - coordinator = tec; - numRobot = coordinator.getIdleRobots().size(); - this.numIteration = numIteration; - //Start a thread that checks and enforces dependencies at every clock tick - this.setupInferenceCallbackLocalSearch(); - - } - - - - protected void setupInferenceCallbackLocalSearch() { - - Thread TaskAssignmentThread = new Thread("Task Assignment") { - private long threadLastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - - public void run() { - while (true) { - System.out.println("Thread Running"); - - if (!taskQueue.isEmpty() && coordinator.getIdleRobots().size() > 2) { - double [][][] assignmentMatrix = solveOptimizationProblemLocalSearch(coordinator,numIteration); - for (int i = 0; i < assignmentMatrix.length; i++) { - int robotID = IDsAllRobots.get((i)); - for (int j = 0; j < assignmentMatrix[0].length; j++) { - int taskID = IDsAllTasks.get((j)); - for(int s = 0; s < maxNumPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if (assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " + robotID +" is assigned to Task "+ taskID +" throw Path " + (s+1)); - } - } - - } - } - TaskAllocation(assignmentMatrix,coordinator); - System.out.print("Task to be completed "+ taskQueue.size()); - if(taskPosponedQueue.size() !=0) { - taskQueue.addAll(taskPosponedQueue); - taskPosponedQueue.removeAll(taskPosponedQueue); - } - } - - //Sleep a little... - if (CONTROL_PERIOD_Task > 0) { - try { - System.out.println("Thread Sleeping"); - Thread.sleep(CONTROL_PERIOD_Task); } //Thread.sleep(Math.max(0, CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate)); } - catch (InterruptedException e) { e.printStackTrace(); } - } - - long threadCurrentUpdate = Calendar.getInstance().getTimeInMillis(); - EFFECTIVE_CONTROL_PERIOD_task = (int)(threadCurrentUpdate-threadLastUpdate); - threadLastUpdate = threadCurrentUpdate; - - if (cb != null) cb.performOperation(); - - } - } - }; - TaskAssignmentThread.setPriority(Thread.MAX_PRIORITY); - TaskAssignmentThread.start(); - } - - - - /** - * Start the Task Allocation Algorithm with Greedy Algorithm - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param alpha -> the weight of B and F function in objective function. It is considered as - * B*alpha + (1-alpha)*F - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - public void startTaskAssignmentGreedyAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec) { - //Create meta solver and solver - coordinator = tec; - numRobot = coordinator.getIdleRobots().size(); - - //Start a thread that checks and enforces dependencies at every clock tick - this.setupInferenceCallbackGreedy(); - - } - - - protected void setupInferenceCallbackGreedy() { - - Thread TaskAssignmentThread = new Thread("Task Assignment") { - private long threadLastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - - public void run() { - while (true) { - System.out.println("Thread Running"); - if (!taskQueue.isEmpty() && coordinator.getIdleRobots().size() != 0 ) { - double [][][] assignmentMatrix = solveOptimizationProblemGreedyAlgorithm(coordinator); - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < maxNumPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if (assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1) +"throw Path " + (s+1)); - } - } - } - } - TaskAllocation(assignmentMatrix,coordinator); - System.out.print("Task to be completed "+ taskQueue.size()); - } - //Sleep a little... - if (CONTROL_PERIOD_Task > 0) { - try { - System.out.println("Thread Sleeping"); - Thread.sleep(CONTROL_PERIOD_Task); } //Thread.sleep(Math.max(0, CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate)); } - catch (InterruptedException e) { e.printStackTrace(); } - } - - long threadCurrentUpdate = Calendar.getInstance().getTimeInMillis(); - EFFECTIVE_CONTROL_PERIOD_task = (int)(threadCurrentUpdate-threadLastUpdate); - threadLastUpdate = threadCurrentUpdate; - - if (cb != null) cb.performOperation(); - - } - } - }; - TaskAssignmentThread.setPriority(Thread.MAX_PRIORITY); - TaskAssignmentThread.start(); - } - - - - /** - * Start the Task Allocation Algorithm with Exact Algorithm - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param alpha -> the weight of B and F function in objective function. It is considered as - * B*alpha + (1-alpha)*F - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - public void startTaskAssignmentExactAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec) { - //Create meta solver and solver - coordinator = tec; - numRobot = coordinator.getIdleRobots().size(); - - //Start a thread that checks and enforces dependencies at every clock tick - this.setupInferenceCallbackExact(); - - } - - - protected void setupInferenceCallbackExact() { - - Thread TaskAssignmentThread = new Thread("Task Assignment") { - private long threadLastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - - public void run() { - while (true) { - System.out.println("Thread Running"); - if (!taskQueue.isEmpty() && coordinator.getIdleRobots().size() != 0 ) { - double [][][] assignmentMatrix = solveOptimizationProblemExactAlgorithm(coordinator,linearWeight); - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < maxNumPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if (assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1) +"throw Path " + (s+1)); - } - } - } - } - TaskAllocation(assignmentMatrix,coordinator); - System.out.print("Task to be completed "+ taskQueue.size()); - } - - //Sleep a little... - if (CONTROL_PERIOD_Task > 0) { - try { - System.out.println("Thread Sleeping"); - Thread.sleep(CONTROL_PERIOD_Task); } //Thread.sleep(Math.max(0, CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate)); } - catch (InterruptedException e) { e.printStackTrace(); } - } - - long threadCurrentUpdate = Calendar.getInstance().getTimeInMillis(); - EFFECTIVE_CONTROL_PERIOD_task = (int)(threadCurrentUpdate-threadLastUpdate); - threadLastUpdate = threadCurrentUpdate; - - if (cb != null) cb.performOperation(); - - } - } - }; - TaskAssignmentThread.setPriority(Thread.MAX_PRIORITY); - TaskAssignmentThread.start(); - } - - } - diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignmentSimple.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignmentSimple.java deleted file mode 100644 index a52448b..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignmentSimple.java +++ /dev/null @@ -1,1797 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment; - -import java.io.BufferedWriter; -import java.io.File; -import java.io.FileNotFoundException; -import java.io.FileOutputStream; -import java.io.FileWriter; -import java.io.IOException; -import java.io.OutputStreamWriter; -import java.io.PrintStream; -import java.io.Writer; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Calendar; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Iterator; -import java.util.List; -import java.util.TreeSet; -import java.util.logging.Logger; - -import org.apache.commons.lang.ArrayUtils; -import org.metacsp.multi.spatial.DE9IM.GeometricShapeDomain; -import org.metacsp.multi.spatial.DE9IM.GeometricShapeVariable; -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelopeSolver; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; -import org.metacsp.utility.UI.Callback; -import org.metacsp.utility.logging.MetaCSPLogging; -import org.sat4j.sat.SolverController; -import org.sat4j.sat.visu.SolverVisualisation; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.Geometry; -import com.vividsolutions.jts.geom.Polygon; -import com.vividsolutions.jts.geom.util.AffineTransformation; - -import aima.core.agent.Model; -import aima.core.util.datastructure.Pair; -import se.oru.coordination.coordination_oru.AbstractTrajectoryEnvelopeCoordinator; -import se.oru.coordination.coordination_oru.AbstractTrajectoryEnvelopeTracker; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.IndexedDelay; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.TrajectoryEnvelopeCoordinator; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.fleetmasterinterface.AbstractFleetMasterInterface; -import se.oru.coordination.coordination_oru.fleetmasterinterface.FleetMasterInterface; -import se.oru.coordination.coordination_oru.fleetmasterinterface.FleetMasterInterfaceLib.CumulatedIndexedDelaysList; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.tests.icaps2018.eval.TrajectoryEnvelopeCoordinatorSimulationICAPS; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.FleetVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.OptimizationProblemType; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.SolverParameters; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.*; -import com.google.ortools.sat.*; -import se.oru.coordination.coordination_oru.taskassignment.Task; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment.SortByDeadline; - - - - - -public class TaskAssignmentSimple{ - //Optimization Problem Parameters - protected int numRobot; - protected int numTask; - protected int dummyRobot; - protected int dummyTask; - protected int numRobotAug; - protected int numTaskAug; - protected double linearWeight = 1; - //Parameters of weights in Optimization Problem - protected double pathLengthWeight = 1; - protected double arrivalTimeWeight = 0; - protected double tardinessWeight = 0; - protected double [][] costValuesMatrix; - - - protected ArrayList taskQueue = new ArrayList (); - //Number of Idle Robots - protected ArrayList IDsIdleRobots = new ArrayList (); - //Path and arrival Time Parameters - //Infinity cost if path to reach a goal note exists - protected double MaxPathLength = 10000000; - //This is the sum of max path length for each robot - protected double sumMaxPathsLength = 1; - //This is the sum of arrival time considering max path length for each robot - protected double sumArrivalTime = 1; - //Parameters of mininum Velocity and Acceleration considering all robots - protected double minMaxVel; - protected double minMaxAcc; - //This is the sum of all tardiness - protected double sumTardiness = 1; - - - //Motion planner and Coordinator Parameters - protected AbstractTrajectoryEnvelopeCoordinator coordinator; - protected AbstractMotionPlanner defaultMotionPlanner = null; - protected HashMap motionPlanners = new HashMap(); - - - //Time required by function parameters - protected long timeRequiretoEvaluatePaths; - protected long timeRequiretofillInPall; - protected long timeRequiretoComputeCriticalSection; - protected long timeRequiretoComputePathsDelay; - - protected ArrayList pathsToTargetStart = new ArrayList (); - protected ArrayList pathsToTargetGoal = new ArrayList (); - protected ArrayList pathsDrivingRobot = new ArrayList (); - //FleetMaster Interface Parameters - - protected AbstractFleetMasterInterface fleetMasterInterface = null; - protected boolean propagateDelays = false; - protected static Logger metaCSPLogger = MetaCSPLogging.getLogger(TrajectoryEnvelopeCoordinator.class); - - - //Task Allocation Thread Parameters - protected int CONTROL_PERIOD_Task = 15000; - public static int EFFECTIVE_CONTROL_PERIOD_task = 0; - - protected FleetVisualization viz = null; - - - /** - * Set a motion planner to be used for re-planning for a specific - * robot. - * @param robotID The robot for which the given motion planner should be used. - * @param mp The motion planner that will be called for re-planning. - */ - public void setMotionPlanner(int robotID, AbstractMotionPlanner mp) { - this.motionPlanners.put(robotID, mp); - } - - - /** - * Get the motion planner used for re-planning for a specific robot. - * @param robotID The ID of a robot. - * @return The motion planner used for re-planning for the given robot. - */ - public AbstractMotionPlanner getMotionPlanner(int robotID) { - return this.motionPlanners.get(robotID); - } - - - - /** - * Set the weights of cost functions in Optimization Problem. THese must be numbers between 0 and 1. - * @param The path length weight; - * @param The arrival time weight; - * @param The tardiness weight - */ - - public void setCostFunctionsWeight(double pathLengthWeight,double arrivalTimeWeight,double tardinessWeight) { - if(pathLengthWeight <0|| arrivalTimeWeight < 0 || tardinessWeight < 0) { - throw new Error("Weights cannot be numbers less than 0!"); - } - double sumWeight = pathLengthWeight +arrivalTimeWeight + tardinessWeight; - if(sumWeight != 1 || sumWeight < 0 ) { - throw new Error("Weights sum must be equal to 1!"); - } - this.pathLengthWeight = pathLengthWeight; - this.arrivalTimeWeight = arrivalTimeWeight; - this.tardinessWeight = tardinessWeight; - } - - - /** - * Set the linear weight used in Optimization Problem - * @param viz -> Visualization to use - */ - - public void setLinearWeight(double linearWeight) { - this.linearWeight = linearWeight; - } - - /** - * Set the Fleet Visualization - * @param viz -> Visualization to use - */ - - public void setFleetVisualization(FleetVisualization viz) { - this.viz = viz; - } - - - /** - * Set the Coordinator - * @param viz -> Visualization to use - */ - - public void setCoordinator(AbstractTrajectoryEnvelopeCoordinator coordinator) { - this.coordinator = coordinator; - } - - - /** - * Get the Coordinator - * @param viz -> Visualization to use - */ - - public AbstractTrajectoryEnvelopeCoordinator getCoordinator() { - return this.coordinator; - } - - - /** - * Check if a goal can be reached by at least one robot of the Fleet - * @param PAll -> Initial PAll - * @return PAll incremented a task cannot be reach by any robot - */ - - private double [][] checkTargetGoals (double [][] PAll){ - for (int j= 0; j< PAll[0].length ; j++) { - boolean targetEndCanBeReach = false; - for (int i = 0; i < PAll.length; i++) { - if(PAll[i][j] != MaxPathLength) { - targetEndCanBeReach = true; - } - - } - //no robot can reach the target end -> need to introduce a dummy task and robot - if(!targetEndCanBeReach) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - - } - } - - double [][] PAllAug = new double [numRobotAug][numTaskAug]; - for(int i = 0;i < numRobotAug; i++) { - for(int j=0; j< numTaskAug;j++) { - if(i < PAll.length && j< PAll[0].length) { - PAllAug[i][j] = PAll[i][j]; - }else { - PAllAug[i][j] = 1; - pathsToTargetGoal.add(i*numTaskAug + j, null); - - } - } - } - return PAllAug; - } - - - - /** - * The default footprint used for robots if none is specified. - * NOTE: coordinates in footprints must be given in in CCW or CW order. - */ - public static Coordinate[] DEFAULT_FOOTPRINT = new Coordinate[] { - new Coordinate(-1.7, 0.7), //back left - new Coordinate(-1.7, -0.7), //back right - new Coordinate(2.7, -0.7), //front right - new Coordinate(2.7, 0.7) //front left - }; - - /** - * Set the minimum values of maximum velocity and acceleration considering all robots of the fleet - * @param MaxVel -> minimum of maximum velocity of all robot models; - * @param MaxAccel -> minimum of maximum acceleration of all robot models; - */ - - public void setminMaxVelandAccel(double MaxVel,double MaxAccel) { - this.minMaxVel = MaxVel; - this.minMaxAcc = MaxAccel; - } - - /** - * Enable and initialize the fleetmaster library to estimate precedences to minimize the overall completion time. - * Note: this function should be called before placing the first robot. - * ATTENTION: If dynamic_size is false, then the user should check that all the paths will lay in the given area. - * @param origin_x The x coordinate (in meters and in global inertial frame) of the lower-left pixel of fleetmaster GridMap. - * @param origin_y The y coordinate (in meters and in global inertial frame) of the lower-left pixel of fleetmaster GridMap. - * @param origin_theta The theta coordinate (in rads) of the lower-left pixel map (counterclockwise rotation). Many parts of the system currently ignore it. - * @param resolution The resolution of the map (in meters/cell), 0.01 <= resolution <= 1. It is assumed this parameter to be global among the fleet. - * The highest the value, the less accurate the estimation, the lowest the more the computational effort. - * @param width Number of columns of the map (>= 1) if dynamic sizing is not enabled. - * @param height Number of rows of the map (>= 1) if dynamic sizing is not enabled. - * @param dynamic_size If true, it allows to store only the bounding box containing each path. - * @param propagateDelays If true, it enables the delay propagation. - * @param debug If true, it enables writing to screen debugging info. - */ - public void instantiateFleetMaster(double origin_x, double origin_y, double origin_theta, double resolution, long width, long height, boolean dynamic_size, boolean propagateDelays, boolean debug) { - this.fleetMasterInterface = new FleetMasterInterface(origin_x, origin_y, origin_theta, resolution, width, height, dynamic_size, debug); - this.fleetMasterInterface.setDefaultFootprint(DEFAULT_FOOTPRINT); - this.propagateDelays = propagateDelays; - } - - /** - * Enable and initialize the fleetmaster library to estimate precedences to minimize the overall completion time - * while minimizing the computational requirements (bounding box are used to set the size of each path-image). - * Note: this function should be called before placing the first robot. - * @param resolution The resolution of the map (in meters/cell), 0.01 <= resolution <= 1. It is assumed this parameter to be global among the fleet. - * The highest the value, the less accurate the estimation, the lowest the more the computational effort. - * @param propagateDelays If true, it enables the delay propagation. - */ - public void instantiateFleetMaster(double resolution, boolean propagateDelays) { - this.fleetMasterInterface = new FleetMasterInterface(0., 0., 0., resolution, 100, 100, true, false); - this.fleetMasterInterface.setDefaultFootprint(DEFAULT_FOOTPRINT); - this.propagateDelays = propagateDelays; - } - - - /** - * Add a path to the fleetmaster interface - * @param robotID -> The ID of the robot - * @param pathID -> the ID of the path - * @param pss -> the path expressed as a PoseSteering vector - * @param boundingBox -> the bounding box of the path - * @param coordinates -> footprint of the robot - */ - protected void addPath(int robotID, int pathID, PoseSteering[] pss, Geometry boundingBox, Coordinate... coordinates) { - if (!fleetMasterInterface.addPath(robotID, pathID, pss, boundingBox, coordinates)) - metaCSPLogger.severe("Unable to add the path to the fleetmaster gridmap. Check if the map contains the given path."); - } - - - - /** - * Delete the path from the fleetmaster interface - * @param pathID -> The ID of the path to remove - */ - protected void removePath(int pathID){ - if (!fleetMasterInterface.clearPath(pathID)) - metaCSPLogger.severe("Unable to remove the path to the fleetmaster gridmap. Check if the map contains the given path."); - } - - - protected CumulatedIndexedDelaysList toIndexedDelaysList(TreeSet delays, int max_depth) { - //Handle exceptions - if (delays == null) { - metaCSPLogger.severe("Invalid input in function toPropagationTCDelays!!"); - throw new Error("Invalid input in function toPropagationTCDelays!!"); - } - if (delays.isEmpty() || max_depth < 1) return new CumulatedIndexedDelaysList(); - - //Cast the type - ArrayList indices = new ArrayList(); - ArrayList values = new ArrayList(); - Iterator it = delays.descendingIterator(); - IndexedDelay prev = delays.last(); - while (it.hasNext()) { - IndexedDelay current = it.next(); - //Check unfeasible values - if (current.getValue() == Double.NaN) { - metaCSPLogger.severe("NaN input in function toPropagationTCDelays!!"); - throw new Error("NaN input in function toPropagationTCDelays!!"); - } - if (current.getValue() == Double.NEGATIVE_INFINITY) { - metaCSPLogger.severe("-Inf input in function toPropagationTCDelays!!"); - throw new Error("-Inf input in function toPropagationTCDelays!!"); - } - if (prev.getIndex() < current.getIndex()) { - metaCSPLogger.severe("Invalid IndexedDelays TreeSet!!"); - throw new Error("Invalid IndexedDelays TreeSet!!"); - } - - //Update the value only if positive and only if the index is lower than the max depth - if (current.getValue() > 0 && current.getValue() < Double.MAX_VALUE && current.getIndex() < max_depth) { - if (values.size() == 0) { - //Add the index the first time its value is positive - indices.add(new Long(current.getIndex())); - values.add(current.getValue()); - } - else if (prev.getIndex() == current.getIndex()) - //Handle multiple delays in the same critical point - values.set(values.size()-1, values.get(values.size()-1) + current.getValue()); - else { - //Add the cumulative value if it is not the first. - indices.add(new Long(current.getIndex())); - values.add(values.get(values.size()-1) + current.getValue()); - } - } - prev = current; - } - CumulatedIndexedDelaysList propTCDelays = new CumulatedIndexedDelaysList(); - if (indices.size() > 0) { - propTCDelays.size = indices.size(); - propTCDelays.indices = ArrayUtils.toPrimitive((Long[]) indices.toArray(new Long[indices.size()])); - ArrayUtils.reverse(propTCDelays.indices); - propTCDelays.values = ArrayUtils.toPrimitive((Double[]) values.toArray(new Double[values.size()])); - ArrayUtils.reverse(propTCDelays.values); - } - return propTCDelays; - } - - - protected Pair estimateTimeToCompletionDelays(int path1ID,PoseSteering[] pss1, TreeSet delaysRobot1, int path2ID,PoseSteering[] pss2, TreeSet delaysRobot2, CriticalSection cs) { - - if (this.fleetMasterInterface != null && fleetMasterInterface.checkPathHasBeenAdded(path1ID)&& fleetMasterInterface.checkPathHasBeenAdded(path2ID)) { - CumulatedIndexedDelaysList te1TCDelays = toIndexedDelaysList(delaysRobot1, pss1.length); - //metaCSPLogger.info("[estimateTimeToCompletionDelays] te1TCDelays: " + te1TCDelays.toString()); - CumulatedIndexedDelaysList te2TCDelays = toIndexedDelaysList(delaysRobot2, pss2.length); - //metaCSPLogger.info("[estimateTimeToCompletionDelays] te2TCDelays: " + te2TCDelays.toString()); - return fleetMasterInterface.queryTimeDelay(cs, te1TCDelays, te2TCDelays,pss1,pss2); - } - - return new Pair (Double.NaN, Double.NaN); - } - - - /** - * Set a motion planner to be used for planning for all robots - * @param mp The motion planner that will be called for planning - */ - public void setDefaultMotionPlanner(AbstractMotionPlanner mp) { - this.defaultMotionPlanner = mp; - } - - - /** - * Get the motion planner to be used for planning for all robots. - * @return The motion planner used for planning for all robots - */ - public AbstractMotionPlanner getDefaultMotionPlanner() { - return this.defaultMotionPlanner; - } - - - /** - * Add a Task to Mission set - * @param task -> the task to add - * @return -> true if task is added correctly, otherwise false - */ - public boolean addTask(Task task) { - if (task == null) { - metaCSPLogger.severe("No task to add. Please give a correct Task."); - throw new Error("Cannot add the task"); - } - boolean TaskisAdded = taskQueue.add(task); - return TaskisAdded; - } - - /** - * Remove a task from the queue - * @param task The task to remove - * @return true if task is removed correctly, otherwise false - */ - public boolean removeTask(Task task) { - return taskQueue.remove(task); - } - - - - /** - * Get the task from Mission set in the index position - * @param index -> the index position of the task - * @return -> the task in index position - */ - public Task getTask(int index) { - if (index < 0 || index > taskQueue.size()) { - metaCSPLogger.severe("Wrong index."); - throw new Error("The task" + index + "not exist"); - }else { - return taskQueue.get(index); - } - } - - class SortByDeadline implements Comparator{ - @Override - public int compare(Task task1, Task task2) { - return (int) (task1.getDeadline()-task2.getDeadline()); - } - } - - private void checkOnTaskDeadline() { - ArrayList taskArray =new ArrayList (); - for(int j=0; j < taskQueue.size(); j++ ) { - taskArray.add(taskQueue.get(j)); - } - taskArray.sort(new SortByDeadline()); - for(int k= 0;k < taskArray.size();k++) { - } - for(int i=0;i < IDsIdleRobots.size(); i++) { - int index = taskQueue.indexOf(taskArray.get(i)); - if(taskQueue.get(index).getDeadline() != -1) { - taskQueue.get(index).setPriority(true); - } - - } - } - - /** - * Compute the number of Dummy Robots and/or Tasks. Consider the possibility to have a different number of robots (N) and tasks (M). If N > M, dummy tasks are - * considered, where a dummy task is a task for which a robot stay in starting position; while if M > N dummy robots - * are considered, where a dummy robot is only a virtual robot. - * @param numRobot Number of robots - * @param numTasks Number of tasks - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - private void dummyRobotorTask(int numRobot, int numTasks,AbstractTrajectoryEnvelopeCoordinator tec) { - numRobotAug = numRobot; - numTaskAug = numTasks; - //Restore initial value for dummy robot and task - dummyTask = 0; - dummyRobot = 0; - //Considering the possibility to have n != m - //If n > m -> we have dummy robot, so at some robot is assign the task to stay in starting position - if (numRobot > numTasks) { - dummyTask = numRobot - numTasks; - numTaskAug = numTasks + dummyTask; - } - else if (numRobot < numTasks) { - dummyRobot = numTasks - numRobot; - numRobotAug = numRobot + dummyRobot; - } - //This second check is used when we have particular cases due to Robot Type and Task Type - //Only if we have already a dummy task this check can be avoided - //If A robot cannot be assigned to any task - if (dummyTask == 0 || dummyRobot != 0) { - for (int i = 0; i < numRobot; i++) { - boolean flagAllocateRobot = false; - for (int j = 0; j < numTasks; j++) { - //check if robot can be assigned to one task - //if (taskQueue.get(j).getTaskType() == tec.getRobotType(IDsIdleRobots[i])) { - if (taskQueue.get(j).isCompatible(tec.getRobot(IDsIdleRobots.get(i)))) { - flagAllocateRobot = true; - - } - } - //the robot cannot be assigned to any task -> add a dummy robot and task - if (!flagAllocateRobot) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - } - } - } - //Only if we have already a dummy robot this check can be avoided - //If A task cannot be assigned to any robot - if (dummyRobot == 0 || dummyTask != 0) { - for (int i = 0; i < numTasks; i++) { - boolean flagAllocateTask = false; - for (int j = 0; j < numRobot; j++) { - //check if task can be assigned to one robot - //if (taskQueue.get(i).getTaskType() == tec.getRobotType(IDsIdleRobots[j])) { - if (taskQueue.get(i).isCompatible(tec.getRobot(IDsIdleRobots.get(j)))) { - flagAllocateTask = true; - } - } - //the task cannot be assigned to any robot -> add a dummy robot and task - if (!flagAllocateTask) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - } - } - } - } - - /** - * Transform a 1D array of MPVariable into a 2D MATRIX - * @param numRobot -> Number of robots - * @param numTasks -> Number of tasks - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblem}, {@link #buildOptimizationProblemWithB} or {@link #buildOptimizationProblemWithBNormalized} - * @return 2D Matrix of Decision Variable of the input problem - */ - private MPVariable [][] tranformArray(MPSolver optimizationProblem) { - //Take the vector of Decision Variable from the Optimization Problem - MPVariable [] array1D = optimizationProblem.variables(); - MPVariable [][] decisionVariable = new MPVariable [numRobotAug][numTaskAug]; - //Store them in a 2D Matrix - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - decisionVariable[i][j] = array1D[i*numTaskAug+j]; - } - } - return decisionVariable; - } - /** - * Impose a constraint on the optimization problem on previous optimal solution in order to not consider more it - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblem},{@link #buildOptimizationProblemWithB} or {@link #buildOptimizationProblemWithBNormalized} in which a solution is found - * @param assignmentMatrix -> The Assignment Matrix of the actual optimal solution - * @return Optimization Problem updated with the new constraint on previous optimal solution found - */ - private MPSolver constraintOnPreviousSolution(MPSolver optimizationProblem, double [][] assignmentMatrix) { - //Take decision Variable from Optimization Problem - MPVariable [][] DecisionVariable = tranformArray(optimizationProblem); - //Initialize a Constraint - MPConstraint c2 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY,1); - //Define the actual optimal solution as a Constraint in order to not consider more it - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - if (assignmentMatrix[i][j] >0) { - c2.setCoefficient(DecisionVariable[i][j],1); - }else { - c2.setCoefficient(DecisionVariable[i][j],0); - } - } - } - //Return the updated Optimization Problem - return optimizationProblem; - } - - - /** - * Impose a constraint on the optimization problem on previous optimal solution cost in order to not consider more solution that has a cost higher - * than this. In this manner is possible to avoid some cases. - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblem} in which a solution is found - * @param assignmentMatrix -> The Assignment Matrix of the actual optimal solution - * @return Optimization Problem updated with the new constraint on optimal solution cost - */ - private MPSolver constraintOnCostSolution(MPSolver optimizationProblem,double objectiveValue) { - //Take the vector of Decision Variable from the input solver - MPVariable [][] decisionVariable = tranformArray(optimizationProblem); - //Initialize a Constraint - MPConstraint c3 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY,objectiveValue); - //Define a constraint for which the next optimal solutions considering only B must have a cost less than objectiveValue - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - c3.setCoefficient(decisionVariable[i][j],costValuesMatrix[i][j]); - } - } - //Return the updated Optimization Problem - return optimizationProblem; - } - - /** - * Store the solution of a optimization problem in a Matrix - * @param numRobot -> Number of robots - * @param numTasks -> Number of tasks - * @param optimizationProblem -> A solved optimization problem - * @return Assignment matrix for the optimization problem given as input - */ - - private double [][] saveAssignmentMatrix(int numRobot,int numTasks,MPSolver optimizationProblem){ - //Take the decision variable from the optimization problem - MPVariable [][] decisionVariable = tranformArray(optimizationProblem); - double [][] assignmentMatrix = new double [numRobot][numTasks]; - //Store decision variable values in a Matrix - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTasks; j++) { - assignmentMatrix[i][j] = decisionVariable[i][j].solutionValue(); - } - } - return assignmentMatrix; - } - - /** - * Evaluate the cost associated to the path length for the a couple of robot and task. - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param robot -> The i-th Robot - * @param task -> The j-th the Task - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The cost associated to the path length for the couple of robot and task given as input - */ - private double evaluatePathLength(int robot , int task, AbstractTrajectoryEnvelopeCoordinator tec){ - //Evaluate the path length for the actual couple of task and ID - //Initialize the path length to infinity - double pathLength = MaxPathLength; - // Only for real robots and tasks - if (robot <= numRobot && task < numTask) { - //Take the state for the i-th Robot - RobotReport rr = tec.getRobotReport(IDsIdleRobots.get(robot-1)); - if (rr == null) { - metaCSPLogger.severe("RobotReport not found for Robot" + robot + "."); - throw new Error("RobotReport not found for Robot" + robot + "."); - } - //Evaluate the path from the Robot Starting Pose to Task End Pose - AbstractMotionPlanner rsp = getMotionPlanner(robot); - if(rsp == null) { // if there is not a specific motion planner for the robot - rsp = getDefaultMotionPlanner(); - } - rsp.setStart(rr.getPose()); - rsp.setGoals(taskQueue.get(task).getStartPose(),taskQueue.get(task).getGoalPose()); - rsp.setFootprint(tec.getFootprint(robot)); - - if (!rsp.plan() ) { - System.out.println("Robot" + robot +" cannot reach the Target End of Task " + (task+1)); - //the path to reach target end not exits - pathsToTargetGoal.add(null); - //Infinity cost is returned - return pathLength; - } - - //If the path exists - //Take the Pose Steering representing the path - PoseSteering[] pss = rsp.getPath(); - - //Add the path to the FleetMaster Interface -> this is necessary for F function - addPath(robot, pss.hashCode(), pss, null, tec.getFootprint(robot)); - //Save the path to Task in the path set - pathsToTargetGoal.add(pss); - //Take the Path Length - pathLength = Missions.getPathLength(pss); - System.out.println("PRovaaaaLLLLLL"+ pathLength + "robot>> "+ robot + " task>>" +task); - - - } else { //There also virtual robot and task are considered - //There are considered real robot and dummy task - if (numRobot >= numTask && robot <= numRobot){ //dummy task -> The Robot receive the task to stay in starting position - //The second condition is used in the special case in which we have that one robot cannot be - //assigned to any tasks due to its type, so we must add a dummy robot and a dummy task, but we - //Create the task to stay in robot starting position - PoseSteering[] dummyTask = new PoseSteering[1]; - //Take the state for the i-th Robot - RobotReport rr = tec.getRobotReport(IDsIdleRobots.get(robot-1)); - if (rr == null) { - metaCSPLogger.severe("RobotReport not found for Robot" + robot + "."); - throw new Error("RobotReport not found for Robot" + robot + "."); - } - //take the starting position of the robot - dummyTask[0] = new PoseSteering(rr.getPose(),0); - //Add the path to the FleetMaster Interface -> so it can be considered as an obstacle from - //the motion planner - addPath(robot, dummyTask.hashCode(), dummyTask, null, tec.getFootprint(robot)); - //Save the path to Dummy Task - pathsToTargetGoal.add(dummyTask); - //Consider a minimal pathLength - pathLength = 1; - return pathLength; - } - else { //There are considered dummy robot and real task - //dummy robot -> Consider a only virtual Robot - pathsToTargetGoal.add(null); - pathLength = 1; - return pathLength; - } - } - //Return the cost of path length - return pathLength; - } - - - /** - * Evaluate the PAll matrix, that is a matrix that contains all path for each possible combination of robot - * and task - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The PAll matrix - */ - private double[][] evaluatePAll(AbstractTrajectoryEnvelopeCoordinator tec){ - - PrintStream fileStream1 = null; - PrintStream fileStream2 = null; - try { - fileStream1 = new PrintStream(new File("PathPlanner.txt")); - fileStream2 = new PrintStream(new File("PAll.txt")); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - long timeInitial2 = 0; - - //Evaluate the path length for the actual couple of task and ID - //Initialize the sum of max paths lengths and time to do it for each robot - //This cost are used then for normalizing cost - double sumPathsLength = 0; - double sumArrivalTime = 0; - //Initialize PAll - double [][] PAll = new double[numRobotAug][numTaskAug]; - - - for (int robot = 0; robot < numRobotAug; robot++) { - double maxPathLength = 1; - for (int task = 0; task < numTaskAug; task++ ) { - - //Take time to understand how much time require this function - long timeInitial = Calendar.getInstance().getTimeInMillis(); - double pathLength = this.MaxPathLength; - - - //Evaluate path Length - boolean typesAreEqual = false; - if (task < numTask && robot < numRobot ) { - typesAreEqual = taskQueue.get(task).isCompatible(tec.getRobot(robot+1)); - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - if(typesAreEqual) { // only if robot and typoe have the same types - pathLength = evaluatePathLength(robot+1,task,tec); - //Take time to evaluate the path - - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoEvaluatePaths = timeRequiretoEvaluatePaths + timeRequired; - fileStream1.println(timeRequired+""); - timeInitial2 = Calendar.getInstance().getTimeInMillis(); - - if ( pathLength > maxPathLength && pathLength != this.MaxPathLength) { - maxPathLength = pathLength; - } - }else { - pathsToTargetGoal.add(null); - } - PAll[robot][task] = pathLength; - - //Take the time to fill in the PAll Matrix - long timeFinal2 = Calendar.getInstance().getTimeInMillis(); - long timeRequired2 = timeFinal2- timeInitial2; - timeRequiretofillInPall = timeRequiretofillInPall + timeRequired2; - fileStream2.println(timeRequired2+""); - }//For Task - //Sum the max path length for each robot - - sumPathsLength += maxPathLength; - //Sum the arrival time for the max path length - sumArrivalTime += computeArrivalTime(maxPathLength,this.minMaxVel,this.minMaxAcc); - } - double [][] PAllAug = checkTargetGoals(PAll); - //Save the sum of max paths length to normalize path length cost - this.sumMaxPathsLength = sumPathsLength; - //Save the sum of arrival time considering max paths length to normalize delay cost - this.sumArrivalTime = sumArrivalTime; - //Return the cost of path length - - - - return PAllAug; - } - - /** - * Evaluate the cost associated to time delay on completion of a task for a specific robot, due to interference with other robot - * and precedence constraints. The cost is evaluated considering the intersection between the path of robot i-th - * with the paths of other robots, considering the actual Assignment, but also paths related to already driving robot - * are considered. - * @param robot -> The i-th Robot - * @param task -> The j-th Task - * @param assignmentMatrix -> The Assignment Matrix related to a solution of the optimization problem - * @param tec -> an Abstract Trajectory Envelope Coordinator - * @return The cost associated to the delay on completion of task j for robot i due to interference with other robot - */ - private double evaluatePathDelay(int robot ,int task,double [][] assignmentMatrix,AbstractTrajectoryEnvelopeCoordinator tec){ - - - PrintStream fileStream1 = null; - PrintStream fileStream2 = null; - try { - fileStream1 = new PrintStream(new FileOutputStream("CriticalSections.txt",true)); - fileStream2 = new PrintStream(new FileOutputStream("PathDelay.txt",true)); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - long timeInitial2 = 0; - - - //Evaluate the delay time on completion time for the actual couple of task and ID - //Initialize the time delay - double delay = 0; - //Considering the Actual Assignment - if (assignmentMatrix[robot-1][task]>0) { - // Only for real robots and tasks - if (task < numTask && robot <= numRobot) { - //Take the Pose steering relate to i-th robot and j-th task from path set - PoseSteering[] pss1 = pathsToTargetGoal.get((robot-1)*assignmentMatrix[0].length + task); - if(pss1 == null) { - return delay; - } - //Initialize Array of delays for the two robots - TreeSet te1TCDelays = new TreeSet() ; - TreeSet te2TCDelays = new TreeSet() ; - //Compute the spatial Envelope for the i-th Robot - SpatialEnvelope se1 = TrajectoryEnvelope.createSpatialEnvelope(pss1,tec.getFootprint(robot)); - //Evaluate other path depending from the Assignment Matrix - for(int m = 0; m < assignmentMatrix.length; m++) { - for(int n = 0; n < assignmentMatrix[0].length; n++) { - if (assignmentMatrix [m][n] > 0 && m+1 != robot && n != task && n < numTask && m < numRobot) { - //Take the path of this second robot from path set - PoseSteering[] pss2 = pathsToTargetGoal.get((m)*assignmentMatrix[0].length + n); - if (pss2 != null) {//is == null if robotType is different to Task type or path not exists - //Evaluate the Spatial Envelope of this second Robot - SpatialEnvelope se2 = TrajectoryEnvelope.createSpatialEnvelope(pss2,tec.getFootprint(m+1)); - long timeInitial = Calendar.getInstance().getTimeInMillis(); - //Compute the Critical Section between this 2 robot - CriticalSection [] css = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, se2,true, Math.min(tec.getFootprintPolygon(robot).getArea(),tec.getFootprintPolygon(m+1).getArea())); - - - //Evaluate the time to compute critical Section - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoComputeCriticalSection = timeRequiretoComputeCriticalSection + timeRequired; - fileStream1.println(timeRequired+""); - - timeInitial2 = Calendar.getInstance().getTimeInMillis(); - //Compute the delay due to precedence constraint in Critical Section - for (int g = 0; g < css.length; g++) { - Pair a1 = estimateTimeToCompletionDelays(pss1.hashCode(),pss1,te1TCDelays,pss2.hashCode(),pss2,te2TCDelays, css[g]); - double delayCriticalSection = a1.getFirst(); - if(delayCriticalSection < 0 ) { - delay += 0; - }else if(delayCriticalSection == Double.POSITIVE_INFINITY) { - delay += 10000; - }else { - delay += delayCriticalSection; - } - } - - //Take the paths of driving robots from coordinator - pathsDrivingRobot = tec.getDrivingEnvelope(); - //Evaluate the delay time due to already driving robots - for(int k = 0; k < pathsDrivingRobot.size(); k++) { - CriticalSection [] cssDrivingRobot = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, pathsDrivingRobot.get(k),true, Math.min(tec.getFootprintPolygon(robot).getArea(),tec.getFootprintPolygon(m+1).getArea())); - for (int b = 0; b < cssDrivingRobot.length; b++) { - Pair a1 = estimateTimeToCompletionDelays(pss1.hashCode(),pss1,te1TCDelays,pathsDrivingRobot.get(k).getPath().hashCode(),pathsDrivingRobot.get(k).getPath(),te2TCDelays, cssDrivingRobot[b]); - delay += a1.getFirst(); - } - } - - long timeFinal2 = Calendar.getInstance().getTimeInMillis(); - long timeRequired2 = timeFinal2- timeInitial2; - timeRequiretoComputePathsDelay = timeRequiretoComputePathsDelay + timeRequired2; - fileStream2.println(timeRequired2+""); - } - } - } - } - - } else { //There also virtual robot and task are considered - //the delay associated to dummy robot and task is considerd 0 - return delay; - } - } - - //return the delay for the i-th robot and the j-th task due to interference with other robots - return delay; - } - - - - /** - * Compute the arrival time to a task for a specified robot - * @param pathLength -> The max path for each robot - * @return The time to drive the path - */ - private double computeArrivalTime(double pathLength,double vel,double acc){ - //Compute the arrival time of this path, considering a robot alone with a velocity trapezoidal model - double arrivalTime = pathLength/vel + vel/acc; - //Return the arrival time - return arrivalTime; - } - - - /** - * Compute the arrival for all robots in fleet to all possible tasks - * @param pathLength -> The max path for each robot - * @return The time to drive the path - */ - private double [][] computeArrivalTimeFleet(double[][]PAll,AbstractTrajectoryEnvelopeCoordinator tec){ - //Compute the arrival time of this path, considering a robot alone with a velocity trapezoidal model - double [][] arrivalTimeMatrix = new double [numRobotAug][numTaskAug]; - for (int i = 0 ; i < IDsIdleRobots.size(); i++) { - for (int j = 0 ; j < taskQueue.size(); j++) { - double vel = tec.getRobot(i+1).getForwardModel().getVel(); - double acc = tec.getRobot(i+1).getForwardModel().getAcc(); - double arrivalTime = computeArrivalTime(PAll[i][j],vel,acc); - arrivalTimeMatrix[i][j] = arrivalTime; - } - } - - //Return the arrival time - return arrivalTimeMatrix; - } - - /** - * Evaluate the tardiness in completion of a task . The tardiness is the defined as the further time required to complete a task - * after the deadline - * @param pathLength -> path length - * @param task -> the task j-th - * @return - */ - - private double[][] computeTardiness(double [][]PAll,AbstractTrajectoryEnvelopeCoordinator tec) { - double tardiness = 0; - - double [][] tardinessMatrix = new double [numRobotAug][numTaskAug]; - for (int i = 0 ; i < IDsIdleRobots.size(); i++) { - for (int j = 0 ; j < taskQueue.size(); j++) { - if (taskQueue.get(j).isDeadlineSpecified()) { // Compute tardiness only if specified in task constructor - double deadline = taskQueue.get(j).getDeadline(); //Expressed in seconds - double vel = tec.getRobot(i+1).getForwardModel().getVel(); - double acc = tec.getRobot(i+1).getForwardModel().getVel(); - double completionTime = computeArrivalTime(PAll[i][j],vel,acc) + taskQueue.get(j).getOperationTime(); - tardiness = Math.max(0, (completionTime-deadline)); - - tardinessMatrix[i][j] = tardiness; - sumTardiness += tardiness; - } - } - } - return tardinessMatrix; - } - - /** - * Evaluate the overall B function, that is the function that consider interference free costs - * Costs considered: - * 1) Path Length - * 2) Tardiness - * Each cost is already normalized; - * @param PAll - * @return - */ - private double [][] evaluateBFunction(double [][]PAll,AbstractTrajectoryEnvelopeCoordinator tec){ - double [][] tardinessMatrix = computeTardiness(PAll,tec); - double [][] BFunction = new double [numRobotAug][numTaskAug]; - costValuesMatrix = new double [numRobotAug][numTaskAug]; - if(linearWeight == 1) { - double [][] arrivalTimeMatrix = computeArrivalTimeFleet(PAll,tec); - for (int i = 0 ; i < numRobotAug; i++) { - for (int j = 0 ; j < numTaskAug; j++) { - BFunction[i][j] = pathLengthWeight*PAll[i][j]/sumMaxPathsLength+ tardinessWeight*tardinessMatrix[i][j]/sumTardiness + arrivalTimeWeight*arrivalTimeMatrix[i][j]/sumArrivalTime; - costValuesMatrix[i][j] = pathLengthWeight*PAll[i][j]+ tardinessWeight*tardinessMatrix[i][j] + arrivalTimeWeight*arrivalTimeMatrix[i][j]; - } - } - } - else { - for (int i = 0 ; i < numRobotAug; i++) { - for (int j = 0 ; j < numTaskAug; j++) { - BFunction[i][j] = pathLengthWeight*PAll[i][j]/sumMaxPathsLength + tardinessWeight*tardinessMatrix[i][j]/sumTardiness; - costValuesMatrix[i][j] = PAll[i][j]+ tardinessMatrix[i][j]; - } - } - } - - return BFunction; - } - - /** - * Evaluates the number of all feasible solutions for an optimization problem, with is defined with {@link #buildOptimizationProblem} - * @param numRobot -> Number of Robot of the optimization problem - * @param numTasks -> Number of Tasks of the optimization problem - * @return The number of all feasible solutions for the Optimization Problem - */ - - public int numberFeasibleSolution(int numRobot,int numTasks){ - //Create an optimization problem - MPSolver optimizationProblemCopy = buildOptimizationProblem(numRobot,numTasks); - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblemCopy.solve(); - int numberFeasibleSolution = 0; - while(resultStatus != MPSolver.ResultStatus.INFEASIBLE ) { - //Solve the optimization Problem - resultStatus = optimizationProblemCopy.solve(); - //If The solution is feasible increment the number of feasible solution - if (resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - numberFeasibleSolution = numberFeasibleSolution+1; - } - double [][] assignmentMatrix = saveAssignmentMatrix(numRobot,numTasks,optimizationProblemCopy); - //Add the constraint to actual solution -> in order to consider this solution as already found - optimizationProblemCopy = constraintOnPreviousSolution(optimizationProblemCopy,assignmentMatrix); - } - //Return the Total number of feasible solution - optimizationProblemCopy.clear(); - return numberFeasibleSolution; - } - - /** - * Evaluates all possible feasible solutions for an optimization problem,with is defined with {@link #buildOptimizationProblem}. A feasible solution is a solution that verify constraints - * @param numRobot -> Number of Robots - * @param numTasks -> Number of Tasks - * @return A set containing all feasible solutions - */ - public double [][][] evaluateFeasibleSolution(int numRobot,int numTasks){ - //Define the optimization problem - MPSolver optimizationProblemCopy = buildOptimizationProblem(numRobot,numTasks); - //Evaluate the number of all feasible solution for the optimization problem - int feasibleSolutions = numberFeasibleSolution(numRobot,numTasks); - //Initialize a set to store all feasible solution - double [][][] AssignmentMatrixOptimalSolutions = new double [feasibleSolutions][numRobot][numTasks]; - /////////////////////////////////////// - for(int k=0; k < feasibleSolutions; k++) { - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblemCopy.solve(); - //Transform the Assignment Vector to Matrix - double [][] AssignmentMatrix = saveAssignmentMatrix(numRobot,numTasks,optimizationProblemCopy); - //Store the optimal solution - AssignmentMatrixOptimalSolutions[k]=AssignmentMatrix; - //Add the constraint to actual solution in order to consider this solution as already found - optimizationProblemCopy = constraintOnPreviousSolution(optimizationProblemCopy,AssignmentMatrix); - } - //Return the set of all Feasible solutions - return AssignmentMatrixOptimalSolutions; - } - - /** - * Builds the optimization problem. Define a decision variable X_ij as a binary variable in which i indicate - * the robot id, j the tasks. Also constraints are defined: - * the constraints considered are : - * 1) Each Task can be assign only to a robot; - * 2) Each Robot can perform only a task at time; - * @param numRobot -> Number of Robots - * @param numTasks -> Number of Tasks. - * @return A constrained optimization problem without the objective function - */ - private MPSolver buildOptimizationProblem(int numRobotAug,int numTasksAug) { - //Initialize a linear solver - MPSolver optimizationProblem = new MPSolver( - "TaskAssignment", MPSolver.OptimizationProblemType.CBC_MIXED_INTEGER_PROGRAMMING); - //START DECISION VARIABLE VARIABLE - MPVariable [][] decisionVariable = new MPVariable[numRobotAug][numTasksAug] ; - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTasksAug; j++) { - decisionVariable[i][j] = optimizationProblem.makeBoolVar("x"+"["+i+","+j+"]"); - } - } - //END DECISION VARIABLE - ////////////////////////// - // START CONSTRAINTS - //Each Robot can be assign only to a Task - for (int i = 0; i < numRobotAug; i++) { - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY, 1); - for (int j = 0; j < numTasksAug; j++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j], 1); - } - } - //Each task can be performed only by a robot - for (int j = 0; j < numTasksAug; j++) { - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(1, 1); - for (int i = 0; i < numRobotAug; i++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j], 1); - } - } - - //END CONSTRAINTS - //In case of having more task than robots, the task with a closest deadline are set with a higher priority - if(taskQueue.size() > IDsIdleRobots.size()) { - checkOnTaskDeadline(); - //Each task can be performed only by a robot - for (int j = 0; j < taskQueue.size(); j++) { - //Initialize the constraint - if(taskQueue.get(j).isPriority()) { - MPConstraint c3 = optimizationProblem.makeConstraint(1, 1); - for (int i = 0; i < IDsIdleRobots.size(); i++) { - //Build the constraint - c3.setCoefficient(decisionVariable[i][j], 1); - } - } - } - } - ///////////////////////////////////////////////// - return optimizationProblem; - } - /** - * * Builds the optimization problem complete with Objective Function. Define a decision variable X_ij as a binary variable in which i indicate - * the robot id, j the tasks. Also constraints are defined: - * the constraints considered are : - * 1) Each Task can be assign only to a robot; - * 2) Each Robot can perform only a task at time. - * The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m) - * with n = number of robot and m = number of tasks. - * Only the B function is considered in this case - * @param tec -> An AbstractTrajectoryEnvelopeCoordinator Coordinator - * @return A constrained optimization problem with the objective function - */ - public MPSolver buildOptimizationProblemWithB(AbstractTrajectoryEnvelopeCoordinator tec) { - - - //Take the number of tasks - numTask = taskQueue.size(); - //Get free robots and their IDs - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - //Build the solver and an objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobotAug,numTaskAug); - MPVariable [][] decisionVariable = tranformArray(optimizationProblem); - ///////////////////////////////// - //START OBJECTIVE FUNCTION - - MPObjective objective = optimizationProblem.objective(); - for (int i = 0; i < numRobotAug; i++) { - boolean typesAreEqual = false; - for (int j = 0; j < numTaskAug; j++) { - if (j < numTask && i < numRobot ) { - typesAreEqual = taskQueue.get(j).isCompatible(tec.getRobot(i+1)); - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - if (typesAreEqual) { - //Set the coefficient of the objective function with the normalized path length - double pathLength = evaluatePathLength(i+1,j,tec); - if ( pathLength != MaxPathLength) { - objective.setCoefficient(decisionVariable[i][j], pathLength); - }else {//the path to reach the task not exists - //the decision variable is set to 0 -> this allocation is not valid - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j],1); - } - }else { //robotType != taskType - //the decision variable is set to 0 -> this allocation is not valid - MPConstraint c2 = optimizationProblem.makeConstraint(0,0); - c2.setCoefficient(decisionVariable[i][j],1); - } - } - } - //Define the problem as a minimization problem - objective.setMinimization(); - //END OBJECTIVE FUNCTION - return optimizationProblem; - } - - /** - * Builds the optimization problem complete with Objective Function. Define a decision variable X_ij as a binary variable in which i indicate - * the robot id, j the tasks. Also constraints are defined: - * the constraints considered are : - * 1) Each Task can be assign only to a robot; - * 2) Each Robot can perform only a task at time. - * The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m) - * with n = number of robot and m = number of tasks. - * Only the B function is considered in this case, and each cost is normalized with the max path length considering - * all missions. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return A constrained optimization problem with the objective function and each cost is normalized - */ - public MPSolver buildOptimizationProblemWithBNormalized(AbstractTrajectoryEnvelopeCoordinator tec) { - //Take the number of tasks - numTask = taskQueue.size(); - //Get free robots and their IDs - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - double[][] PAll = evaluatePAll(tec); - double[][] BFunction = evaluateBFunction(PAll,tec); - //Build the solver and an objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobotAug,numTaskAug); - MPVariable [][] decisionVariable = tranformArray(optimizationProblem); - ///////////////////////////////// - //START OBJECTIVE FUNCTION - MPObjective objective = optimizationProblem.objective(); - - - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - double pathLength = PAll[i][j]; - double costBFunction = BFunction[i][j]; - if ( pathLength != MaxPathLength) { - //Set the coefficient of the objective function with the normalized path length - //objective.setCoefficient(decisionVariable[i][j], pathLength); - objective.setCoefficient(decisionVariable[i][j],costBFunction); - }else { // if the path does not exists or the robot type is different from the task type - //the path to reach the task not exists - //the decision variable is set to 0 -> this allocation is not valid - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j],1); - } - - } - } - //Define the problem as a minimization problem - objective.setMinimization(); - //END OBJECTIVE FUNCTION - return optimizationProblem; - } - - - /** - * Solve the optimization problem given as input considering both B and F Functions. The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m). - * with n = number of robot and m = number of tasks. The solver first finds the optimal solution considering only B function and then - * for this each solution (that is an assignment) evaluates the cost of F function. Then a new optimal solution considering only B is - * computed and it is consider only if the cost of this new assignment considering only B is less than the min cost of previous assignments - * considering both F and B function - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblemWithB} - * @param tec -> an Abstract Trajectory Envelope Coordinator - * @param alpha -> the linear parameter used to weights B and F function expressed in percent( 1-> 100%). The objective function is - * considered as B*alpha + (1-alpha)*F - * @return An Optimal Assignment that minimize the objective function - */ - - public double [][] solveOptimizationProblem(MPSolver optimizationProblem,AbstractTrajectoryEnvelopeCoordinator tec,double alpha){ - - PrintStream fileStream = null; - PrintStream fileStream1 = null; - try { - fileStream = new PrintStream(new File("RequiredTime.txt")); - fileStream1 = new PrintStream(new File("CriticalSections.txt")); - PrintStream fileStream2 = new PrintStream(new File("PathDelay.txt")); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - //Initialize the optimal assignment and the cost associated to it - double [][] optimalAssignmentMatrix = new double[numRobotAug][numTaskAug]; - double objectiveOptimalValue = 100000000; - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblem.solve(); - int cont=0; - while(resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - //Evaluate an optimal assignment that minimize only the B function - resultStatus = optimizationProblem.solve(); - //Evaluate the Assignment Matrix - double [][] AssignmentMatrix = saveAssignmentMatrix(numRobotAug,numTaskAug,optimizationProblem); - //Initialize cost of objective value - double objectiveFunctionValue = 0; - double costValue = 0; // -> is the cost of B function non normalized - double costofAssignment = 0; - double costofAssignmentForConstraint = 0; - double costF = 0; - //Evaluate the cost of F Function for this Assignment - timeRequiretoComputeCriticalSection = 0; - timeRequiretoComputePathsDelay = 0; - - //Take time to understand how much time require this function - for (int i = 0; i < numRobotAug; i++) { - for(int j = 0;j < numTaskAug; j++) { - if ( AssignmentMatrix[i][j] > 0) { - if (alpha != 1) { - //Evaluate cost of F function only if alpha is not equal to 1 - costF = evaluatePathDelay(i+1,j,AssignmentMatrix,tec)/sumArrivalTime; - double costB = optimizationProblem.objective().getCoefficient(optimizationProblem.variables()[i*numTaskAug+j]); - costofAssignment = alpha*costB + (1-alpha)*costF + costofAssignment ; - costofAssignmentForConstraint = costB + costF + costofAssignmentForConstraint; - - } - else { - //In order to solve the case with more optimal solution with the same cost, the pow of each cost is considered - double costB = optimizationProblem.objective().getCoefficient(optimizationProblem.variables()[i*numTaskAug+j]); - costofAssignment = Math.pow(alpha*costB, 2) + costofAssignment ; - costofAssignmentForConstraint = Math.pow(costB,2) + costofAssignmentForConstraint; - - } - } - } - } - System.out.println("cost>>"+ costofAssignmentForConstraint); - - fileStream.println(timeRequiretoEvaluatePaths+""); - fileStream.println(timeRequiretofillInPall+""); - fileStream.println(timeRequiretoComputeCriticalSection+""); - fileStream.println(timeRequiretoComputePathsDelay+""); - //Compare actual solution and optimal solution finds so far - if (costofAssignment < objectiveOptimalValue && resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - objectiveOptimalValue = costofAssignment; - optimalAssignmentMatrix = AssignmentMatrix; - - - - } - //Add the constraint on cost for next solution - optimizationProblem = constraintOnCostSolution(optimizationProblem,costofAssignmentForConstraint); - //Add the constraint to actual solution in order to consider this solution as already found - optimizationProblem = constraintOnPreviousSolution(optimizationProblem,AssignmentMatrix); - - } - - //Return the Optimal Assignment Matrix - return optimalAssignmentMatrix; - } - /** - * Solve the optimization problem given as input considering both B and F Functions. The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m). - * with n = number of robot and m = number of tasks - * Assignments are computed at each step in this case. In this case the solver consider all the feasible solutions for the problem. - * Exact Algorithm. - * @param tec -> TrajectoryEnvelopeCoordinatorSimulation - * @param alpha -> the linear parameter used to weights B and F function expressed in percent( 1-> 100%). The objective function is - * considered as B*alpha + (1 - alpha)*F - * @return The Optimal Assignment that minimize the objective function - */ - - public double [][] solveOptimizationProblemExactAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec,double alpha){ - - PrintStream fileStream = null; - try { - fileStream = new PrintStream(new File("RequiredTime.txt")); - PrintStream fileStream1 = new PrintStream(new File("CriticalSections.txt")); - PrintStream fileStream2 = new PrintStream(new File("PathDelay.txt")); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - numTask = taskQueue.size(); - //Get free robots - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - //Consider possibility to have dummy Robot or Tasks - double [][] PAll = evaluatePAll(tec); - double [][] BFunction = evaluateBFunction(PAll,tec); - //Build the optimization Problem without the objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobotAug,numTaskAug); - //Initialize the optimal assignment and the cost associated to it - double [][] optimalAssignmentMatrix = new double[numRobotAug][numTaskAug]; - double objectiveOptimalValue = 100000000; - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblem.solve(); - - while(resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - //Evaluate a feasible assignment - resultStatus = optimizationProblem.solve(); - //Evaluate the Assignment Matrix - double [][] AssignmentMatrix = saveAssignmentMatrix(numRobotAug,numTaskAug,optimizationProblem); - //Initialize cost of objective value - double objectiveFunctionValue = 0; - double costBFunction = 0; - double costFFunction = 0; - //Evaluate the cost for this Assignment - for (int i = 0; i < numRobotAug ; i++) { - for(int j=0;j < numTaskAug ; j++) { - if (AssignmentMatrix[i][j]>0) { - //costBFunction = costBFunction + PAll[i][j]/sumMaxPathsLength; - costBFunction = costBFunction + BFunction[i][j]; - if (alpha != 1) { - costFFunction = costFFunction + evaluatePathDelay(i+1,j,AssignmentMatrix,tec)/sumArrivalTime; - } - } - } - } - - - fileStream.println(timeRequiretoEvaluatePaths+""); - fileStream.println(timeRequiretofillInPall+""); - fileStream.println(timeRequiretoComputeCriticalSection+""); - fileStream.println(timeRequiretoComputePathsDelay+""); - - objectiveFunctionValue = alpha * costBFunction + (1-alpha)*costFFunction; - //Compare actual solution and optimal solution finds so far - if (objectiveFunctionValue < objectiveOptimalValue && resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - objectiveOptimalValue = objectiveFunctionValue; - optimalAssignmentMatrix = AssignmentMatrix; - } - //Add the constraint to actual solution in order to consider this solution as already found - optimizationProblem = constraintOnPreviousSolution(optimizationProblem,AssignmentMatrix); - } - //Return the Optimal Assignment Matrix - return optimalAssignmentMatrix; - } - - - /** - * Solve the optimization problem given as input considering both B and F Functions. The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m). - * with n = number of robot and m = number of tasks - * The problem is resolved considering a Greedy algorithm, so each robot is assigned to the best task for him and then the robot and Task are - * removed from their set - * @param tec -> TrajectoryEnvelopeCoordinatorSimulation - * @param alpha -> the linear parameter used to weights B and F function expressed in percent( 1-> 100%). The objective function is - * considered as B*alpha + (1 - alpha)*F - * @return The Optimal Assignment that minimize the objective function - */ - - public double [][] solveOptimizationProblemGreedyAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec,double alpha){ - - PrintStream fileStream = null; - try { - fileStream = new PrintStream(new File("RequiredTime.txt")); - PrintStream fileStream1 = new PrintStream(new File("CriticalSections.txt")); - PrintStream fileStream2 = new PrintStream(new File("PathDelay.txt")); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - - numTask = taskQueue.size(); - //Get free robots - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - double [][] PAll = evaluatePAll(tec); - double [][] BFunction = evaluateBFunction(PAll,tec); - double [][] optimalAssignmentMatrix = new double[numRobotAug][numTaskAug]; - //Initialize optimal indexes - int iOtt = 0; - int jOtt = 0; - //Initialize a boolean vector related to task set in order to consider already allocate task - boolean [] TasksMissionsAllocates = new boolean [numTaskAug]; - for (int i = 0; i < numRobotAug ; i++) { - double costBFunction = 0; - double OptimalValueBFunction = 100000000; - boolean typesAreEqual = false; - for(int j=0;j < numTaskAug ; j++) { - if (j < numTask && i < numRobot ) { - typesAreEqual = taskQueue.get(j).isCompatible(tec.getRobot(i+1)); - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - if (typesAreEqual) { - //costBFunction = PAll[i][j]/sumMaxPathsLength; - costBFunction = BFunction[i][j]; - if (costBFunction < OptimalValueBFunction && !TasksMissionsAllocates[j] ) { - OptimalValueBFunction = costBFunction; - iOtt = i; - jOtt= j; - } - } - } - - fileStream.println(timeRequiretoEvaluatePaths+""); - fileStream.println(timeRequiretofillInPall+""); - fileStream.println(timeRequiretoComputeCriticalSection+""); - fileStream.println(timeRequiretoComputePathsDelay+""); - - optimalAssignmentMatrix[iOtt][jOtt] = 1; - //the task is already assigned - TasksMissionsAllocates[jOtt] = true; - } - //Return the Optimal Assignment Matrix - return optimalAssignmentMatrix; - } - - /** - * Perform the task Assignment defining the mission for each robot - * @param AssignmentMatrix -> An Assignment Matrix of the optimization problem - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return An updated Trajectory Envelope Coordinator Simulation in which the mission for each - * robot is defined - */ - public boolean TaskAllocation(double [][] AssignmentMatrix,AbstractTrajectoryEnvelopeCoordinator tec){ - System.out.println("Number of Robot : " + numRobot); - System.out.println("Number of Task : " + numTask); - System.out.println("Number of dummy Robot : " + dummyRobot); - System.out.println("Number of dummy Task : " + dummyTask); - System.out.println("Total Number of Robot : " + numRobotAug); - System.out.println("Total Number of Task : " + numTaskAug); - for (int i = 0; i < AssignmentMatrix.length; i++) { - for (int j = 0; j < AssignmentMatrix[0].length; j++) { - if (AssignmentMatrix[i][j] > 0) { - if (i < IDsIdleRobots.size()) { //Considering only real Robot - PoseSteering[] pss = pathsToTargetGoal.get(i*AssignmentMatrix[0].length + j); - //For Dispatch mission - if (j < taskQueue.size() && pss != null) { - taskQueue.get(j).assignRobot(i+1); - taskQueue.get(j).setPaths(pss); - Mission[] robotMissions = taskQueue.get(j).getMissions(); - viz.displayTask(taskQueue.get(j).getStartPose(), taskQueue.get(j).getGoalPose(), (j+1), "red"); - - //tec.addMissions(new Mission(IDsIdleRobots[i],pss)); - System.out.println("Task # "+ (j+1) + " is Assigned"); - - tec.addMissions(robotMissions); - }else { - System.out.println("Virtual Task # "+ (j+1) + " is Assigned to a real robot"); - } - }else{ - System.out.println("Task # "+ (j+1) + " is not Assigned to a real robot"); - - } - } - } - } - //Remove Assigned Tasks from the set - int i = 0; - int cont = 0; - while (i < Math.min(IDsIdleRobots.size(), taskQueue.size())) { - if (taskQueue.size() == 0 || taskQueue.size() <= i) { - break; - } - if (taskQueue.get(i).isTaskAssigned()){ - taskQueue.remove(i); - System.out.println("Task # "+ (cont+1) + " is removed "); - }else { - i = i+1; - } - cont +=1; - - } - System.out.println("Remaining task: "+ taskQueue.size()); - //Remove all path from the path set - pathsToTargetGoal.removeAll(pathsToTargetGoal); - return true; - }//End Task Assignment Function - - - Callback cb = new Callback() { - private long lastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - public void performOperation() { - long timeNow = Calendar.getInstance().getTimeInMillis(); - if (timeNow-lastUpdate > 1000) { - lastUpdate = timeNow; - } - } - }; - /** - * Start the Task Allocation Algorithm - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param alpha -> the weight of B and F function in objective function. It is considered as - * B*alpha + (1-alpha)*F - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - public void startTaskAssignment(AbstractTrajectoryEnvelopeCoordinator tec) { - //Create meta solver and solver - coordinator = tec; - numRobot = coordinator.getIdleRobots().size(); - - //Start a thread that checks and enforces dependencies at every clock tick - this.setupInferenceCallback(); - - } - - - protected void setupInferenceCallback() { - - Thread TaskAssignmentThread = new Thread("Task Assignment") { - private long threadLastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - - public void run() { - while (true) { - System.out.println("Thread Running"); - if (!taskQueue.isEmpty() && coordinator.getIdleRobots().size() != 0 ) { - MPSolver solverOnline = buildOptimizationProblemWithBNormalized(coordinator); - double [][] assignmentMatrix = solveOptimizationProblem(solverOnline,coordinator,linearWeight); - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+"]"+" is "+ assignmentMatrix[i][j]); - if (assignmentMatrix[i][j] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)); - } - } - } - TaskAllocation(assignmentMatrix,coordinator); - System.out.print("Task to be completed "+ taskQueue.size()); - solverOnline.clear(); - } - - //Sleep a little... - if (CONTROL_PERIOD_Task > 0) { - try { - System.out.println("Thread Sleeping"); - Thread.sleep(CONTROL_PERIOD_Task); } //Thread.sleep(Math.max(0, CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate)); } - catch (InterruptedException e) { e.printStackTrace(); } - } - - long threadCurrentUpdate = Calendar.getInstance().getTimeInMillis(); - EFFECTIVE_CONTROL_PERIOD_task = (int)(threadCurrentUpdate-threadLastUpdate); - threadLastUpdate = threadCurrentUpdate; - - if (cb != null) cb.performOperation(); - - } - } - }; - TaskAssignmentThread.setPriority(Thread.MAX_PRIORITY); - TaskAssignmentThread.start(); - } - - /** - * Start the Task Allocation Algorithm with Greedy Algorithm - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param alpha -> the weight of B and F function in objective function. It is considered as - * B*alpha + (1-alpha)*F - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - public void startTaskAssignmentGreedyAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec) { - //Create meta solver and solver - coordinator = tec; - numRobot = coordinator.getIdleRobots().size(); - //Start a thread that checks and enforces dependencies at every clock tick - this.setupInferenceCallbackGreedy(); - - } - - - protected void setupInferenceCallbackGreedy() { - - Thread TaskAssignmentThread = new Thread("Task Assignment") { - private long threadLastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - - public void run() { - while (true) { - System.out.println("Thread Running"); - if (!taskQueue.isEmpty() && coordinator.getIdleRobots().size() != 0 ) { - double [][] assignmentMatrix = solveOptimizationProblemGreedyAlgorithm(coordinator,linearWeight); - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+"]"+" is "+ assignmentMatrix[i][j]); - if (assignmentMatrix[i][j] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)); - } - } - } - TaskAllocation(assignmentMatrix,coordinator); - System.out.print("Task to be completed "+ taskQueue.size()); - } - //Sleep a little... - if (CONTROL_PERIOD_Task > 0) { - try { - System.out.println("Thread Sleeping"); - Thread.sleep(CONTROL_PERIOD_Task); } //Thread.sleep(Math.max(0, CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate)); } - catch (InterruptedException e) { e.printStackTrace(); } - } - - long threadCurrentUpdate = Calendar.getInstance().getTimeInMillis(); - EFFECTIVE_CONTROL_PERIOD_task = (int)(threadCurrentUpdate-threadLastUpdate); - threadLastUpdate = threadCurrentUpdate; - - if (cb != null) cb.performOperation(); - - } - } - }; - TaskAssignmentThread.setPriority(Thread.MAX_PRIORITY); - TaskAssignmentThread.start(); - } - - - - /** - * Start the Task Allocation Algorithm with Exact Algorithm - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param alpha -> the weight of B and F function in objective function. It is considered as - * B*alpha + (1-alpha)*F - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - public void startTaskAssignmentExactAlgorithm(double alpha,AbstractTrajectoryEnvelopeCoordinator tec) { - //Create meta solver and solver - coordinator = tec; - numRobot = coordinator.getIdleRobots().size(); - linearWeight = alpha; - //Start a thread that checks and enforces dependencies at every clock tick - this.setupInferenceCallbackExact(); - - } - - - protected void setupInferenceCallbackExact() { - - Thread TaskAssignmentThread = new Thread("Task Assignment") { - private long threadLastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - - public void run() { - while (true) { - System.out.println("Thread Running"); - if (!taskQueue.isEmpty() && coordinator.getIdleRobots().size() != 0 ) { - double [][] assignmentMatrix = solveOptimizationProblemExactAlgorithm(coordinator,linearWeight); - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+"]"+" is "+ assignmentMatrix[i][j]); - if (assignmentMatrix[i][j] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)); - } - } - } - TaskAllocation(assignmentMatrix,coordinator); - System.out.print("Task to be completed "+ taskQueue.size()); - } - - //Sleep a little... - if (CONTROL_PERIOD_Task > 0) { - try { - System.out.println("Thread Sleeping"); - Thread.sleep(CONTROL_PERIOD_Task); } //Thread.sleep(Math.max(0, CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate)); } - catch (InterruptedException e) { e.printStackTrace(); } - } - - long threadCurrentUpdate = Calendar.getInstance().getTimeInMillis(); - EFFECTIVE_CONTROL_PERIOD_task = (int)(threadCurrentUpdate-threadLastUpdate); - threadLastUpdate = threadCurrentUpdate; - - if (cb != null) cb.performOperation(); - - } - } - }; - TaskAssignmentThread.setPriority(Thread.MAX_PRIORITY); - TaskAssignmentThread.start(); - } - }//End Class - diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignmentSimulatedAnnealing.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignmentSimulatedAnnealing.java deleted file mode 100644 index 62cc891..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/TaskAssignmentSimulatedAnnealing.java +++ /dev/null @@ -1,2048 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment; - -import java.io.BufferedWriter; -import java.io.File; -import java.io.FileNotFoundException; -import java.io.FileOutputStream; -import java.io.FileWriter; -import java.io.IOException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Calendar; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Iterator; -import java.util.List; -import java.util.Random; -import java.util.TreeSet; -import java.util.logging.Logger; - -import org.apache.commons.lang.ArrayUtils; -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; -import org.metacsp.utility.UI.Callback; -import org.metacsp.utility.logging.MetaCSPLogging; - - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.Geometry; - - -import aima.core.util.datastructure.Pair; -import se.oru.coordination.coordination_oru.AbstractTrajectoryEnvelopeCoordinator; - -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.IndexedDelay; -import se.oru.coordination.coordination_oru.Mission; - -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.TrajectoryEnvelopeCoordinator; - -import se.oru.coordination.coordination_oru.fleetmasterinterface.AbstractFleetMasterInterface; -import se.oru.coordination.coordination_oru.fleetmasterinterface.FleetMasterInterface; -import se.oru.coordination.coordination_oru.fleetmasterinterface.FleetMasterInterfaceLib.CumulatedIndexedDelaysList; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; - -import se.oru.coordination.coordination_oru.util.FleetVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import com.google.ortools.linearsolver.*; - - - -public class TaskAssignmentSimulatedAnnealing { - - //Optimization Problem Parameters - protected int numRobot; - protected int numTask; - protected int dummyRobot; - protected int dummyTask; - protected int numRobotAug; - protected int numTaskAug; - protected int maxNumPaths = 1; - protected double linearWeight = 1; - protected double [][][] costValuesMatrix; - protected double timeOut = Double.POSITIVE_INFINITY; - protected ArrayList taskQueue = new ArrayList (); - - protected String scenario; - protected double [][][] ScenarioAllocation; - //Parameters of weights in Optimization Problem - protected double pathLengthWeight = 1; - protected double arrivalTimeWeight = 0; - protected double tardinessWeight = 0; - //Number of Idle Robots - protected ArrayList IDsIdleRobots = new ArrayList (); - protected ArrayList IDsRealTasks = new ArrayList (); - protected ArrayList IDsAllRobots = new ArrayList (); - protected ArrayList IDsAllTasks = new ArrayList (); - //Path and arrival Time Parameters - //Infinity cost if path to reach a goal note exists - protected double MaxPathLength = 10000000; - //This is the sum of max path length for each robot - protected double sumMaxPathsLength = 1; - //This is the sum of arrival time considering max path length for each robot - protected double sumArrivalTime = 1; - //Parameters of mininum Velocity and Acceleration considering all robots - protected double minMaxVel; - protected double minMaxAcc; - //This is the sum of all tardiness - protected double sumTardiness = 1; - - protected int virtualRobotIDs = Integer.MAX_VALUE; - protected int virtualTaskIDs = Integer.MAX_VALUE; - - //Motion planner and Coordinator Parameters - protected AbstractTrajectoryEnvelopeCoordinator coordinator; - - //Time required by function parameters - protected long timeRequiretoEvaluatePaths; - protected long timeRequiretofillInPall; - protected long timeRequiretoComputeCriticalSection; - protected long timeRequiretoComputePathsDelay; - protected long initialTime; - - protected HashMap pathsToTargetGoal = new HashMap(); - protected ArrayList pathsDrivingRobot = new ArrayList (); - protected HashMap criticalSections = new HashMap(); - - //FleetMaster Interface Parameters - - protected AbstractFleetMasterInterface fleetMasterInterface = null; - protected boolean propagateDelays = false; - protected static Logger metaCSPLogger = MetaCSPLogging.getLogger(TrajectoryEnvelopeCoordinator.class); - - - //Task Allocation Thread Parameters - protected int CONTROL_PERIOD_Task = 15000; - public static int EFFECTIVE_CONTROL_PERIOD_task = 0; - protected FleetVisualization viz = null; - - /** - * Set the number of paths to reach a goal. - * @param maxNumPaths -> number of path to reach a goal - */ - public void setmaxNumPaths(int maxNumPaths) { - - this.maxNumPaths = maxNumPaths; - } - - public void writeMatrix(String filename, double[][][] optimalAssignmentMatrix) { - try { - BufferedWriter bw = new BufferedWriter(new FileWriter((filename),true)); - bw.write("{{"); - for (int i = 0; i < optimalAssignmentMatrix.length; i++) { - for (int j = 0; j < optimalAssignmentMatrix[i].length; j++) { - bw.write("{"); - for (int s = 0; s < maxNumPaths; s++) { - bw.write(optimalAssignmentMatrix[i][j][s]+""); - } - bw.write("}"); - bw.write(","); - - } - bw.write("}"); - bw.write(","); - bw.newLine(); - bw.write("{"); - } - bw.write("-------------"); - bw.newLine(); - bw.flush(); - } catch (IOException e) {} - } - - /** - * Load a Scenario - * @param scenario -> Scenario to load - */ - - public void LoadScenario(String scenario) { - this.scenario = scenario; - } - - /** - * Load an Optimal Task Allocation - * @param scenario -> Scenario to load - */ - - public void LoadScenarioAllocation(double [][][] Allocation) { - this.ScenarioAllocation = Allocation; - } - - /** - * Set the timeOut for the optimazion Problem in minutes. The algorithm will search a solution until this time. - * Use number from 0.1 to 0.6 for seconds - * @param timeOus -> timeout value in minutes - * - */ - public void setTimeOutinMin(double timeOut) { - if(timeOut < 0.1) { - throw new Error("Timeout cannot be negative!"); - } - if(timeOut < 0.6) { - this.timeOut = timeOut*1000; - } - else { - this.timeOut = timeOut*60*1000; - } - - } - - /** - * Set the Coordinator - * @param viz -> Visualization to use - */ - - public void setCoordinator(AbstractTrajectoryEnvelopeCoordinator coordinator) { - this.coordinator = coordinator; - } - - - /** - * Get the Coordinator - * @param viz -> Visualization to use - */ - - public AbstractTrajectoryEnvelopeCoordinator getCoordinator() { - return this.coordinator; - } - - - /** - * Set the weights of cost functions in Optimization Problem. THese must be numbers between 0 and 1. - * @param The path length weight; - * @param The arrival time weight; - * @param The tardiness weight - */ - - public void setCostFunctionsWeight(double pathLengthWeight,double arrivalTimeWeight,double tardinessWeight) { - if(pathLengthWeight <0|| arrivalTimeWeight < 0 || tardinessWeight < 0) { - throw new Error("Weights cannot be numbers less than 0!"); - } - double sumWeight = pathLengthWeight +arrivalTimeWeight + tardinessWeight; - if(sumWeight != 1 || sumWeight < 0 ) { - throw new Error("Weights sum must be equal to 1!"); - } - this.pathLengthWeight = pathLengthWeight; - this.arrivalTimeWeight = arrivalTimeWeight; - this.tardinessWeight = tardinessWeight; - } - - - - /** - * Set the linear weight used in Optimization Problem - * @param viz -> Visualization to use - */ - - public void setLinearWeight(double linearWeight) { - this.linearWeight = linearWeight; - } - - - /** - * Set the Fleet Visualization - * @param viz -> Visualization to use - */ - - public void setFleetVisualization(FleetVisualization viz) { - this.viz = viz; - } - - /** - * Create the IDs for virtual robot only; Them are created starting from IDs of last - * @param viz -> Visualization to use - */ - - private void getAllRobotIDs() { - int virtualRobotID = this.virtualRobotIDs; - for(int i= 0; i < numRobotAug; i++) { - if(i < IDsIdleRobots.size()) { - IDsAllRobots.add(IDsIdleRobots.get(i)); - }else { - IDsAllRobots.add(virtualRobotID); - virtualRobotID = virtualRobotID-1; - this.virtualRobotIDs -=1; - } - } - } - - /** - * Get IDs of all tasks given in Optimization problem - * @return - */ - public ArrayList getTaskIDs() { - ArrayList taskGivenIDs = new ArrayList (); - for(int j=0; j < taskQueue.size();j++) { - taskGivenIDs.add(taskQueue.get(j).getID()); - } - return taskGivenIDs; - } - - private void getRealTaskIDs() { - for(int j=0; j < taskQueue.size();j++) { - IDsRealTasks.add(taskQueue.get(j).getID()); - } - } - - - private void getAllTaskIDs() { - getRealTaskIDs(); - int virtaulTaskID = this.virtualTaskIDs; - for(int i= 0; i < numTaskAug; i++) { - if(i < IDsRealTasks.size()) { - IDsAllTasks.add(IDsRealTasks.get(i)); - }else { - IDsAllTasks.add(virtaulTaskID); - virtaulTaskID = virtaulTaskID-1; - this.virtualTaskIDs -=1; - } - } - - } - - /** - * Check if a goal can be reached by at least one robot of the Fleet - * @param PAll -> Initial PAll - * @return PAll incremented a task cannot be reach by any robot - */ - - private double [][][] checkTargetGoals (double [][][] PAll){ - for (int j= 0; j< PAll[0].length ; j++) { - boolean targetEndCanBeReach = false; - for (int i = 0; i < PAll.length; i++) { - for (int s = 0; s < maxNumPaths; s++) { - if(PAll[i][j][s] != MaxPathLength) { - targetEndCanBeReach = true; - } - } - } - //no robot can reach the target end -> need to introduce a dummy task and robot - if(!targetEndCanBeReach) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - - } - } - - double [][][] PAllAug = new double [numRobotAug][numTaskAug][maxNumPaths]; - int robotID = 0; - int taskID = 0; - for(int i = 0;i < numRobotAug; i++) { - for(int j = 0; j< numTaskAug;j++) { - for (int s = 0; s < maxNumPaths; s++) { - - if(i < PAll.length && j< PAll[0].length) { - PAllAug[i][j][s] = PAll[i][j][s]; - }else { - if(i < IDsAllRobots.size()) { - robotID = IDsAllRobots.get(i); - - - }else { - robotID = this.virtualRobotIDs -1 ; - this.virtualRobotIDs -= 1; - IDsAllRobots.add(robotID); - } - if (j < IDsAllTasks.size()) { - taskID = IDsAllTasks.get(j); - }else { - taskID = virtualTaskIDs - 1; - this.virtualTaskIDs -= 1; - IDsAllTasks.add(taskID); - } - PAllAug[i][j][s] = 1; - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s, null); - - } - } - - } - } - return PAllAug; - } - - /** - * The default footprint used for robots if none is specified. - * NOTE: coordinates in footprints must be given in in CCW or CW order. - */ - public static Coordinate[] DEFAULT_FOOTPRINT = new Coordinate[] { - new Coordinate(-1.7, 0.7), //back left - new Coordinate(-1.7, -0.7), //back right - new Coordinate(2.7, -0.7), //front right - new Coordinate(2.7, 0.7) //front left - }; - - /** - * Set the minimum values of maximum velocity and acceleration considering all robots of the fleet - * @param MaxVel -> minimum of maximum velocity of all robot models; - * @param MaxAccel -> minimum of maximum acceleration of all robot models; - */ - - public void setminMaxVelandAccel(double MaxVel,double MaxAccel) { - this.minMaxVel = MaxVel; - this.minMaxAcc = MaxAccel; - } - - class SortByDeadline implements Comparator{ - @Override - public int compare(Task task1, Task task2) { - return (int) (task1.getDeadline()-task2.getDeadline()); - } - } - - - private void checkOnTaskDeadline() { - ArrayList taskArray =new ArrayList (); - for(int j=0; j < taskQueue.size(); j++ ) { - taskArray.add(taskQueue.get(j)); - } - taskArray.sort(new SortByDeadline()); - for(int i=0;i < IDsIdleRobots.size(); i++) { - int index = taskQueue.indexOf(taskArray.get(i)); - if(taskQueue.get(index).getDeadline() != -1) { - taskQueue.get(index).setPriority(true); - } - - } - } - - - - /** - * Enable and initialize the fleetmaster library to estimate precedences to minimize the overall completion time. - * Note: this function should be called before placing the first robot. - * ATTENTION: If dynamic_size is false, then the user should check that all the paths will lay in the given area. - * @param origin_x The x coordinate (in meters and in global inertial frame) of the lower-left pixel of fleetmaster GridMap. - * @param origin_y The y coordinate (in meters and in global inertial frame) of the lower-left pixel of fleetmaster GridMap. - * @param origin_theta The theta coordinate (in rads) of the lower-left pixel map (counterclockwise rotation). Many parts of the system currently ignore it. - * @param resolution The resolution of the map (in meters/cell), 0.01 <= resolution <= 1. It is assumed this parameter to be global among the fleet. - * The highest the value, the less accurate the estimation, the lowest the more the computational effort. - * @param width Number of columns of the map (>= 1) if dynamic sizing is not enabled. - * @param height Number of rows of the map (>= 1) if dynamic sizing is not enabled. - * @param dynamic_size If true, it allows to store only the bounding box containing each path. - * @param propagateDelays If true, it enables the delay propagation. - * @param debug If true, it enables writing to screen debugging info. - */ - public void instantiateFleetMaster(double origin_x, double origin_y, double origin_theta, double resolution, long width, long height, boolean dynamic_size, boolean propagateDelays, boolean debug) { - this.fleetMasterInterface = new FleetMasterInterface(origin_x, origin_y, origin_theta, resolution, width, height, dynamic_size, debug); - this.fleetMasterInterface.setDefaultFootprint(DEFAULT_FOOTPRINT); - this.propagateDelays = propagateDelays; - } - - /** - * Enable and initialize the fleetmaster library to estimate precedences to minimize the overall completion time - * while minimizing the computational requirements (bounding box are used to set the size of each path-image). - * Note: this function should be called before placing the first robot. - * @param resolution The resolution of the map (in meters/cell), 0.01 <= resolution <= 1. It is assumed this parameter to be global among the fleet. - * The highest the value, the less accurate the estimation, the lowest the more the computational effort. - * @param propagateDelays If true, it enables the delay propagation. - */ - public void instantiateFleetMaster(double resolution, boolean propagateDelays) { - this.fleetMasterInterface = new FleetMasterInterface(0., 0., 0., resolution, 100, 100, true, false); - this.fleetMasterInterface.setDefaultFootprint(DEFAULT_FOOTPRINT); - this.propagateDelays = propagateDelays; - } - - - /** - * Add a path to the fleetmaster interface - * @param robotID -> The ID of the robot - * @param pathID -> the ID of the path - * @param pss -> the path expressed as a PoseSteering vector - * @param boundingBox -> the bounding box of the path - * @param coordinates -> footprint of the robot - */ - protected void addPath(int robotID, int pathID, PoseSteering[] pss, Geometry boundingBox, Coordinate... coordinates) { - if (!fleetMasterInterface.addPath(robotID, pathID, pss, boundingBox, coordinates)) - metaCSPLogger.severe("Unable to add the path to the fleetmaster gridmap. Check if the map contains the given path."); - } - - - - /** - * Delete the path from the fleetmaster interface - * @param pathID -> The ID of the path to remove - */ - protected void removePath(int pathID){ - if (!fleetMasterInterface.clearPath(pathID)) - metaCSPLogger.severe("Unable to remove the path to the fleetmaster gridmap. Check if the map contains the given path."); - } - - - protected CumulatedIndexedDelaysList toIndexedDelaysList(TreeSet delays, int max_depth) { - //Handle exceptions - if (delays == null) { - metaCSPLogger.severe("Invalid input in function toPropagationTCDelays!!"); - throw new Error("Invalid input in function toPropagationTCDelays!!"); - } - if (delays.isEmpty() || max_depth < 1) return new CumulatedIndexedDelaysList(); - - //Cast the type - ArrayList indices = new ArrayList(); - ArrayList values = new ArrayList(); - Iterator it = delays.descendingIterator(); - IndexedDelay prev = delays.last(); - while (it.hasNext()) { - IndexedDelay current = it.next(); - //Check unfeasible values - if (current.getValue() == Double.NaN) { - metaCSPLogger.severe("NaN input in function toPropagationTCDelays!!"); - throw new Error("NaN input in function toPropagationTCDelays!!"); - } - if (current.getValue() == Double.NEGATIVE_INFINITY) { - metaCSPLogger.severe("-Inf input in function toPropagationTCDelays!!"); - throw new Error("-Inf input in function toPropagationTCDelays!!"); - } - if (prev.getIndex() < current.getIndex()) { - metaCSPLogger.severe("Invalid IndexedDelays TreeSet!!"); - throw new Error("Invalid IndexedDelays TreeSet!!"); - } - - //Update the value only if positive and only if the index is lower than the max depth - if (current.getValue() > 0 && current.getValue() < Double.MAX_VALUE && current.getIndex() < max_depth) { - if (values.size() == 0) { - //Add the index the first time its value is positive - indices.add(new Long(current.getIndex())); - values.add(current.getValue()); - } - else if (prev.getIndex() == current.getIndex()) - //Handle multiple delays in the same critical point - values.set(values.size()-1, values.get(values.size()-1) + current.getValue()); - else { - //Add the cumulative value if it is not the first. - indices.add(new Long(current.getIndex())); - values.add(values.get(values.size()-1) + current.getValue()); - } - } - prev = current; - } - CumulatedIndexedDelaysList propTCDelays = new CumulatedIndexedDelaysList(); - if (indices.size() > 0) { - propTCDelays.size = indices.size(); - propTCDelays.indices = ArrayUtils.toPrimitive((Long[]) indices.toArray(new Long[indices.size()])); - ArrayUtils.reverse(propTCDelays.indices); - propTCDelays.values = ArrayUtils.toPrimitive((Double[]) values.toArray(new Double[values.size()])); - ArrayUtils.reverse(propTCDelays.values); - } - return propTCDelays; - } - - - protected Pair estimateTimeToCompletionDelays(int path1ID,PoseSteering[] pss1, TreeSet delaysRobot1, int path2ID,PoseSteering[] pss2, TreeSet delaysRobot2, CriticalSection cs) { - - if (this.fleetMasterInterface != null && fleetMasterInterface.checkPathHasBeenAdded(path1ID)&& fleetMasterInterface.checkPathHasBeenAdded(path2ID)) { - CumulatedIndexedDelaysList te1TCDelays = toIndexedDelaysList(delaysRobot1, pss1.length); - //metaCSPLogger.info("[estimateTimeToCompletionDelays] te1TCDelays: " + te1TCDelays.toString()); - CumulatedIndexedDelaysList te2TCDelays = toIndexedDelaysList(delaysRobot2, pss2.length); - //metaCSPLogger.info("[estimateTimeToCompletionDelays] te2TCDelays: " + te2TCDelays.toString()); - return fleetMasterInterface.queryTimeDelay(cs, te1TCDelays, te2TCDelays,pss1,pss2); - } - - return new Pair (Double.NaN, Double.NaN); - } - - - - /** - * Add a Task to Mission set - * @param task -> the task to add - * @return -> true if task is added correctly, otherwise false - */ - public boolean addTask(Task task) { - if (task == null) { - metaCSPLogger.severe("No task to add. Please give a correct Task."); - throw new Error("Cannot add the task"); - } - boolean TaskisAdded = taskQueue.add(task); - return TaskisAdded; - } - - /** - * Remove a task from the queue - * @param task The task to remove - * @return true if task is removed correctly, otherwise false - */ - public boolean removeTask(Task task) { - return taskQueue.remove(task); - } - - - - /** - * Get the task from Mission set in the index position - * @param index -> the index position of the task - * @return -> the task in index position - */ - public Task getTask(int index) { - if (index < 0 || index > taskQueue.size()) { - metaCSPLogger.severe("Wrong index."); - throw new Error("The task" + index + "not exist"); - }else { - return taskQueue.get(index); - } - } - - /** - * Compute the number of Dummy Robots and/or Tasks. Consider the possibility to have a different number of robots (N) and tasks (M). If N > M, dummy tasks are - * considered, where a dummy task is a task for which a robot stay in starting position; while if M > N dummy robots - * are considered, where a dummy robot is only a virtual robot. - * @param numRobot Number of robots - * @param numTasks Number of tasks - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - private void dummyRobotorTask(int numRobot, int numTasks,AbstractTrajectoryEnvelopeCoordinator tec) { - numRobotAug = numRobot; - numTaskAug = numTasks; - //Restore initial value for dummy robot and task - dummyTask = 0; - dummyRobot = 0; - //Considering the possibility to have n != m - //If n > m -> we have dummy robot, so at some robot is assign the task to stay in starting position - if (numRobot > numTasks) { - dummyTask = numRobot - numTasks; - numTaskAug = numTasks + dummyTask; - } - else if (numRobot < numTasks) { - dummyRobot = numTasks - numRobot; - numRobotAug = numRobot + dummyRobot; - } - //This second check is used when we have particular cases due to Robot Type and Task Type - //Only if we have already a dummy task this check can be avoided - //If A robot cannot be assigned to any task - if (dummyTask == 0 || dummyRobot != 0) { - for (int i = 0; i < numRobot; i++) { - boolean flagAllocateRobot = false; - for (int j = 0; j < numTasks; j++) { - //check if robot can be assigned to one task - //if (taskQueue.get(j).getTaskType() == tec.getRobotType(IDsIdleRobots[i])) { - if (taskQueue.get(j).isCompatible(tec.getRobot(IDsIdleRobots.get(i)))) { - flagAllocateRobot = true; - - } - } - //the robot cannot be assigned to any task -> add a dummy robot and task - if (!flagAllocateRobot) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - } - } - } - //Only if we have already a dummy robot this check can be avoided - //If A task cannot be assigned to any robot - if (dummyRobot == 0 || dummyTask != 0) { - for (int i = 0; i < numTasks; i++) { - boolean flagAllocateTask = false; - for (int j = 0; j < numRobot; j++) { - //check if task can be assigned to one robot - //if (taskQueue.get(i).getTaskType() == tec.getRobotType(IDsIdleRobots[j])) { - if (taskQueue.get(i).isCompatible(tec.getRobot(IDsIdleRobots.get(j)))) { - flagAllocateTask = true; - } - } - //the task cannot be assigned to any robot -> add a dummy robot and task - if (!flagAllocateTask) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - } - } - } - } - - - /** - * Transform a 1D array of MPVariable into a 3D MATRIX - * @param numRobot -> Number of robots - * @param numTasks -> Number of tasks - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblem}, {@link #buildOptimizationProblemWithB} or {@link #buildOptimizationProblemWithBNormalized} - * @return 3D Matrix of Decision Variable of the input problem - */ - private MPVariable [][][] tranformArray(MPSolver optimizationProblem) { - //Take the vector of Decision Variable from the Optimization Problem - MPVariable [] array1D = optimizationProblem.variables(); - MPVariable [][][] decisionVariable = new MPVariable [numRobotAug][numTaskAug][maxNumPaths]; - //Store them in a 2D Matrix - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - for (int s = 0; s < maxNumPaths; s++) { - decisionVariable[i][j][s] = array1D[i*numTaskAug*maxNumPaths+j*maxNumPaths+s]; - } - - } - } - return decisionVariable; - } - - /** - * Impose a constraint on the optimization problem on previous optimal solution in order to not consider more it - * @param optimizationProblem -> An optimization problem defined with {@link #buildOptimizationProblem},{@link #buildOptimizationProblemWithB} or {@link #buildOptimizationProblemWithBNormalized} in which a solution is found - * @param assignmentMatrix -> The Assignment Matrix of the actual optimal solution - * @return Optimization Problem updated with the new constraint on previous optimal solution found - */ - private MPSolver constraintOnPreviousSolution(MPSolver optimizationProblem, double [][][] assignmentMatrix) { - //Take decision Variable from Optimization Problem - MPVariable [][][] DecisionVariable = tranformArray(optimizationProblem); - //Initialize a Constraint - MPConstraint c2 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY,1); - //Define the actual optimal solution as a Constraint in order to not consider more it - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTaskAug; j++) { - for(int s = 0;s < maxNumPaths; s++) { - if (assignmentMatrix[i][j][s] >0) { - c2.setCoefficient(DecisionVariable[i][j][s],1); - }else { - c2.setCoefficient(DecisionVariable[i][j][s],0); - } - } - } - } - //Return the updated Optimization Problem - return optimizationProblem; - } - - - - - - /** - * Store the solution of a optimization problem in a Matrix - * @param numRobot -> Number of robots - * @param numTasks -> Number of tasks - * @param optimizationProblem -> A solved optimization problem - * @return Assignment matrix for the optimization problem given as input - */ - - private double [][][] saveAssignmentMatrix(int numRobot,int numTasks,MPSolver optimizationProblem){ - //Take the decision variable from the optimization problem - MPVariable [][][] decisionVariable = tranformArray(optimizationProblem); - double [][][] assignmentMatrix = new double [numRobot][numTasks][maxNumPaths]; - //Store decision variable values in a Matrix - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTasks; j++) { - for(int s = 0;s < maxNumPaths; s++) { - assignmentMatrix[i][j][s] = decisionVariable[i][j][s].solutionValue(); - } - } - } - return assignmentMatrix; - } - - - /** - * Evaluate the cost associated to the path length for the a couple of robot and task. - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param robot -> The Robot ID - * @param task -> The Task ID - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The cost associated to the path length for the couple of robot and task given as input - */ - private double evaluatePathLength(int robotID , int taskID, int path, AbstractTrajectoryEnvelopeCoordinator tec){ - //Evaluate the path length for the actual couple of task and ID - //Initialize the path length to infinity - double pathLength = MaxPathLength; - //take index positon of robotID in Robot set - int robotindex = IDsIdleRobots.indexOf(robotID); - // Only for real robots and tasks - if (IDsIdleRobots.contains(robotID) && IDsRealTasks.contains(taskID)) { - //Take the state for the i-th Robot - - RobotReport rr = tec.getRobotReport(IDsIdleRobots.get(robotindex)); - if (rr == null) { - metaCSPLogger.severe("RobotReport not found for Robot" + robotID + "."); - throw new Error("RobotReport not found for Robot" + robotID + "."); - } - //Evaluate the path from the Robot Starting Pose to Task End Pose - int taskIndex = IDsRealTasks.indexOf(taskID); - AbstractMotionPlanner rsp = tec.getMotionPlanner(robotID).getCopy(false); - rsp.setStart(rr.getPose()); - rsp.setGoals(taskQueue.get(taskIndex).getStartPose(),taskQueue.get(taskIndex).getGoalPose()); - rsp.setFootprint(tec.getFootprint(robotID)); - - if (!rsp.plan()) { - System.out.println("Robot" + robotID +" cannot reach the Target End of Task " + taskID); - //the path to reach target end not exits - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, null); - //Infinity cost is returned - return pathLength; - } - - //If the path exists - //Take the Pose Steering representing the path - PoseSteering[] pss = rsp.getPath(); - //Add the path to the FleetMaster Interface -> this is necessary for F function - addPath(robotID, pss.hashCode(), pss, null, tec.getFootprint(robotID)); - //Save the path to Task in the path set - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, pss); - //Take the Path Length - Mission m1 = new Mission(robotID,pss); - Missions.enqueueMission(m1); - pathLength = Missions.getPathLength(pss); - } else { //There also virtual robot and task are considered - //There are considered real robot and dummy task - if (numRobot >= numTask && IDsIdleRobots.contains(robotID)){ //dummy task -> The Robot receive the task to stay in starting position - //The second condition is used in the special case in which we have that one robot cannot be - //assigned to any tasks due to its type, so we must add a dummy robot and a dummy task, but we - //Create the task to stay in robot starting position - PoseSteering[] dummyTask = new PoseSteering[1]; - //Take the state for the i-th Robot - RobotReport rr = tec.getRobotReport(robotID); - if (rr == null) { - metaCSPLogger.severe("RobotReport not found for Robot" + robotID + "."); - throw new Error("RobotReport not found for Robot" + robotID + "."); - } - //take the starting position of the robot - dummyTask[0] = new PoseSteering(rr.getPose(),0); - //Add the path to the FleetMaster Interface -> so it can be considered as an obstacle from - //the motion planner - addPath(robotID, dummyTask.hashCode(), dummyTask, null, tec.getFootprint(robotID)); - //Save the path to Dummy Task - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, dummyTask); - //Consider a minimal pathLength - pathLength = 1; - Mission m1 = new Mission(robotID,dummyTask); - Missions.enqueueMission(m1); - return pathLength; - } - else { //There are considered dummy robot and real task - //dummy robot -> Consider a only virtual Robot - PoseSteering[] dummyRobot = new PoseSteering[1]; - dummyRobot[0] = new PoseSteering(taskQueue.get(0).getGoalPose(),0); - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, dummyRobot); - pathLength = 1; - Mission m1 = new Mission(robotID,dummyRobot); - Missions.enqueueMission(m1); - return pathLength; - } - } - - return pathLength; - } - - - - /** - * Evaluate the PAll matrix with a precomputed Scenario, that is a matrix that contains all path for each possible combination of robot - * and task - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The PAll matrix - */ - private double [][][] evaluatePAllWithScenario(AbstractTrajectoryEnvelopeCoordinator tec){ - - //Evaluate the path length for the actual couple of task and ID - //Initialize the sum of max paths lengths and time to do it for each robot - //This cost are used then for normalizing cost - double sumPathsLength = 0; - double sumArrivalTime = 0; - Missions.loadScenario(scenario); - - double [][][] PAll = new double[numRobotAug][numTaskAug][maxNumPaths]; - for (int robotID : IDsAllRobots) { - int robotindex = IDsAllRobots.indexOf(robotID); - double maxPathLength = 1; - for (int taskID : IDsAllTasks ) { - int taskIndex = IDsAllTasks.indexOf(taskID); - double pathLength = MaxPathLength; - //Evaluate path Length - boolean typesAreEqual = false; - if (taskIndex < numTask && robotindex < numRobot ) { - typesAreEqual = taskQueue.get(taskIndex).isCompatible(tec.getRobot(robotID)); - - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - for(int path = 0;path < maxNumPaths; path++) { - if(typesAreEqual) { - if(IDsIdleRobots.contains(robotID)) { - - }else { - - } - - int cont= 0; - - - if(IDsIdleRobots.contains(robotID)&& taskIndex < taskQueue.size()) { - - while(cont < Missions.getMissions(robotID).size() ) { - if( ArrayUtils.isEquals(Missions.getMission(robotID, cont).getToPose().toString(),taskQueue.get(taskIndex).getGoalPose().toString())) { - PoseSteering[] pss = Missions.getMission(robotID, cont).getPath(); - addPath(robotID, pss.hashCode(), pss, null, tec.getFootprint(robotID)); - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, pss); - pathLength = Missions.getPathLength(pss); - } - cont +=1; - - } - }else { - if (numRobot >= numTask && IDsIdleRobots.contains(robotID)){ - PoseSteering[] pss = new PoseSteering[] {new PoseSteering(tec.getRobotReport(robotID).getPose(),0)}; - pathLength =1 ; - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, pss); - - }else { - PoseSteering[] pss = new PoseSteering[] {new PoseSteering(taskQueue.get(0).getGoalPose(),0)}; - pathLength =1 ; - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, pss); - } - } - - - - - if ( pathLength > maxPathLength && pathLength != MaxPathLength) { - maxPathLength = pathLength; - } - }else { - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, null); - } - PAll[robotindex][taskIndex][path] = pathLength; - }//For path - }//For task - //Sum the max path length for each robot - sumPathsLength += maxPathLength; - //Sum the arrival time for the max path length - sumArrivalTime += computeArrivalTime(maxPathLength,this.minMaxVel,this.minMaxAcc); - } - double [][][] PAllAug = checkTargetGoals(PAll); - //Save the sum of max paths length to normalize path length cost - this.sumMaxPathsLength = sumPathsLength; - //Save the sum of arrival time considering max paths length to normalize delay cost - this.sumArrivalTime = sumArrivalTime; - - //Return the cost of path length - - return PAllAug; - } - - - /** - * Evaluate the PAll matrix, that is a matrix that contains all path for each possible combination of robot - * and task - * If a path between a couple of robot and task does not exists the cost is consider infinity. - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return The PAll matrix - */ - private double [][][] evaluatePAll(AbstractTrajectoryEnvelopeCoordinator tec){ - - //Evaluate the path length for the actual couple of task and ID - //Initialize the sum of max paths lengths and time to do it for each robot - //This cost are used then for normalizing cost - double sumPathsLength = 0; - double sumArrivalTime = 0; - double [][][] PAll = new double[numRobotAug][numTaskAug][maxNumPaths]; - - long timeInitial = Calendar.getInstance().getTimeInMillis(); - if(scenario != null) { - double [][][] PAllScenario = evaluatePAllWithScenario(tec); - return PAllScenario; - } - for (int robotID : IDsAllRobots) { - double maxPathLength = 1; - int robotIndex = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int taskIndex = IDsAllTasks.indexOf(taskID); - - //Evaluate path Length - boolean typesAreEqual = false; - if (IDsIdleRobots.contains(robotID) && IDsRealTasks.contains(taskID) ) { - typesAreEqual = taskQueue.get(taskIndex).isCompatible(tec.getRobot(robotID)); - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - for(int path = 0;path < maxNumPaths; path++) { - final int pathID = path; - if(typesAreEqual) { // only if robot and typoe have the same types - new Thread("Robot" + robotID) { - public void run() { - evaluatePathLength(robotID,taskID,pathID,tec); - } - }.start(); - //Take time to evaluate the path - - } - else { - pathsToTargetGoal.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path, null); - } - } - }//For Task - - } - boolean allResultsReady = false; - while (!allResultsReady) { - allResultsReady = true; - - for (int robotID : IDsIdleRobots) { - for (int task : IDsAllTasks) { - for(int path=0;path < maxNumPaths ; path ++) { - if (!pathsToTargetGoal.containsKey(robotID*numTaskAug*maxNumPaths+task*maxNumPaths+path) ) { - allResultsReady = false; - } - } - - } - try { Thread.sleep(500); } - catch (InterruptedException e) { - e.printStackTrace(); - } - } - - } - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoEvaluatePaths = timeRequiretoEvaluatePaths + timeRequired; - long timeInitial2 = Calendar.getInstance().getTimeInMillis(); - for (int robotID : IDsAllRobots) { - int robotindex = IDsAllRobots.indexOf(robotID); - double maxPathLength = 1; - for (int taskID : IDsAllTasks ) { - int taskIndex = IDsAllTasks.indexOf(taskID); - double pathLength = MaxPathLength; - //Evaluate path Length - boolean typesAreEqual = false; - if (taskIndex < numTask && robotindex < numRobot ) { - typesAreEqual = taskQueue.get(taskIndex).isCompatible(tec.getRobot(robotID)); - } - else { - //Considering a dummy robot or a dummy task -> they don't have type - typesAreEqual = true; - } - for(int path = 0;path < maxNumPaths; path++) { - if(typesAreEqual) { - if(pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path)==null) { - pathLength = MaxPathLength; - }else { - pathLength=Missions.getPathLength(pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+path)); - } - - if ( pathLength > maxPathLength && pathLength != MaxPathLength) { - maxPathLength = pathLength; - } - } - - PAll[robotindex][taskIndex][path] = pathLength; - }//For path - }//For task - //Sum the max path length for each robot - sumPathsLength += maxPathLength; - //Sum the arrival time for the max path length - sumArrivalTime += computeArrivalTime(maxPathLength,this.minMaxVel,this.minMaxAcc); - } - //Take the time to fill in the PAll Matrix - long timeFinal2 = Calendar.getInstance().getTimeInMillis(); - long timeRequired2 = timeFinal2- timeInitial2; - timeRequiretofillInPall = timeRequiretofillInPall + timeRequired2; - double [][][] PAllAug = checkTargetGoals(PAll); - //Save the sum of max paths length to normalize path length cost - this.sumMaxPathsLength = sumPathsLength; - //Save the sum of arrival time considering max paths length to normalize delay cost - this.sumArrivalTime = sumArrivalTime; - //Return the cost of path length - Missions.saveScenario("ProvaScenario"); - return PAllAug; - } - /** - * Evaluate the cost associated to time delay on completion of a task for a specific robot, due to interference with other robot - * and precedence constraints. The cost is evaluated considering the intersection between the path of robot i-th - * with the paths of other robots, considering the actual Assignment, but also paths related to already driving robot - * are considered. - * @param robot -> The i-th Robot - * @param task -> The j-th Task - * @param pathID -> The s-th path - * @param assignmentMatrix -> The Assignment Matrix related to a solution of the optimization problem - * @param tec -> an Abstract Trajectory Envelope Coordinator - * @return The cost associated to the delay on completion of task j for robot i due to interference with other robot - */ - private double evaluatePathDelay(int robotID ,int taskID,int pathID,double [][][] assignmentMatrix,AbstractTrajectoryEnvelopeCoordinator tec){ - CriticalSection[][][][] cssMatrix = new CriticalSection [IDsIdleRobots.size()][IDsRealTasks.size()][maxNumPaths][1]; - Integer [] arrayIDs = new Integer[2] ; - PrintStream fileStream1 = null; - PrintStream fileStream2 = null; - try { - fileStream1 = new PrintStream(new FileOutputStream("CriticalSections.txt",true)); - fileStream2 = new PrintStream(new FileOutputStream("PathDelay.txt",true)); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - long timeInitial2 = 0; - - int robotIndex = IDsAllRobots.indexOf(robotID); - int taskIndex = IDsAllTasks.indexOf(taskID); - //Evaluate the delay time on completion time for the actual couple of task and ID - //Initialize the time delay - double delay = 0; - //Considering the Actual Assignment - if (assignmentMatrix[robotIndex][taskIndex][pathID]>0) { - // Only for real robots and tasks - if (IDsIdleRobots.contains(robotID) && IDsRealTasks.contains(taskID)) { - //Take the Pose steering relate to i-th robot and j-th task from path set - //PoseSteering[] pss1 = pathsToTargetGoalTotal.get((robot-1)*numTaskAug*maxNumPaths + task*maxNumPaths +pathID); - PoseSteering[] pss1 = pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID); - if(pss1 == null) { - return delay; - } - //Initialize Array of delays for the two robots - TreeSet te1TCDelays = new TreeSet() ; - TreeSet te2TCDelays = new TreeSet() ; - //Compute the spatial Envelope for the i-th Robot - SpatialEnvelope se1 = TrajectoryEnvelope.createSpatialEnvelope(pss1,tec.getFootprint(robotID)); - //Evaluate other path depending from the Assignment Matrix - for(int secondRobotID : IDsIdleRobots) { - int secondRobotIndex = IDsIdleRobots.indexOf(secondRobotID); - for(int secondTaskID: IDsRealTasks) { - int secondTaskIndex = IDsRealTasks.indexOf(secondTaskID); - for(int s = 0;s < maxNumPaths; s++) { - if (assignmentMatrix [secondRobotIndex][secondTaskIndex][s] > 0 && secondRobotID != robotID && secondTaskID != taskID) { - //Take the path of this second robot from path set - - PoseSteering[] pss2 = pathsToTargetGoal.get(secondRobotID*numTaskAug*maxNumPaths + secondTaskID*maxNumPaths+s); - if (pss2 != null) {//is == null if robotType is different to Task type - //Evaluate the Spatial Envelope of this second Robot - SpatialEnvelope se2 = TrajectoryEnvelope.createSpatialEnvelope(pss2,tec.getFootprint(secondRobotID)); - long timeInitial = Calendar.getInstance().getTimeInMillis(); - //Compute the Critical Section between this 2 robot - arrayIDs[0] = robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID; - arrayIDs[1] = secondRobotID*numTaskAug*maxNumPaths + secondTaskID*maxNumPaths+s; - CriticalSection [] css = new CriticalSection[1]; - /* - if(criticalSections.containsKey(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID) ) { - css= criticalSections.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID)[secondRobotIndex][secondTaskIndex][s]; - if(css.length == 1) { - css = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, se2,true, Math.min(tec.getFootprintPolygon(robotID).getArea(),tec.getFootprintPolygon(secondRobotID).getArea())); - - } - }else { - css = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, se2,true, Math.min(tec.getFootprintPolygon(robotID).getArea(),tec.getFootprintPolygon(secondRobotID).getArea())); - cssMatrix[secondRobotIndex][secondTaskIndex][s] = css; - criticalSections.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID, cssMatrix); - //Evaluate the time to compute critical Section - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoComputeCriticalSection = timeRequiretoComputeCriticalSection + timeRequired; - fileStream1.println(timeRequired+""); - - } - */ - css = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, se2,true, Math.min(tec.getFootprintPolygon(robotID).getArea(),tec.getFootprintPolygon(secondRobotID).getArea())); - cssMatrix[secondRobotIndex][secondTaskIndex][s] = css; - criticalSections.put(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+pathID, cssMatrix); - //Evaluate the time to compute critical Section - long timeFinal = Calendar.getInstance().getTimeInMillis(); - long timeRequired = timeFinal- timeInitial; - timeRequiretoComputeCriticalSection = timeRequiretoComputeCriticalSection + timeRequired; - fileStream1.println(timeRequired+""); - - timeInitial2 = Calendar.getInstance().getTimeInMillis(); - //Compute the delay due to precedence constraint in Critical Section - for (int g = 0; g < css.length; g++) { - Pair a1 = estimateTimeToCompletionDelays(pss1.hashCode(),pss1,te1TCDelays,pss2.hashCode(),pss2,te2TCDelays, css[g]); - double delayCriticalSection = a1.getFirst(); - if(delayCriticalSection < 0 ) { - delay += 0; - }else if(delayCriticalSection == Double.POSITIVE_INFINITY) { - delay += 10000; - }else { - delay += delayCriticalSection; - } - } - - //Take the paths of driving robots from coordinator - pathsDrivingRobot = tec.getDrivingEnvelope(); - //Evaluate the delay time due to already driving robots - for(int k = 0; k < pathsDrivingRobot.size(); k++) { - CriticalSection [] cssDrivingRobot = AbstractTrajectoryEnvelopeCoordinator.getCriticalSections(se1, pathsDrivingRobot.get(k),true, Math.min(tec.getFootprintPolygon(robotID).getArea(),tec.getFootprintPolygon(secondRobotID).getArea())); - for (int b = 0; b < cssDrivingRobot.length; b++) { - Pair a1 = estimateTimeToCompletionDelays(pss1.hashCode(),pss1,te1TCDelays,pathsDrivingRobot.get(k).getPath().hashCode(),pathsDrivingRobot.get(k).getPath(),te2TCDelays, cssDrivingRobot[b]); - double delayCriticalSection = a1.getFirst(); - if(delayCriticalSection < 0 ) { - delay += 0; - }else if(delayCriticalSection == Double.POSITIVE_INFINITY) { - delay += 10000; - }else { - delay += delayCriticalSection; - } - } - } - - long timeFinal2 = Calendar.getInstance().getTimeInMillis(); - long timeRequired2 = timeFinal2- timeInitial2; - timeRequiretoComputePathsDelay = timeRequiretoComputePathsDelay + timeRequired2; - fileStream2.println(timeRequired2+""); - } - } - } - } - } - - } else { //There also virtual robot and task are considered - //the delay associated to dummy robot and task is considerd 0 - return delay; - } - } - //return the delay for the i-th robot and the j-th task due to interference with other robots - return delay; - } - - - - /** - * Compute the arrival time to a task for a specified robot - * @param pathLength -> The max path for each robot - * @return The time to drive the path - */ - private double computeArrivalTime(double pathLength,double vel,double acc){ - //Compute the arrival time of this path, considering a robot alone with a velocity trapezoidal model - double arrivalTime = pathLength/vel + vel/acc; - //Return the arrival time - return arrivalTime; - } - - /** - * Compute the arrival for all robots in fleet to all possible tasks - * @param pathLength -> The max path for each robot - * @return The time to drive the path - */ - private double [][][] computeArrivalTimeFleet(double[][][]PAll,AbstractTrajectoryEnvelopeCoordinator tec){ - //Compute the arrival time of this path, considering a robot alone with a velocity trapezoidal model - double [][][] arrivalTimeMatrix = new double [numRobotAug][numTaskAug][maxNumPaths]; - for (int robotID : IDsIdleRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsRealTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int path = 0;path < maxNumPaths; path++) { - double vel = tec.getRobot(robotID).getForwardModel().getVel(); - double acc = tec.getRobot(robotID).getForwardModel().getAcc(); - double arrivalTime = computeArrivalTime(PAll[i][j][path],vel,acc); - arrivalTimeMatrix[i][j][path] = arrivalTime; - } - - } - } - - //Return the arrival time - return arrivalTimeMatrix; - } - - - /** - * Evaluate the tardiness in completion of a task for a single robot . The tardiness is the defined as the further time required to complete a task - * after the deadline - * @param pathLength -> path length - * @param task -> the task j-th - * @return - */ - - private double computeTardiness(int robotID,int taskID,double pathLength, AbstractTrajectoryEnvelopeCoordinator tec) { - double tardiness = 0; - if(IDsRealTasks.contains(taskID)) { - int taskIndex = IDsRealTasks.indexOf(taskID); - if (taskQueue.get(taskIndex).isDeadlineSpecified()) { // Compute tardiness only if specified in task constructor - double deadline = taskQueue.get(taskIndex).getDeadline(); //Expressed in seconds - double vel = tec.getRobot(robotID).getForwardModel().getVel(); - double acc = tec.getRobot(robotID).getForwardModel().getVel(); - double completionTime = computeArrivalTime(pathLength,vel,acc) + taskQueue.get(taskIndex).getOperationTime(); - tardiness = Math.max(0, (completionTime-deadline)); - } - } - return tardiness; - } - - - /** - * Evaluate the tardiness in completion of a task . The tardiness is the defined as the further time required to complete a task - * after the deadline - * @param pathLength -> path length - * @param task -> the task j-th - * @return - */ - - private double[][][] computeTardinessFleet(double [][][]PAll,AbstractTrajectoryEnvelopeCoordinator tec) { - double tardiness = 0; - - double [][][] tardinessMatrix = new double [numRobotAug][numTaskAug][maxNumPaths]; - for (int robotID : IDsIdleRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsRealTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int path = 0;path < maxNumPaths; path++) { - if (taskQueue.get(j).isDeadlineSpecified()) { // Compute tardiness only if specified in task constructor - double deadline = taskQueue.get(j).getDeadline(); //Expressed in seconds - double vel = tec.getRobot(robotID).getForwardModel().getVel(); - double acc = tec.getRobot(robotID).getForwardModel().getVel(); - double completionTime = computeArrivalTime(PAll[i][j][path],vel,acc) + taskQueue.get(j).getOperationTime(); - tardiness = Math.max(0, (completionTime-deadline)); - tardinessMatrix[i][j][path] = tardiness; - sumTardiness += tardiness; - } - } - - } - } - return tardinessMatrix; - } - - /** - * Evaluate the overall B function, that is the function that consider interference free costs - * Costs considered: - * 1) Path Length - * 2) Tardiness - * Each cost is already normalized; - * @param PAll - * @return - */ - private double [][][] evaluateBFunction(double [][][]PAll,AbstractTrajectoryEnvelopeCoordinator tec){ - double [][][] tardinessMatrix = computeTardinessFleet(PAll,tec); - double [][][] BFunction = new double [numRobotAug][numTaskAug][maxNumPaths]; - costValuesMatrix = new double [numRobotAug][numTaskAug][maxNumPaths]; - if(linearWeight == 1) { - double [][][] arrivalTimeMatrix = computeArrivalTimeFleet(PAll,tec); - for (int i = 0 ; i < numRobotAug; i++) { - for (int j = 0 ; j < numTaskAug; j++) { - for(int path = 0;path < maxNumPaths; path++) { - - BFunction[i][j][path] = pathLengthWeight*PAll[i][j][path]/sumMaxPathsLength+ tardinessWeight*tardinessMatrix[i][j][path]/sumTardiness + arrivalTimeWeight*arrivalTimeMatrix[i][j][path]/sumArrivalTime; - costValuesMatrix[i][j][path] = BFunction[i][j][path]; - //costValuesMatrix[i][j][path] = PAll[i][j][path]/sumMaxPathsLength+ tardinessMatrix[i][j][path]/sumTardiness + arrivalTimeMatrix[i][j][path]/sumArrivalTime; - } - - } - } - } - else { - for (int i = 0 ; i < numRobotAug; i++) { - for (int j = 0 ; j < numTaskAug; j++) { - for(int path = 0;path < maxNumPaths; path++) { - BFunction[i][j][path] = pathLengthWeight*PAll[i][j][path]/sumMaxPathsLength+ tardinessWeight*tardinessMatrix[i][j][path]/sumTardiness; - //costValuesMatrix[i][j][path] = PAll[i][j][path]/sumMaxPathsLength+ tardinessMatrix[i][j][path]/sumTardiness; - costValuesMatrix[i][j][path] = BFunction[i][j][path] ; - - - } - - - - } - } - } - - return BFunction; - } - - /** - * Evaluates the number of all feasible solutions for an optimization problem, with is defined with {@link #buildOptimizationProblem} - * @param numRobot -> Number of Robot of the optimization problem - * @param numTasks -> Number of Tasks of the optimization problem - * @return The number of all feasible solutions for the Optimization Problem - */ - - public int numberFeasibleSolution(int numRobot,int numTasks){ - //Create an optimization problem - MPSolver optimizationProblemCopy = buildOptimizationProblem(numRobot,numTasks); - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblemCopy.solve(); - int numberFeasibleSolution = 0; - while(resultStatus != MPSolver.ResultStatus.INFEASIBLE ) { - //Solve the optimization Problem - resultStatus = optimizationProblemCopy.solve(); - //If The solution is feasible increment the number of feasible solution - if (resultStatus != MPSolver.ResultStatus.INFEASIBLE) { - numberFeasibleSolution = numberFeasibleSolution+1; - } - double [][][] assignmentMatrix = saveAssignmentMatrix(numRobot,numTasks,optimizationProblemCopy); - //Add the constraint to actual solution -> in order to consider this solution as already found - optimizationProblemCopy = constraintOnPreviousSolution(optimizationProblemCopy,assignmentMatrix); - } - //Return the Total number of feasible solution - optimizationProblemCopy.clear(); - return numberFeasibleSolution; - } - - - /** - * Evaluates all possible feasible solutions for an optimization problem,with is defined with {@link #buildOptimizationProblem}. A feasible solution is a solution that verify constraints - * @param numRobot -> Number of Robots - * @param numTasks -> Number of Tasks - * @return A set containing all feasible solutions - */ - public double [][][][] evaluateFeasibleSolution(int numRobot,int numTasks){ - //Define the optimization problem - MPSolver optimizationProblemCopy = buildOptimizationProblem(numRobot,numTasks); - //Evaluate the number of all feasible solution for the optimization problem - int feasibleSolutions = numberFeasibleSolution(numRobot,numTasks); - //Initialize a set to store all feasible solution - double [][][][] AssignmentMatrixOptimalSolutions = new double [feasibleSolutions][numRobot][numTasks][maxNumPaths]; - /////////////////////////////////////// - for(int k=0; k < feasibleSolutions; k++) { - //Solve the optimization problem - MPSolver.ResultStatus resultStatus = optimizationProblemCopy.solve(); - //Transform the Assignment Vector to Matrix - double [][][] AssignmentMatrix = saveAssignmentMatrix(numRobot,numTasks,optimizationProblemCopy); - //Store the optimal solution - AssignmentMatrixOptimalSolutions[k]=AssignmentMatrix; - //Add the constraint to actual solution in order to consider this solution as already found - optimizationProblemCopy = constraintOnPreviousSolution(optimizationProblemCopy,AssignmentMatrix); - } - //Return the set of all Feasible solutions - return AssignmentMatrixOptimalSolutions; - } - - - - /** - * Builds the optimization problem. Define a decision variable X_ij as a binary variable in which i indicate - * the robot id, j the tasks. Also constraints are defined: - * the constraints considered are : - * 1) Each Task can be assign only to a robot; - * 2) Each Robot can perform only a task at time; - * @param numRobot -> Number of Robots - * @param numTasksAug -> Number of Tasks. - * @return A constrained optimization problem without the objective function - */ - private MPSolver buildOptimizationProblem(int numRobotAug,int numTasksAug) { - //Initialize a linear solver - - MPSolver optimizationProblem = new MPSolver( - "TaskAssignment", MPSolver.OptimizationProblemType.CBC_MIXED_INTEGER_PROGRAMMING); - //START DECISION VARIABLE VARIABLE - MPVariable [][][] decisionVariable = new MPVariable[numRobotAug][numTasksAug][maxNumPaths]; - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTasksAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - decisionVariable[i][j][s] = optimizationProblem.makeBoolVar("x"+"["+i+","+j+","+s+"]"); - } - - } - } - //END DECISION VARIABLE - ////////////////////////// - // START CONSTRAINTS - //Each Robot can be assign only to a Task - for (int i = 0; i < numRobotAug; i++) { - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY, 1); - for (int j = 0; j < numTasksAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j][s], 1); - } - - } - } - //Each task can be performed only by a robot - for (int j = 0; j < numTasksAug; j++) { - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(1, 1); - for (int i = 0; i < numRobotAug; i++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j][s], 1); - } - } - } - - - - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - if (i < numRobot) { //Considering only real Robot - PoseSteering[] pss = pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s); - if(pss==null) { - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j][s],1); - } - } - } - } - } - - //END CONSTRAINTS - //In case of having more task than robots, the task with a closest deadline are set with a higher priority - if(taskQueue.size() > IDsIdleRobots.size()) { - checkOnTaskDeadline(); - //Each task can be performed only by a robot - for (int j = 0; j < taskQueue.size(); j++) { - //Initialize the constraint - if(taskQueue.get(j).isPriority()) { - MPConstraint c3 = optimizationProblem.makeConstraint(1, 1); - for (int i = 0; i < IDsIdleRobots.size(); i++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c3.setCoefficient(decisionVariable[i][j][s], 1); - } - } - } - } - } - ///////////////////////////////////////////////// - return optimizationProblem; - } - - - public static double probability(double f1, double f2, int iteration) { - if (f2 < f1) return 1; - return 1-Math.exp((f1 - f2) / iteration); - } - - /** - * Solve the optimization problem given as input considering both B and F Functions with a Local Search Algorithm. The objective function is defined as sum(c_ij * x_ij) for (i = 1...n)(j = 1...m). - * with n = number of robot and m = number of tasks. Starting from a feasible solution, all his neighbors are computed; then the new solution - * is the optimal solutions considering the neighbors. The algorithm go on until the specified number of iterations - * @param tec -> an Abstract Trajectory Envelope Coordinator - * @param iteration -> number of iteration to consider ( if -1 all feasible solution are considered) - * @return An Optimal Assignment that minimize the objective function - */ - - public double [][][] simulatedAnnealingAlgorithm(AbstractTrajectoryEnvelopeCoordinator tec,int iteration){ - long initialTime = Calendar.getInstance().getTimeInMillis(); - PrintStream fileStream = null; - PrintStream fileStream1 = null; - PrintStream fileStream3 = null; - PrintStream fileStream4 = null; - PrintStream fileStream5 = null; - PrintStream fileStream7 = null; - PrintStream fileStream8 = null; - PrintStream fileStream9 = null; - Random rand = new Random(3455343); - - - - int a = tec.getTestNumber(); - int b = tec.getFolderNumber(); - String ppRequiredTime = "Test"+b+"/SA/RequiredTime-T" + a +".txt"; - String ppCriticalSections = "Test"+b+"/SA/CriticalSections-T" + a +".txt"; - String ppPathDelay = "Test"+b+"/SA/PathDelay-T" + a +".txt"; - String ppCostOptimalSolution = "Test"+b+"/SA/CostOptimalSolution-T" + a +".txt"; - String ppAssignMatrix = "Test"+b+"/SA/AssignMatrix-T" + a +".txt"; - String ppTotalTime = "Test"+b+"/SA/TotalTime-T" + a +".txt"; - String ppAnalysisProblem = "Test"+b+"/SA/AnalysisProblem-T" + a +".txt"; - String ppAssignMatrix2 = "Test"+b+"/SA/AssignMatrix2-T" + a +".txt"; - String ppCostObjectiveFunction = "Test"+b+"/SA/CostObjectiveFunction-T" + a +".txt"; - - try { - /* - fileStream = new PrintStream(new FileOutputStream("Test1/SA/RequiredTime.txt",true)); - fileStream1 = new PrintStream(new FileOutputStream("Test1/SA/CriticalSections.txt",true)); - PrintStream fileStream2 = new PrintStream(new FileOutputStream("Test1/SA/PathDelay.txt",true)); - fileStream3 = new PrintStream(new FileOutputStream("Test1/SA/CostOptimalSolution.txt",true)); - fileStream4 = new PrintStream(new FileOutputStream("Test1/SA/AssignMatrix.txt",true)); - fileStream5 = new PrintStream(new FileOutputStream("Test1/SA/TotalTime.txt",true)); - fileStream8 = new PrintStream(new FileOutputStream("Test1/SA/AssignMatrix2.txt",true)); - fileStream7 = new PrintStream(new FileOutputStream("Test1/SA/AnalysisProblem.txt",true)); - fileStream9 = new PrintStream(new FileOutputStream("Test1/SA/CostObjectiveFunction.txt",true)); - */ - - - fileStream = new PrintStream((new FileOutputStream(ppRequiredTime,true))); - fileStream1 = new PrintStream(new File(ppCriticalSections)); - PrintStream fileStream2 = new PrintStream(new File(ppPathDelay)); - fileStream3 = new PrintStream(new File(ppCostOptimalSolution)); - fileStream4 = new PrintStream(new FileOutputStream(ppAssignMatrix,true)); - fileStream5 = new PrintStream(new FileOutputStream(ppTotalTime,true)); - - //fileStream5 = new PrintStream(new File("TotalTime.txt")); - fileStream7 = new PrintStream(new FileOutputStream(ppAnalysisProblem,true)); - fileStream8 = new PrintStream(new FileOutputStream(ppAssignMatrix2,true)); - fileStream9 = new PrintStream(new FileOutputStream(ppCostObjectiveFunction,true)); - - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - numTask = taskQueue.size(); - //Get free robots - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - //Evaluate dummy robot and dummy task - dummyRobotorTask(numRobot,numTask,tec); - getAllRobotIDs(); - getAllTaskIDs(); - - //Consider possibility to have dummy Robot or Tasks - double [][][] PAll = evaluatePAll(tec); - - double[][][] BFunction = evaluateBFunction(PAll,tec); - //Build the optimization Problem without the objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobotAug,numTaskAug); - MPObjective objective = optimizationProblem.objective(); - objective.setMinimization(); - //Initialize the optimal assignment and the cost associated to it - double [][][] optimalAssignmentMatrix = new double[numRobotAug][numTaskAug][maxNumPaths]; - double objectiveOptimalValue = 100000000; - double costBOptimal = 0; - double costFOptimal = 0; - //Solve the optimization problem - optimizationProblem.solve(); - double [][][] AssignmentMatrix = saveAssignmentMatrix(numRobotAug,numTaskAug,optimizationProblem); - int prova1 = 0; - int prova2 = 0; - - int numIteration =1; - for(int fat= 1 ; fat <= numRobotAug; fat++) { - numIteration *= fat; - } - if(iteration > numIteration || iteration == -1) { - iteration = numIteration; - }else { - numIteration = iteration; - } - int index1i = 0; - int index1j = 0; - int index1s = 0; - int index2i = 0; - int index2j = 0; - int index2s = 0; - double [][][] newAssignmentMatrix = new double[numRobotAug][numTaskAug][maxNumPaths]; - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j IDsRandomRobots = new ArrayList (); - ArrayList IDsRandomRobots2 = new ArrayList (); - IDsRandomRobots.addAll(IDsAllRobots); - //IDsRandomRobots2.addAll(IDsAllRobots); - //IDsRandomRobots2.addAll(IDsAllRobots); - int ind = (int) Math.floor(rand.nextDouble()*IDsRandomRobots.size()); - prova1 = IDsRandomRobots.get(ind); - - for(int m: IDsAllRobots) { - if(IDsIdleRobots.contains(m) && tec.getRobot(prova1)!=null){ - tec.getRobot(m).getRobotType(); - tec.getRobot(prova1).getRobotType(); - if(tec.getRobot(m).getRobotType() == tec.getRobot(prova1).getRobotType()) { - IDsRandomRobots2.add(m); - - } - }else {//virtual robots - IDsRandomRobots2.add(m); - } - } - int index = IDsRandomRobots2.indexOf(prova1); - IDsRandomRobots2.remove(index); - //IDsRandomRobots.remove(ind); - if(ScenarioAllocation!= null) { - return this.ScenarioAllocation; - } - fileStream7.println("Tasks>> "+ IDsRealTasks.size() + " Robot>> " + IDsIdleRobots.size() ); - - long timeOffsetInitial = Calendar.getInstance().getTimeInMillis(); - long timeOffset = 0; - - for(int k=0; k <= iteration ;k++){ - if(timeOffset < timeOut) { - //Evaluate an optimal assignment that minimize only the B function - //Initialize cost of objective value - - double costofAssignment = 0; - double costofAssignmentForConstraint = 0; - double costBFunction = 0; - double costFFunction = 0; - if(k>0) { - if(IDsRandomRobots2.size() == 0) { - //prova1 += 1; - if(IDsRandomRobots.size()==0) { - IDsRandomRobots.addAll(IDsAllRobots); - } - ind = (int) Math.floor(rand.nextDouble()*IDsRandomRobots.size()); - prova1 = IDsRandomRobots.get(ind); - IDsRandomRobots.remove(ind); - for(int m: IDsAllRobots) { - if(IDsIdleRobots.contains(m) && tec.getRobot(prova1)!=null){ - tec.getRobot(m).getRobotType(); - tec.getRobot(prova1).getRobotType(); - if(tec.getRobot(m).getRobotType() == tec.getRobot(prova1).getRobotType()) { - IDsRandomRobots2.add(m); - - } - }else {//virtual robots - IDsRandomRobots2.add(m); - } - } - index = IDsRandomRobots2.indexOf(prova1); - IDsRandomRobots2.remove(index); - - } - //prova2 +=1; - int ind2 = (int) Math.floor(rand.nextDouble()*IDsRandomRobots2.size()); - if(IDsRandomRobots2.size()>0) { - prova2 = IDsRandomRobots2.get(ind2); - IDsRandomRobots2.remove(ind2); - } - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j 0) { - if (linearWeight != 1) { - //Evaluate cost of F function only if alpha is not equal to 1 - costBFunction = BFunction[i][j][s]; - costFFunction = evaluatePathDelay(robotID,taskID,s,newAssignmentMatrix,tec)/sumArrivalTime; - costofAssignment = linearWeight*costBFunction + (1-linearWeight)*costFFunction + costofAssignment ; - costofAssignmentForConstraint = costValuesMatrix[i][j][s] + costFFunction + costofAssignmentForConstraint; - - } - else { - costBFunction= BFunction[i][j][s]; - costofAssignment = Math.pow(linearWeight*costBFunction, 2) + costofAssignment ; - costofAssignmentForConstraint = costValuesMatrix[i][j][s] + costofAssignmentForConstraint; - } - - } - matrixSolutionFound[k][i][j][s] = newAssignmentMatrix[i][j][s]; - } - - } - } - costSolutions[k] = costofAssignment; - }else { - fileStream4.println("-------------------"); - fileStream4.println("Solution already found" + solutionAlreadyFound); - fileStream4.println("Switch was" + prova1 + "--" + prova2 ); - fileStream4.println("-------------------"); - costofAssignment = costSolutions[indexSolutionFound]; - costSolutions[k] = costofAssignment; - } - - fileStream4.println("-------------------"); - fileStream4.println(costofAssignment+""); - fileStream4.println("-------------------"); - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j 0) { - - double kk= rand.nextDouble(); - double kk2 = probability(costSolutions[k-1],costofAssignment,k); - - if(kk < kk2) { - - for(int i=0; i< AssignmentMatrix.length;i ++) { - for(int j = 0 ; j An Assignment Matrix of the optimization problem - * @param tec -> An Abstract Trajectory Envelope Coordinator - * @return An updated Trajectory Envelope Coordinator Simulation in which the mission for each - * robot is defined - */ - public boolean TaskAllocation(double [][][] AssignmentMatrix,AbstractTrajectoryEnvelopeCoordinator tec){ - System.out.println("Number of Robot : " + numRobot); - System.out.println("Number of Task : " + numTask); - System.out.println("Number of dummy Robot : " + dummyRobot); - System.out.println("Number of dummy Task : " + dummyTask); - System.out.println("Total Number of Robot : " + numRobotAug); - System.out.println("Total Number of Task : " + numTaskAug); - for (int robotID : IDsAllRobots ) { - int i = IDsAllRobots.indexOf(robotID); - for (int taskID : IDsAllTasks ) { - int j = IDsAllTasks.indexOf(taskID); - for(int s = 0; s < maxNumPaths; s++) { - if (AssignmentMatrix[i][j][s] > 0) { - if (i < numRobot) { //Considering only real Robot - PoseSteering[] pss = pathsToTargetGoal.get(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s); - //For Dispatch mission - if (j < numTask && pss != null) { - taskQueue.get(j).assignRobot(robotID); - taskQueue.get(j).setPaths(pss); - Mission[] robotMissions = taskQueue.get(j).getMissions(); - viz.displayTask(taskQueue.get(j).getStartPose(), taskQueue.get(j).getGoalPose(),taskID, "red"); - //tec.addMissions(new Mission(IDsIdleRobots[i],pss)); - System.out.println("Task # "+ taskID + " is Assigned"); - tec.setTaskAssigned(robotID,taskID); - tec.addMissions(robotMissions); - }else { - System.out.println("Virtual Task # "+ taskID + " is Assigned to a real robot"); - } - }else{ - - - - System.out.println("Task # "+ taskID + " is not Assigned to a real robot"); - - } - } - //Remove path from the path set - pathsToTargetGoal.remove(robotID*numTaskAug*maxNumPaths+taskID*maxNumPaths+s); - } - - } - } - //Remove Assigned Tasks from the set - int i = 0; - int cont = 0; - while (i < Math.min(numRobot, numTask)) { - if (taskQueue.size() == 0 || taskQueue.size() <= i) { - break; - } - if (taskQueue.get(i).isTaskAssigned()){ - taskQueue.remove(i); - System.out.println("Task # "+ (cont+1) + " is removed "); - }else { - i = i+1; - } - cont +=1; - - } - System.out.println("Remaining task: "+ taskQueue.size()); - IDsAllRobots.removeAll(IDsAllRobots); - IDsAllTasks.removeAll(IDsAllTasks); - IDsRealTasks.removeAll(IDsRealTasks); - IDsIdleRobots.removeAll(IDsIdleRobots); - - - return true; - }//End Task Assignment Function - - - Callback cb = new Callback() { - private long lastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - public void performOperation() { - long timeNow = Calendar.getInstance().getTimeInMillis(); - if (timeNow-lastUpdate > 1000) { - lastUpdate = timeNow; - } - } - }; - - /** - * Start the Task Allocation Algorithm - * @param rsp -> The motion planner that will be called for planning for any - * robot. - * @param alpha -> the weight of B and F function in objective function. It is considered as - * B*alpha + (1-alpha)*F - * @param tec -> An Abstract Trajectory Envelope Coordinator - */ - public void startTaskAssignment(AbstractTrajectoryEnvelopeCoordinator tec) { - //Create meta solver and solver - coordinator = tec; - numRobot = coordinator.getIdleRobots().size(); - //Start a thread that checks and enforces dependencies at every clock tick - this.setupInferenceCallback(); - - } - - - protected void setupInferenceCallback() { - - Thread TaskAssignmentThread = new Thread("Task Assignment") { - private long threadLastUpdate = Calendar.getInstance().getTimeInMillis(); - @Override - - public void run() { - while (true) { - System.out.println("Thread Running"); - if (!taskQueue.isEmpty() && coordinator.getIdleRobots().size() != 0 ) { - double [][][] assignmentMatrix = simulatedAnnealingAlgorithm(coordinator,-1); - for (int i = 0; i < assignmentMatrix.length; i++) { - int robotID = IDsAllRobots.get((i)); - for (int j = 0; j < assignmentMatrix[0].length; j++) { - int taskID = IDsAllTasks.get((j)); - for(int s = 0; s < maxNumPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if (assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " + robotID +" is assigned to Task "+ taskID +" throw Path " + (s+1)); - } - } - - } - } - TaskAllocation(assignmentMatrix,coordinator); - System.out.print("Task to be completed "+ taskQueue.size()); - } - - //Sleep a little... - if (CONTROL_PERIOD_Task > 0) { - try { - System.out.println("Thread Sleeping"); - Thread.sleep(CONTROL_PERIOD_Task); } //Thread.sleep(Math.max(0, CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate)); } - catch (InterruptedException e) { e.printStackTrace(); } - } - - long threadCurrentUpdate = Calendar.getInstance().getTimeInMillis(); - EFFECTIVE_CONTROL_PERIOD_task = (int)(threadCurrentUpdate-threadLastUpdate); - threadLastUpdate = threadCurrentUpdate; - - if (cb != null) cb.performOperation(); - - } - } - }; - TaskAssignmentThread.setPriority(Thread.MAX_PRIORITY); - TaskAssignmentThread.start(); - } - } - diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentCorridors2.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentCorridors2.java deleted file mode 100644 index 2a585dc..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentCorridors2.java +++ /dev/null @@ -1,253 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner.PLANNING_ALGORITHM; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentCorridors2 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - - tec.setupSolver(0, 100000000); - tec.startInference(); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(13, 6.8, 9.41); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - //tec.setFakeCoordinator(true); - viz.setMap("maps/map-corridors-modified.yaml"); - String yamlFile = new String("maps/map-corridors-modified.yaml"); - - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - //Set the default motion planner - ReedsSheppCarPlanner rsp1 = new ReedsSheppCarPlanner(PLANNING_ALGORITHM.RRTstar); - rsp1.setRadius(0.1); - rsp1.setTurningRadius(4.0); - rsp1.setFootprint(tec.getDefaultFootprint()); - rsp1.setDistanceBetweenPathPoints(0.5); - rsp1.setMap(yamlFile); - rsp1.setPlanningTimeInSecs(10); - - Pose startPoseRobot1 = new Pose(8.0,11.0,0.0); - Pose startPoseRobot2 = new Pose(8.0,13.0,0.0); - //Pose startPoseRobot3 = new Pose(8.0,18.0,0.0); - Pose startPoseRobot3 = new Pose(68.0,18.0,Math.PI); - Pose startPoseRobot4 = new Pose(8.0,22.5,0.0); - - - //Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 2 ); - Robot robot3 = new Robot(3, 3 ); - Robot robot4 = new Robot(4, 4 ); - Robot robot5 = new Robot(5, 1 ); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - //tec.addRobot(robot4,startPoseRobot4); - //tec.addRobot(robot5,startPoseRobot5); - - - - /* - Pose startinter = new Pose(26.0,15.0,0.0); - Pose fin = new Pose(68.0,12.0,0.0); - if(robotID==1 && path==0) { - startinter = new Pose(26.0,15.0,0.0); - fin = new Pose(68.0,12.0,0.0); - }else if(robotID==1 && path==1) { - startinter = new Pose(30.0,24.0,0.0); - fin = new Pose(68.1,12.0,0.0); - }else if(robotID==2 && path==0) { - startinter = new Pose(26.0,15.0,0.0); - fin = new Pose(68.0,15.0,0.0); - }else if(robotID==2 && path==1) { - startinter = new Pose(30.0,24.0,0.0); - fin = new Pose(68.1,15.0,0.0); - }else if(robotID==3 && path==0) { - startinter = new Pose(50.0,15.0,0.0); - fin = new Pose(8.0,18.0,Math.PI); - }else if(robotID==3 && path==1) { - startinter = new Pose(50.0,24.0,0.0); - fin = new Pose(8.1,18.0,Math.PI); - } - - - */ - - - - - - - - - - //Pose startPoseGoal1 = new Pose(28.0,32.0,0.0); - Pose startPoseGoal1 = new Pose(17.0,15.0,0.0); - Pose startPoseGoal2 = new Pose(17.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(62.0,18.0,0.0); - - - - - - Pose goalPoseRobot1 = new Pose(68.0,12.0,0.0); - Pose goalPoseRobot2 = new Pose(68.0,15.0,0.0); - //Pose goalPoseRobot3 = new Pose(68.0,18.0,0.0); - Pose goalPoseRobot3 = new Pose(8.0,18.0,Math.PI); - - - - //Pose goalPoseRobot8 = new Pose(68.0,20.0,0.0); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,2); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,3); - - - - - - //Task task8 = new Task(8,startPoseGoal8,goalPoseRobot8,4); - - for (int robotID : tec.getIdleRobots()) { - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = (ReedsSheppCarPlanner) rsp1.getCopy(false); - tec.setMotionPlanner(robotID, rsp); - } - - /////////////////////////////////////////////////////// - - int a = 5; - //FOLDER NUMBER - int b = 3; - tec.setTestNumber(a); - tec.setFolderNumber(b); - - //Solve the problem to find some feasible solution - double alpha = 1.0; - TaskAssignment assignmentProblem = new TaskAssignment(); - - assignmentProblem.LoadScenario("ProvaScenario1"); - - double [][][]optimalAllocation = new double[3][3][3]; - - - optimalAllocation[0][0][0]= 1; - optimalAllocation[1][1][0]= 1; - optimalAllocation[2][2][1]= 1; - - - - assignmentProblem.LoadScenarioAllocation(optimalAllocation); - - - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - //assignmentProblem.addTask(task8); - - assignmentProblem.setFleetVisualization(viz); - - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.setLinearWeight(alpha); - //assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setmaxNumPaths(2); - - assignmentProblem.startTaskAssignment(tec); - //assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - } -} - diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap.java deleted file mode 100644 index cb83387..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap.java +++ /dev/null @@ -1,228 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; - -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; - -import com.google.ortools.linearsolver.MPSolver; -import com.vividsolutions.jts.geom.Coordinate; - - - -import se.oru.coordination.coordination_oru.CriticalSection; - -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; - -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; - -import se.oru.coordination.coordination_oru.util.BrowserVisualization; - -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimulatedAnnealing; - - - - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMap { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - - - - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - - - - - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4, startPoseRobot4); - tec.addRobot(robot5, startPoseRobot5); - - String yamlFile = "maps/map-empty.yaml"; - - - - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(4.0,9.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,3.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(21.0,3.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(4.0,30.0,Math.PI/2); - - - - - - - - - //Pose startPoseGoal4 = new Pose(27.0,20.0,Math.PI/2); - //Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - - - //Pose goalPoseRobot4 = new Pose(48.0,27.0,0.0); - //Pose goalPoseRobot5 = new Pose(52.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseGoal5,1); - - //Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - //TaskAssignmentSimulatedAnnealing assignmentProblem = new TaskAssignmentSimulatedAnnealing(); - TaskAssignment assignmentProblem = new TaskAssignment(); - int numPaths = 1; - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - //assignmentProblem.addTask(task6); - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - //rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/CentroPiaggio.yaml")); - //double res = Double.parseDouble(Missions.getProperty("resolution", "maps/CentroPiaggio.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - /////////////////////////////////////////////////////// - - double [][][]optimalAllocation = {{{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0}}}; - - //Solve the problem to find some feasible solution - double alpha = 1.0; - - //tec.setFakeCoordinator(true); - tec.setAvoidDeadlocksGlobally(true); - //assignmentProblem.LoadScenario("ProvaScenario"); - //assignmentProblem.LoadScenarioAllocation(optimalAllocation); - - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(1.0, 0.0, 0.0); - MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblem(solver,tec); - //double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemLocalSearch(tec,-1); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap2.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap2.java deleted file mode 100644 index 1066a9d..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap2.java +++ /dev/null @@ -1,198 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMap2 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", yamlFile)); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - //tec.addRobot(robot4, startPoseRobot4); - //tec.addRobot(robot5, startPoseRobot5); - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(7.0,25.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,3.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(21.0,3.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(12.0,45.0,Math.PI/2); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseGoal5,1); - - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.7; - double numPaths = 1; - TaskAssignmentSimple assignmentProblem = new TaskAssignmentSimple(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setDefaultMotionPlanner(rsp); - assignmentProblem.setFleetVisualization(viz); - tec.setDefaultMotionPlanner(assignmentProblem.getDefaultMotionPlanner()); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(1.0, 0.0, 0.0); - - MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - double [][] assignmentMatrix = assignmentProblem.solveOptimizationProblem(solver,tec,alpha); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+"]"+" is "+ assignmentMatrix[i][j]); - if(assignmentMatrix[i][j] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)); - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap3.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap3.java deleted file mode 100644 index e5c853f..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap3.java +++ /dev/null @@ -1,196 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMap3 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", yamlFile)); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - //tec.addRobot(robot4, startPoseRobot4); - //tec.addRobot(robot5, startPoseRobot5); - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(7.0,25.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,3.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(21.0,3.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(12.0,45.0,Math.PI/2); - - - Task task1 = new Task(1,1000,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,1001,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,1002,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,500,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,300,startPoseGoal5,goalPoseGoal5,1); - - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.7; - double numPaths = 1; - TaskAssignmentSimple assignmentProblem = new TaskAssignmentSimple(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setDefaultMotionPlanner(rsp); - assignmentProblem.setFleetVisualization(viz); - tec.setDefaultMotionPlanner(assignmentProblem.getDefaultMotionPlanner()); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(1.0, 0.0, 0.0); - MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - double [][] assignmentMatrix = assignmentProblem.solveOptimizationProblem(solver,tec,alpha); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+"]"+" is "+ assignmentMatrix[i][j]); - if(assignmentMatrix[i][j] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)); - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap4.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap4.java deleted file mode 100644 index 4bfabb0..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMap4.java +++ /dev/null @@ -1,216 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMap4 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - /* - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", yamlFile)); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - */ - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - //tec.addRobot(robot4, startPoseRobot4); - //tec.addRobot(robot5, startPoseRobot5); - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(7.0,25.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,3.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(21.0,3.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(12.0,45.0,Math.PI/2); - - - Task task1 = new Task(1,1000,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,1001,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,1002,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,500,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,300,startPoseGoal5,goalPoseGoal5,1); - - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.4; - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setmaxNumPaths(numPaths); - - MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblem(solver,tec); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapExact.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapExact.java deleted file mode 100644 index 5219502..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapExact.java +++ /dev/null @@ -1,209 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMapExact { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - - - - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - //tec.addRobot(robot4, startPoseRobot4); - //tec.addRobot(robot5, startPoseRobot5); - - String yamlFile = "maps/map-empty.yaml"; - - - - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(7.0,25.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,3.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(21.0,3.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(12.0,45.0,Math.PI/2); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseGoal5,1); - - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - TaskAssignment assignmentProblem = new TaskAssignment(); - int numPaths = 1; - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.7; - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemExactAlgorithm(tec, alpha); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy.java deleted file mode 100644 index 29dd2a0..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy.java +++ /dev/null @@ -1,206 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMapGreedy { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - - - - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4, startPoseRobot4); - tec.addRobot(robot5, startPoseRobot5); - - String yamlFile = "maps/map-empty.yaml"; - - - - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(7.0,25.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,3.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(21.0,3.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(12.0,45.0,Math.PI/2); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseGoal5,1); - - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - TaskAssignment assignmentProblem = new TaskAssignment(); - int numPaths = 1; - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - tec.setFakeCoordinator(true); - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemGreedyAlgorithm(tec); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy2.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy2.java deleted file mode 100644 index 4882557..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy2.java +++ /dev/null @@ -1,174 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - - - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMapGreedy2 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", yamlFile)); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(-5.0,-5.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1, startPoseRobot1); - tec.addRobot(robot2, startPoseRobot2); - tec.addRobot(robot3, startPoseRobot3); - //tec.addRobot(robot4, startPoseRobot4); - //tec.addRobot(robot5, startPoseRobot5); - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose goalPoseRobot1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseRobot2 = new Pose(25.0,3.0,-Math.PI/4); - Pose goalPoseRobot3 = new Pose(21.0,3.0,-Math.PI/2); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(-5.0,-5.0,Math.PI/2); - Pose endPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose endPoseGoal5 = new Pose(-15.0,-8.0,Math.PI/2); - - - Task task4 = new Task(5,startPoseGoal4,endPoseGoal4,1); - Task task5 = new Task(6,startPoseGoal5,endPoseGoal5,1); - - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.6; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - //assignmentProblem.addTask(task4); - //assignmentProblem.addTask(task5); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - - MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy3.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy3.java deleted file mode 100644 index 75d3ae1..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapGreedy3.java +++ /dev/null @@ -1,209 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMapGreedy3 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - - - - - Pose startPoseRobot1 = new Pose(9.0,12.0,0.0); - Pose startPoseRobot2 = new Pose(11.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(10.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - //tec.addRobot(robot4, startPoseRobot4); - //tec.addRobot(robot5, startPoseRobot5); - - String yamlFile = "maps/map-empty.yaml"; - - - - - - Pose startPoseGoal1 = new Pose(16.0,4.0,0.0); - Pose startPoseGoal2 = new Pose(20.0,12.0,0.0); - Pose startPoseGoal3 = new Pose(25.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(4.0,9.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(22.0,9.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,10.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(30.0,7.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(4.0,30.0,Math.PI/2); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseGoal5,1); - - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - TaskAssignment assignmentProblem = new TaskAssignment(); - int numPaths = 1; - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - //assignmentProblem.addTask(task4); - //assignmentProblem.addTask(task5); - //assignmentProblem.addTask(task6); - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.2; - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(1.0, 0.0, 0.0); - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemGreedyAlgorithm(tec); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapOnline.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapOnline.java deleted file mode 100644 index 2faccd3..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapOnline.java +++ /dev/null @@ -1,211 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMapOnline { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //You probably also want to provide a non-trivial forward model - //(the default assumes that robots can always stop) - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - //tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - //tec.addRobot(robot4, startPoseRobot4); - //tec.addRobot(robot5, startPoseRobot5); - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(7.0,25.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,3.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(21.0,3.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(12.0,45.0,Math.PI/2); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseGoal5,1); - - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.7; - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - //assignmentProblem.addTask(task3); - //assignmentProblem.addTask(task4); - //assignmentProblem.addTask(task5); - assignmentProblem.setmaxNumPaths(numPaths); - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - - assignmentProblem.startTaskAssignment(tec); - - - - Pose startPoseGoal1new = new Pose(18.0,12.0,0.0); - Pose startPoseGoal2new = new Pose(30.0,9.0,0.0); - Pose startPoseGoal3new = new Pose(35.0,37.0,0.0); - - Pose newPoseGoal1 = new Pose(20.0,15.0,0.0); - Pose newPoseGoal2 = new Pose(32.0,10.0,0.0); - Pose newPoseGoal3 = new Pose(42.0,45.0,0.0); - Task task1new = new Task(7,startPoseGoal1new,newPoseGoal1,1); - Task task2new = new Task(8,startPoseGoal2new,newPoseGoal2,1); - Task task3new = new Task(9,startPoseGoal3new,newPoseGoal3,1); - - - Thread.sleep(5000); - - - //assignmentProblem.addTask(task1new); - //assignmentProblem.addTask(task2new); - //assignmentProblem.addTask(task3new); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapOnline2.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapOnline2.java deleted file mode 100644 index de1afd2..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapOnline2.java +++ /dev/null @@ -1,221 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMapOnline2 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //You probably also want to provide a non-trivial forward model - //(the default assumes that robots can always stop) - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", yamlFile)); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - - - ReedsSheppCarPlanner rsp2 = new ReedsSheppCarPlanner(); - rsp2.setRadius(0.2); - rsp2.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp2.setTurningRadius(4.0); - rsp2.setDistanceBetweenPathPoints(0.5); - rsp2.setMapFilename("maps"+File.separator+Missions.getProperty("image", yamlFile)); - double res2 = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res2); - - - Pose startPoseRobot1 = new Pose(4.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(6.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(9.0,6.0,Math.PI/2); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - //tec.addRobot(robot4, startPoseRobot4); - //tec.addRobot(robot5, startPoseRobot5); - - - Pose startPoseGoal1 = new Pose(16.0,25.0,0.0); - Pose startPoseGoal2 = new Pose(25.0,7.0,0.0); - Pose startPoseGoal3 = new Pose(4.0,8.0,0.0); - Pose startPoseGoal4 = new Pose(8.0,16.0,-Math.PI/2); - Pose startPoseGoal5 = new Pose(25.0,16.0,Math.PI/2); - - Pose startPoseGoal6 = new Pose(7.0,25.0,Math.PI/2); - - - Pose goalPoseGoal1 = new Pose(16.0,15.0,Math.PI/4); - Pose goalPoseGoal2 = new Pose(27.0,3.0,-Math.PI/4); - Pose goalPoseGoal3 = new Pose(21.0,3.0,-Math.PI/2); - Pose goalPoseGoal4 = new Pose(12.0,20.0,-Math.PI/2); - Pose goalPoseGoal5 = new Pose(32.0,25.0,Math.PI/2); - - Pose goalPoseGoal6 = new Pose(12.0,45.0,Math.PI/2); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseGoal3,1); - - Task task4 = new Task(4,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseGoal5,1); - - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.7; - double numPaths = 1; - TaskAssignmentSimple assignmentProblem = new TaskAssignmentSimple(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setDefaultMotionPlanner(rsp); - //assignmentProblem.setDefaultMotionPlanner2(rsp2); - assignmentProblem.setFleetVisualization(viz); - tec.setDefaultMotionPlanner(assignmentProblem.getDefaultMotionPlanner()); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - //assignmentProblem.setNumThreadToUse(2); - - assignmentProblem.startTaskAssignment(tec); - - - - Pose startPoseGoal1new = new Pose(18.0,12.0,0.0); - Pose startPoseGoal2new = new Pose(30.0,9.0,0.0); - Pose startPoseGoal3new = new Pose(35.0,37.0,0.0); - - Pose newPoseGoal1 = new Pose(20.0,15.0,0.0); - Pose newPoseGoal2 = new Pose(32.0,10.0,0.0); - Pose newPoseGoal3 = new Pose(42.0,45.0,0.0); - Task task1new = new Task(7,startPoseGoal1new,newPoseGoal1,1); - Task task2new = new Task(8,startPoseGoal2new,newPoseGoal2,1); - Task task3new = new Task(9,startPoseGoal3new,newPoseGoal3,1); - - - Thread.sleep(5000); - - - //assignmentProblem.addTask(task1new); - //assignmentProblem.addTask(task2new); - //assignmentProblem.addTask(task3new); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapRandomGoals.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapRandomGoals.java deleted file mode 100644 index 5dedd6f..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentMultiRobotsWithoutMapRandomGoals.java +++ /dev/null @@ -1,210 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintStream; -import java.util.Comparator; -import java.util.Random; -import org.metacsp.multi.spatioTemporal.paths.Pose; -import com.vividsolutions.jts.geom.Coordinate; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; -import com.google.ortools.linearsolver.*; - - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentMultiRobotsWithoutMapRandomGoals { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - - - - - Pose startPoseRobot1 = new Pose(9.0,12.0,0.0); - Pose startPoseRobot2 = new Pose(11.0,16.0,-Math.PI/4); - Pose startPoseRobot3 = new Pose(10.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(16.0,30.0,-Math.PI/2); - Pose startPoseRobot5 = new Pose(5.0,20.0,Math.PI/2); - Pose startPoseRobot6 = new Pose(5.0,8.0,0.0); - Pose startPoseRobot7 = new Pose(18.0,20.0,Math.PI/2); - Pose startPoseRobot8 = new Pose(7.0,32.0,-Math.PI/2); - - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - Robot robot6 = new Robot(6, 1); - Robot robot7 = new Robot(7, 1); - Robot robot8 = new Robot(8, 1); - - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4, startPoseRobot4); - tec.addRobot(robot5, startPoseRobot5); - //tec.addRobot(robot6, startPoseRobot6); - //tec.addRobot(robot7, startPoseRobot7); - - String yamlFile = "maps/map-empty.yaml"; - - - - - - PrintStream fileStream = null; - try { - fileStream = new PrintStream(new File("RandomPose.txt")); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - Random rand = new Random(); - //create Random Start and Goal Poses for Task - - ReedsSheppCarPlanner rsp2 = new ReedsSheppCarPlanner(); - rsp2.setRadius(0.2); - rsp2.setTurningRadius(4.0); - rsp2.setFootprint(tec.getDefaultFootprint()); - rsp2.setDistanceBetweenPathPoints(0.5); - rsp2.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res2 = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp2.setMapResolution(res2); - rsp2.setPlanningTimeInSecs(2); - - - TaskAssignment assignmentProblem = new TaskAssignment(); - - double rangeMinStart = 15.0; - double rangeMinStarty = 4.0; - double rangeMinGoal = 20.0; - double rangeMinGoaly = 6.0; - double rangeMax = 50.0; - double rangeMaxy = 40.0; - Pose RandomStartPose = new Pose(0.0,0.0,0); - Pose RandomGoalPose = new Pose(0.0,0.0,0); - for(int j=1;j <= 5; j++) { - boolean flag1 = false; - while(! flag1) { - double randomStartPosex = rangeMinStart + (rangeMax-rangeMinStart)*rand.nextDouble(); - double randomStartPosey = rangeMinStarty + (rangeMaxy-rangeMinStarty)*rand.nextDouble(); - RandomStartPose = new Pose(randomStartPosex,randomStartPosey,Math.PI/2); - flag1 = rsp2.isFree(RandomStartPose); - fileStream.println("Pose startPoseGoal"+j+" = new Pose" + RandomStartPose+";"); - } - boolean flag2 = false; - while(! flag2) { - double randomGoalPosex = rangeMinGoal + (rangeMax-rangeMinGoal)*rand.nextDouble(); - double randomGoalPosey = rangeMinGoaly + (rangeMaxy-rangeMinGoaly)*rand.nextDouble(); - RandomGoalPose = new Pose(randomGoalPosex,randomGoalPosey,Math.PI/2); - flag2 = rsp2.isFree(RandomGoalPose); - fileStream.println("Pose goalPoseGoal"+j+" = new Pose" + RandomGoalPose+";"); - } - Task taskRandom = new Task(j,RandomStartPose,RandomGoalPose,1); - assignmentProblem.addTask(taskRandom); - - - } - - - - int numPaths = 1; - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 1.0; - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(1.0, 0.0, 0.0); - MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblem(solver,tec); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - //Missions.saveScenario("RandomGoals"); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouse.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouse.java deleted file mode 100644 index 6dc92dc..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouse.java +++ /dev/null @@ -1,826 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Random; -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; - -import com.vividsolutions.jts.geom.Coordinate; - -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.NetworkConfiguration; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner.PLANNING_ALGORITHM; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; -import com.google.ortools.linearsolver.*; - - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentOrebroWarehouse { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - - //Maximum acceleration/deceleration and speed for all robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - - //Create a coordinator with interfaces to robots - //in the built-in 2D simulator - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Define a network with uncertainties (see Mannucci et al., 2019) - NetworkConfiguration.setDelays(0, 0); - NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS = 0.0; - - //Tell the coordinator - // (1) what is known about the communication channel, and - // (2) the accepted probability of constraint violation - tec.setNetworkParameters(NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS, NetworkConfiguration.getMaximumTxDelay(), 0.01); - - //Avoid deadlocks via global re-ordering - tec.setBreakDeadlocks(true, false, false); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - - //Set up infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - - //Start the thread that revises precedences at every period - tec.startInference(); - - //set the map yalm file - String yamlFile = new String("maps/basement.yaml"); - - //Start a visualization (will open a new browser tab) - BrowserVisualization viz = new BrowserVisualization(); - viz.setMap(yamlFile); - viz.setInitialTransform(13, 6.1, 6.8); - tec.setVisualization(viz); - tec.setBreakDeadlocks(true, false, false); - //Robot IDs can be non-sequential (but must be unique) - double xl = 1.0; - double yl = .5; - Coordinate footprint1 = new Coordinate(-xl,yl); - Coordinate footprint2 = new Coordinate(xl,yl); - Coordinate footprint3 = new Coordinate(xl,-yl); - Coordinate footprint4 = new Coordinate(-xl,-yl); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - - Coordinate[] fp2 = new Coordinate[] { - new Coordinate(-0.7,0.7), - new Coordinate(0.7,0.7), - new Coordinate(0.7,-0.7), - new Coordinate(-0.7,-0.7), - }; - - Robot robot1 = new Robot(1,1,tec.getDefaultFootprint()); - Robot robot2 = new Robot(2,1,tec.getDefaultFootprint()); - Robot robot3 = new Robot(3,1,tec.getDefaultFootprint()); - Robot robot4 = new Robot(4,1,tec.getDefaultFootprint()); - Robot robot5 = new Robot(5,1,tec.getDefaultFootprint()); - Robot robot6 = new Robot(6,1,tec.getDefaultFootprint()); - Robot robot7 = new Robot(7,1,tec.getDefaultFootprint()); - - //tec.setFootprint(2, fp2); - //tec.setFootprint(6, fp2); - //Define start and goal poses for each robot - Pose startPoseRobot1 = new Pose(33.0,5.0,Math.PI); - Pose startPoseRobot2 = new Pose(3.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(3.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(3.0,25.0,0.0); - Pose startPoseRobot5 = new Pose(8.0,2.8,Math.PI/2); - Pose startPoseRobot6 = new Pose(11.0,2.8,Math.PI/2); - Pose startPoseRobot7 = new Pose(20.0,2.8,Math.PI/2); - - - - - Pose startPoseGoal1 = new Pose(7.0,7.0,Math.PI/2); - Pose startPoseGoal2 = new Pose(13.0,20.0,0.0); - Pose startPoseGoal3 = new Pose(13.0,24.25,0.0); - Pose startPoseGoal4 = new Pose(25.0,5.0,0.0); - Pose startPoseGoal5 = new Pose(18.0,15.0,0.0); - Pose startPoseGoal6 = new Pose(11.0,5.0,Math.PI/2); - Pose startPoseGoal7 = new Pose(20.0,11.0,0.0); - - Pose goalPoseRobot1 = new Pose(6.0,15.5,Math.PI/2); - Pose goalPoseRobot2 = new Pose(3.0,23.0,0.0); - Pose goalPoseRobot3 = new Pose(20.0,24.25,0.0); - Pose goalPoseRobot4 = new Pose(30.5,7.0,Math.PI/2); - Pose goalPoseRobot5 = new Pose(25.0,15.0,0.0); - Pose goalPoseRobot6 = new Pose(3.0,26.0,0.0); - Pose goalPoseRobot7 = new Pose(24.0,11.0,0.0); - - - /////////////////////////////////////////////// - - //Set the default motion planner - ReedsSheppCarPlanner rsp1 = new ReedsSheppCarPlanner(PLANNING_ALGORITHM.RRTstar); - rsp1.setRadius(0.1); - rsp1.setTurningRadius(4.0); - rsp1.setFootprint(tec.getDefaultFootprint()); - rsp1.setDistanceBetweenPathPoints(0.5); - rsp1.setMap(yamlFile); - rsp1.setPlanningTimeInSecs(10); - - tec.addRobot(robot1, startPoseRobot1); - tec.addRobot(robot2, startPoseRobot2); - tec.addRobot(robot3, startPoseRobot3); - tec.addRobot(robot4, startPoseRobot4); - tec.addRobot(robot5, startPoseRobot5); - tec.addRobot(robot6, startPoseRobot6); - tec.addRobot(robot7, startPoseRobot7); - - //Primo set di tasks - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - Task task6 = new Task(6,startPoseGoal6,goalPoseRobot6,1); - Task task7 = new Task(7,startPoseGoal7,goalPoseRobot7,1); - - - - - - int a = 2; - //FOLDER NUMBER - int b = 2; - tec.setTestNumber(a); - tec.setFolderNumber(b); - - TaskAssignment assignmentProblem = new TaskAssignment(); - - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - assignmentProblem.setFleetVisualization(viz); - int numPaths = 1; - - for (int robotID : tec.getIdleRobots()) { - - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = (ReedsSheppCarPlanner) rsp1.getCopy(false); - tec.setMotionPlanner(robotID, rsp); - } - - - ///////////////////////// ////////////////////////////// - //Solve the problem to find some feasible solution - assignmentProblem.setTimeOutinMin(6.5); - double alpha = 0.5; - double [][][]optimalAllocation = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - - }; - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario1"); - //assignmentProblem.LoadScenarioAllocation(optimalAllocation); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - //tec.setFakeCoordinator(true); - //assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setSaveFutureAllocation(true); - - assignmentProblem.startTaskAssignment(tec); - //assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - //assignmentProblem.startTaskAssignmentLocalSearchAlgorithm(tec, -1); - - - //New tasks real - //SET 2 - //Second set of tasks - //-> 4 more tasks ( 3 of type 1 and 1 of type 2) - Pose startPoseGoal1Set2 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot1Set2 = new Pose(15.0,28.0,Math.PI/2); - Task task1Set2 = new Task(8,startPoseGoal1Set2,goalPoseRobot1Set2,1); - /////////////////// - Pose startPoseGoal2Set2 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot2Set2 = new Pose(33.5,4.0,0.0); - Task task2Set2 = new Task(9,startPoseGoal2Set2,goalPoseRobot2Set2,1); - /////////////////// - Pose startPoseGoal3Set2 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot3Set2 = new Pose(33.7,2.0,0.0); - Task task3Set2 = new Task(10,startPoseGoal3Set2,goalPoseRobot3Set2,1); - /////////////////// - Pose startPoseGoal4Set2 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot4Set2 = new Pose(2.8,6.5,Math.PI); - Task task4Set2 = new Task(11,startPoseGoal4Set2,goalPoseRobot4Set2,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - //Third set of tasks - //-> 5 more tasks (4 of type 1 and 1 of type 2) - Pose startPoseGoal1Set3 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot1Set3 = new Pose(43.5,30.6,0.0); - Task task1Set3 = new Task(12,startPoseGoal1Set3,goalPoseRobot1Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal2Set3 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot2Set3 = new Pose(2.4,3.0,Math.PI); - Task task2Set3 = new Task(13,startPoseGoal2Set3,goalPoseRobot2Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set3 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot3Set3 = new Pose(17.0,2.8,-Math.PI/2); - Task task3Set3 = new Task(14,startPoseGoal3Set3,goalPoseRobot3Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set3 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot4Set3 = new Pose(3.0,28.0,0.0); - Task task4Set3 = new Task(15,startPoseGoal4Set3,goalPoseRobot4Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set3 = new Pose(23.5,33.5,0.0); - Pose goalPoseRobot5Set3 = new Pose(43.5,33.6,0.0); - Task task5Set3 = new Task(16,startPoseGoal5Set3,goalPoseRobot5Set3,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - - - - - - - //Forth set of tasks - //-> 4 more tasks (4 of type 1 and 0 of type 2) - Pose startPoseGoal1Set4 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot1Set4 = new Pose(25.0,15.0,0.0); - Task task1Set4 = new Task(17,startPoseGoal1Set4,goalPoseRobot1Set4,1); - ////////////////////////////////////// - Pose startPoseGoal2Set4 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot2Set4 = new Pose(20.0,24.5,0.0); - Task task2Set4 = new Task(18,startPoseGoal2Set4,goalPoseRobot2Set4,1); - ////////////////////////////////////// - Pose startPoseGoal3Set4 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot3Set4 = new Pose(24.0,11.0,0.0); - Task task3Set4 = new Task(19,startPoseGoal3Set4,goalPoseRobot3Set4,1); - ////////////////////////////////////// - Pose startPoseGoal4Set4 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot4Set4 = new Pose(6.0,33.5,Math.PI/2); - Task task4Set4 = new Task(20,startPoseGoal4Set4,goalPoseRobot4Set4,1); - ////////////////////////////////////// - //Fifth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set5 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set5 = new Pose(6.0,10.0,Math.PI); - Task task1Set5 = new Task(21,startPoseGoal1Set5,goalPoseRobot1Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set5 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set5 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set5 = new Task(22,startPoseGoal2Set5,goalPoseRobot2Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set5 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set5 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set5 = new Task(23,startPoseGoal3Set5,goalPoseRobot3Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set5 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set5 = new Pose(23.0,19.0,0.0); - Task task4Set5 = new Task(24,startPoseGoal4Set5,goalPoseRobot4Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set5 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set5 = new Pose(33.5,7.5,0.0); - Task task5Set5 = new Task(25,startPoseGoal5Set5,goalPoseRobot5Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal6Set5 = new Pose(20.0,4.0,0.0); - Pose goalPoseRobot6Set5 = new Pose(25.5,3.0,Math.PI/2); - Task task6Set5 = new Task(26,startPoseGoal6Set5,goalPoseRobot6Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal7Set5 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot7Set5 = new Pose(3.0,33.5,Math.PI/2); - Task task7Set5 = new Task(27,startPoseGoal7Set5,goalPoseRobot7Set5,1); - //////////////////////////////////////////////////// - ////////////////////////////////////// - //Sixth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set6 = new Pose(18.0,33.0,0.0); - Pose goalPoseRobot1Set6 = new Pose(43.5,15.6,0.0); - //Pose goalPoseRobot1Set6 = new Pose(43.5,27.6,0.0) - Task task1Set6 = new Task(28,startPoseGoal1Set6,goalPoseRobot1Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set6 = new Pose(19.0,33.0,0.0); - Pose goalPoseRobot2Set6 = new Pose(43.5,24.6,0.0); - Task task2Set6 = new Task(29,startPoseGoal2Set6,goalPoseRobot2Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set6 = new Pose(43.5,21.6,0.0); - Task task3Set6 = new Task(30,startPoseGoal3Set6,goalPoseRobot3Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set6 = new Pose(22.0,33.0,0.0); - Pose goalPoseRobot4Set6 = new Pose(43.5,18.6,0.0); - Task task4Set6 = new Task(31,startPoseGoal4Set6,goalPoseRobot4Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set6 = new Pose(21.0,33.0,0.0); - //Pose goalPoseRobot5Set6 = new Pose(43.5,15.6,0.0); - Pose goalPoseRobot5Set6 = new Pose(43.5,9.6,0.0); - Task task5Set6 = new Task(32,startPoseGoal5Set6,goalPoseRobot5Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set6 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot6Set6 = new Pose(43.5,12.6,0.0); - Task task6Set6 = new Task(33,startPoseGoal6Set6,goalPoseRobot6Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set6 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot7Set6 = new Pose(3.0,26.0,0.0); - Task task7Set6 = new Task(34,startPoseGoal7Set6,goalPoseRobot7Set6,1); - ///////////////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////////////// - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set10 = new Pose(22.0,33.0,0.0); - Pose goalPoseRobot1Set10 = new Pose(43.5,27.6,0.0); - Task task1Set10 = new Task(35,startPoseGoal1Set10,goalPoseRobot1Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set10 = new Pose(24.0,33.0,0.0); - Pose goalPoseRobot2Set10 = new Pose(43.5,24.6,0.0); - Task task2Set10 = new Task(36,startPoseGoal2Set10,goalPoseRobot2Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set10 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot3Set10 = new Pose(43.5,21.6,0.0); - Task task3Set10 = new Task(37,startPoseGoal3Set10,goalPoseRobot3Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot4Set10 = new Pose(43.5,18.6,0.0); - Task task4Set10 = new Task(38,startPoseGoal4Set10,goalPoseRobot4Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set10 = new Pose(18.0,32.0,0.0); - Pose goalPoseRobot5Set10 = new Pose(43.5,15.6,0.0); - Task task5Set10 = new Task(39,startPoseGoal5Set10,goalPoseRobot5Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot6Set10 = new Pose(43.5,12.6,0.0); - Task task6Set10 = new Task(40,startPoseGoal6Set10,goalPoseRobot6Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set10 = new Pose(16.0,32.0,0.0); - Pose goalPoseRobot7Set10 = new Pose(43.5,9.6,0.0); - Task task7Set10 = new Task(41,startPoseGoal7Set10,goalPoseRobot7Set10,1); - - - - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set9 = new Pose(29.0,4.0,Math.PI); - Pose goalPoseRobot1Set9 = new Pose(33.0,4.0,Math.PI); - Task task1Set9 = new Task(42,startPoseGoal1Set9,goalPoseRobot1Set9,1); - //////////////////////////////////////////////////////////// - Pose startPoseGoal2Set9 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set9 = new Pose(3.0,28.0,0.0); - Task task2Set9 = new Task(43,startPoseGoal2Set9,goalPoseRobot2Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal3Set9 = new Pose(6.0,20.0,0.0); - Pose goalPoseRobot3Set9 = new Pose(3.0,20.0,0.0); - Task task3Set9 = new Task(44,startPoseGoal3Set9,goalPoseRobot3Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal4Set9 = new Pose(7.0,25.0,0.0); - Pose goalPoseRobot4Set9 = new Pose(3.0,25.0,0.0); - Task task4Set9 = new Task(45,startPoseGoal4Set9,goalPoseRobot4Set9,1); - ////////////////////////////////////////////////////////////////// - ////////////////////////GIVE AGAIN THE SAME TASKS TO HAVE 75 TOTAL TASKS/////////////////////////////////////////// - ////// - //SET 11 - Pose startPoseGoal1Set11 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot1Set11 = new Pose(3.0,26.0,0.0); - Task task1Set11 = new Task(46,startPoseGoal1Set11,goalPoseRobot1Set11,1); - /////////////////// - Pose startPoseGoal2Set11 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot2Set11 = new Pose(24.0,11.0,0.0); - Task task2Set11 = new Task(47,startPoseGoal2Set11,goalPoseRobot2Set11,1); - //////////////////////////////////////////// - - Pose startPoseGoal3Set11 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot3Set11 = new Pose(14.5,28.0,Math.PI/2); - Task task3Set11 = new Task(48,startPoseGoal3Set11,goalPoseRobot3Set11,1); - /////////////////// - Pose startPoseGoal4Set11 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot4Set11 = new Pose(33.5,4.0,0.0); - Task task4Set11 = new Task(49,startPoseGoal4Set11,goalPoseRobot4Set11,1); - /////////////////// - Pose startPoseGoal5Set11 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot5Set11 = new Pose(33.7,2.0,0.0); - Task task5Set11 = new Task(50,startPoseGoal5Set11,goalPoseRobot5Set11,1); - //////////////////////////////////////// - /////////////////////////////////////// - //SET 12 - /////////////////// - Pose startPoseGoal1Set12 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set12 = new Pose(2.8,6.5,Math.PI); - Task task1Set12 = new Task(51,startPoseGoal1Set12,goalPoseRobot1Set12,1); - ////////////////////////////////////////////// - Pose startPoseGoal2Set12 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot2Set12 = new Pose(43.5,30.6,0.0); - Task task2Set12 = new Task(52,startPoseGoal2Set12,goalPoseRobot2Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set12 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot3Set12 = new Pose(2.4,3.0,Math.PI); - Task task3Set12 = new Task(53,startPoseGoal3Set12,goalPoseRobot3Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set12 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot4Set12 = new Pose(17.0,2.8,-Math.PI/2); - Task task4Set12 = new Task(54,startPoseGoal4Set12,goalPoseRobot4Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set12 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot5Set12 = new Pose(3.0,28.0,0.0); - Task task5Set12 = new Task(55,startPoseGoal5Set12,goalPoseRobot5Set12,1); - //////////////////////////////////////////// - /////////////////////////////////////////// - //SET 13 - Pose startPoseGoal1Set13 = new Pose(23.5,33.5,0.0); - Pose goalPoseRobot1Set13 = new Pose(43.5,33.6,0.0); - Task task1Set13 = new Task(56,startPoseGoal1Set13,goalPoseRobot1Set13,1); - ////////////////////////////////////////////// - Pose startPoseGoal2Set13 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot2Set13 = new Pose(25.0,15.0,0.0); - Task task2Set13 = new Task(57,startPoseGoal2Set13,goalPoseRobot2Set13,1); - ////////////////////////////////////// - Pose startPoseGoal3Set13 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot3Set13 = new Pose(20.0,24.5,0.0); - Task task3Set13 = new Task(58,startPoseGoal3Set13,goalPoseRobot3Set13,1); - ////////////////////////////////////// - Pose startPoseGoal4Set13 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot4Set13 = new Pose(24.0,11.0,0.0); - Task task4Set13 = new Task(59,startPoseGoal4Set13,goalPoseRobot4Set13,1); - ////////////////////////////////////// - Pose startPoseGoal5Set13 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot5Set13 = new Pose(6.0,33.5,Math.PI/2); - Task task5Set13 = new Task(60,startPoseGoal5Set13,goalPoseRobot5Set13,1); - ////////////////////////////////////// - ///////////////////////////////////////////////////////// - //SET 14 - Pose startPoseGoal1Set14 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set14 = new Pose(6.0,10.0,Math.PI); - Task task1Set14 = new Task(61,startPoseGoal1Set14,goalPoseRobot1Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set14 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set14 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set14 = new Task(62,startPoseGoal2Set14,goalPoseRobot2Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set14 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set14 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set14 = new Task(63,startPoseGoal3Set14,goalPoseRobot3Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set14 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set14 = new Pose(23.0,19.0,0.0); - Task task4Set14 = new Task(64,startPoseGoal4Set14,goalPoseRobot4Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set14 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set14 = new Pose(33.5,7.5,0.0); - Task task5Set14 = new Task(65,startPoseGoal5Set14,goalPoseRobot5Set14,1); - //////////////////////////////////////////////////// - /////////////////////////////////////////////////// - //SET 15 - Pose startPoseGoal1Set15 = new Pose(20.0,4.0,0.0); - Pose goalPoseRobot1Set15 = new Pose(25.5,3.0,Math.PI/2); - Task task1Set15 = new Task(66,startPoseGoal1Set15,goalPoseRobot1Set15,1); - /////////////////////////////////////// - Pose startPoseGoal2Set15 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set15 = new Pose(3.0,33.5,Math.PI/2); - Task task2Set15 = new Task(67,startPoseGoal2Set15,goalPoseRobot2Set15,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set15 = new Pose(43.5,27.6,0.0); - Task task3Set15 = new Task(68,startPoseGoal3Set15,goalPoseRobot3Set15,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot4Set15 = new Pose(43.5,24.6,0.0); - Task task4Set15 = new Task(69,startPoseGoal4Set15,goalPoseRobot4Set15,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot5Set15 = new Pose(43.5,21.6,0.0); - Task task5Set15 = new Task(70,startPoseGoal5Set15,goalPoseRobot5Set15,1); - ///////////////////////////////////////////////////////////////////// - //////////////////////////// - //SET 16 - - Pose startPoseGoal1Set16 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot1Set16 = new Pose(43.5,18.6,0.0); - Task task1Set16 = new Task(71,startPoseGoal1Set16,goalPoseRobot1Set16,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set16 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot2Set16 = new Pose(43.5,15.6,0.0); - Task task2Set16 = new Task(72,startPoseGoal2Set16,goalPoseRobot2Set16,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set16 = new Pose(8.0,5.8,Math.PI/2); - Pose goalPoseRobot3Set16 = new Pose(8.0,2.8,Math.PI/2); - Task task3Set16 = new Task(73,startPoseGoal3Set16,goalPoseRobot3Set16,1); - //////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set16 = new Pose(11.0,5.8,Math.PI/2); - Pose goalPoseRobot4Set16 = new Pose(11.0,2.8,Math.PI/2); - Task task4Set16 = new Task(74,startPoseGoal4Set16,goalPoseRobot4Set16,1); - /////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set16 = new Pose(18.0,7.8,Math.PI/2); - Pose goalPoseRobot5Set16 = new Pose(20.0,2.5,Math.PI/2); - Task task5Set16 = new Task(75,startPoseGoal5Set16,goalPoseRobot5Set16,1); - - - - - - - - - - - - //TEST ZONE - Pose startPoseGoal1New = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1New = new Pose(2.0,5.0,Math.PI); - - Robot robot1New = new Robot(8, 1,tec.getDefaultFootprint() ); - Robot robot2New = new Robot(9,2,tec.getDefaultFootprint() ); - Robot robot3New = new Robot(10,1,tec.getDefaultFootprint() ); - Robot robot4New = new Robot(11,1,tec.getDefaultFootprint()); - Robot robot5New = new Robot(12,1,tec.getDefaultFootprint()); - Robot robot6New = new Robot(13,2,fp2); - - //tec.addRobot(robot1New,startPoseGoal1Set3); - //tec.addRobot(robot2New,goalPoseRobot1Set3); - //tec.addRobot(robot3New,startPoseGoal1Set7); - //tec.addRobot(robot4New, startPoseGoal7Set5); - //tec.addRobot(robot5New, startPoseGoal6Set5); - //tec.addRobot(robot6New, goalPoseRobot6Set5); - /////// - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(55000); //sys alpha != 1 synchronous - //Thread.sleep(450000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - //Add the second set of tasks - assignmentProblem.addTask(task6); - assignmentProblem.addTask(task1Set10); - assignmentProblem.addTask(task1Set2); - assignmentProblem.addTask(task2Set2); - assignmentProblem.addTask(task3Set2); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario2"); - double [][][]optimalAllocation2 = {{{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - - }; - //assignmentProblem.LoadScenarioAllocation(optimalAllocation2); - - - ///Sleep for a while - - //Thread.sleep(550000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set3); - assignmentProblem.addTask(task2Set3); - assignmentProblem.addTask(task3Set3); - assignmentProblem.addTask(task4Set3); - assignmentProblem.addTask(task5Set3); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario3"); - double [][][]optimalAllocation3 = {{{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - //assignmentProblem.LoadScenarioAllocation(optimalAllocation3); - - - //Thread.sleep(150000); //sys synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(200000); //sys alpha != 1 synchronous - //Thread.sleep(250000); - //Thread.sleep(500000); - //Thread.sleep(75000); - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task4Set2); - assignmentProblem.addTask(task1Set6); - assignmentProblem.addTask(task2Set6); - assignmentProblem.addTask(task3Set6); - assignmentProblem.addTask(task4Set6); - //assignmentProblem.LoadScenario(null); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario4"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(600000); //sys alpha _=! 1 - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task5Set6); - assignmentProblem.addTask(task6Set6); - assignmentProblem.addTask(task7Set6); - assignmentProblem.addTask(task7); - assignmentProblem.addTask(task1Set4); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario5"); - - //Thread.sleep(300000); - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(500000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task2Set4); - assignmentProblem.addTask(task3Set4); - assignmentProblem.addTask(task4Set4); - assignmentProblem.addTask(task1Set5); - assignmentProblem.addTask(task2Set5); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario6"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(500000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - assignmentProblem.addTask(task3Set5); - assignmentProblem.addTask(task4Set5); - assignmentProblem.addTask(task5Set5); - assignmentProblem.addTask(task6Set5); - assignmentProblem.addTask(task7Set5); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario7"); - - - //Thread.sleep(150000);//sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task2Set10); - assignmentProblem.addTask(task3Set10); - assignmentProblem.addTask(task4Set10); - assignmentProblem.addTask(task5Set10); - assignmentProblem.addTask(task6Set10); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario8"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task7Set10); - assignmentProblem.addTask(task1Set9); - assignmentProblem.addTask(task2Set9); - assignmentProblem.addTask(task3Set9); - assignmentProblem.addTask(task4Set9); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario9"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - assignmentProblem.addTask(task1Set11); - assignmentProblem.addTask(task2Set11); - assignmentProblem.addTask(task3Set11); - assignmentProblem.addTask(task4Set11); - assignmentProblem.addTask(task5Set11); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario10"); - - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set12); - assignmentProblem.addTask(task2Set12); - assignmentProblem.addTask(task3Set12); - assignmentProblem.addTask(task4Set12); - assignmentProblem.addTask(task5Set12); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario11"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set13); - assignmentProblem.addTask(task2Set13); - assignmentProblem.addTask(task3Set13); - assignmentProblem.addTask(task4Set13); - assignmentProblem.addTask(task5Set13); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario12"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task1Set14); - assignmentProblem.addTask(task2Set14); - assignmentProblem.addTask(task3Set14); - assignmentProblem.addTask(task4Set14); - assignmentProblem.addTask(task5Set14); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario13"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set15); - assignmentProblem.addTask(task2Set15); - assignmentProblem.addTask(task3Set15); - assignmentProblem.addTask(task4Set15); - assignmentProblem.addTask(task5Set15); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario14"); - - - //Thread.sleep(150000); //sys alpha = 1 synchronous - Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task1Set16); - assignmentProblem.addTask(task2Set16); - assignmentProblem.addTask(task3Set16); - assignmentProblem.addTask(task4Set16); - assignmentProblem.addTask(task5Set16); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario15"); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouse2.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouse2.java deleted file mode 100644 index 4c62c6b..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouse2.java +++ /dev/null @@ -1,486 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Random; -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; - -import com.vividsolutions.jts.geom.Coordinate; - -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.NetworkConfiguration; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner.PLANNING_ALGORITHM; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; -import com.google.ortools.linearsolver.*; - - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentOrebroWarehouse2 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - - //Maximum acceleration/deceleration and speed for all robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - - //Create a coordinator with interfaces to robots - //in the built-in 2D simulator - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Define a network with uncertainties (see Mannucci et al., 2019) - NetworkConfiguration.setDelays(0, 0); - NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS = 0.0; - - //Tell the coordinator - // (1) what is known about the communication channel, and - // (2) the accepted probability of constraint violation - tec.setNetworkParameters(NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS, NetworkConfiguration.getMaximumTxDelay(), 0.01); - - //Avoid deadlocks via global re-ordering - tec.setBreakDeadlocks(true, false, false); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - - //Set up infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - - //Start the thread that revises precedences at every period - tec.startInference(); - - //set the map yalm file - String yamlFile = new String("maps/basement.yaml"); - - //Start a visualization (will open a new browser tab) - BrowserVisualization viz = new BrowserVisualization(); - viz.setMap(yamlFile); - viz.setInitialTransform(15, 15.10, 11.36); - tec.setVisualization(viz); - tec.setBreakDeadlocks(true, false, false); - //Robot IDs can be non-sequential (but must be unique) - double xl = 1.0; - double yl = .5; - Coordinate footprint1 = new Coordinate(-xl,yl); - Coordinate footprint2 = new Coordinate(xl,yl); - Coordinate footprint3 = new Coordinate(xl,-yl); - Coordinate footprint4 = new Coordinate(-xl,-yl); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - - Coordinate[] fp2 = new Coordinate[] { - new Coordinate(-0.7,0.7), - new Coordinate(0.7,0.7), - new Coordinate(0.7,-0.7), - new Coordinate(-0.7,-0.7), - }; - - Robot robot1 = new Robot(1,1,tec.getDefaultFootprint() ); - Robot robot2 = new Robot(2,2,fp2); - Robot robot3 = new Robot(3,1,tec.getDefaultFootprint()); - Robot robot4 = new Robot(4,1,tec.getDefaultFootprint()); - Robot robot5 = new Robot(5,1,tec.getDefaultFootprint()); - Robot robot6 = new Robot(6,2,fp2); - Robot robot7 = new Robot(7,1,tec.getDefaultFootprint()); - - tec.setFootprint(2, fp2); - tec.setFootprint(6, fp2); - //Define start and goal poses for each robot - Pose startPoseRobot1 = new Pose(33.0,5.0,Math.PI); - Pose startPoseRobot2 = new Pose(3.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(3.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(3.0,25.0,0.0); - Pose startPoseRobot5 = new Pose(8.0,2.8,Math.PI/2); - Pose startPoseRobot6 = new Pose(11.0,2.8,Math.PI/2); - Pose startPoseRobot7 = new Pose(20.0,2.8,Math.PI/2); - - - - - Pose startPoseGoal1 = new Pose(7.0,7.0,Math.PI/2); - Pose startPoseGoal2 = new Pose(13.0,20.0,0.0); - Pose startPoseGoal3 = new Pose(13.0,24.25,0.0); - Pose startPoseGoal4 = new Pose(25.0,5.0,0.0); - Pose startPoseGoal5 = new Pose(18.0,15.0,0.0); - Pose startPoseGoal6 = new Pose(11.0,5.0,Math.PI/2); - Pose startPoseGoal7 = new Pose(20.0,11.0,0.0); - - Pose goalPoseRobot1 = new Pose(6.0,15.5,Math.PI/2); - Pose goalPoseRobot2 = new Pose(3.0,23.0,0.0); - Pose goalPoseRobot3 = new Pose(20.0,24.25,0.0); - Pose goalPoseRobot4 = new Pose(30.5,7.0,Math.PI/2); - Pose goalPoseRobot5 = new Pose(25.0,15.0,0.0); - Pose goalPoseRobot6 = new Pose(3.0,26.0,0.0); - Pose goalPoseRobot7 = new Pose(24.0,11.0,0.0); - - - /////////////////////////////////////////////// - - //Set the default motion planner - ReedsSheppCarPlanner rsp1 = new ReedsSheppCarPlanner(PLANNING_ALGORITHM.RRTstar); - rsp1.setRadius(0.1); - rsp1.setTurningRadius(4.0); - rsp1.setFootprint(tec.getDefaultFootprint()); - rsp1.setDistanceBetweenPathPoints(0.5); - rsp1.setMap(yamlFile); - rsp1.setPlanningTimeInSecs(10); - - tec.addRobot(robot1, startPoseRobot1); - tec.addRobot(robot2, startPoseRobot2); - tec.addRobot(robot3, startPoseRobot3); - tec.addRobot(robot4, startPoseRobot4); - tec.addRobot(robot5, startPoseRobot5); - tec.addRobot(robot6, startPoseRobot6); - tec.addRobot(robot7, startPoseRobot7); - - //Primo set di tasks - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,2); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - Task task6 = new Task(6,startPoseGoal6,goalPoseRobot6,2); - Task task7 = new Task(7,startPoseGoal7,goalPoseRobot7,1); - - - TaskAssignment assignmentProblem = new TaskAssignment(); - - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - assignmentProblem.addTask(task6); - assignmentProblem.addTask(task7); - assignmentProblem.setFleetVisualization(viz); - int numPaths = 1; - - for (int robotID : tec.getIdleRobots()) { - - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = (ReedsSheppCarPlanner) rsp1.getCopy(false); - tec.setMotionPlanner(robotID, rsp); - } - - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 1.0; - double [][][]optimalAllocation = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - - //assignmentProblem.LoadScenario("ProvaScenario"); - //assignmentProblem.LoadScenarioAllocation(optimalAllocation); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - //tec.setFakeCoordinator(true); - //assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - - - assignmentProblem.startTaskAssignment(tec); - //assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - //assignmentProblem.startTaskAssignmentLocalSearchAlgorithm(tec, -1); - - - //New tasks real - //Second set of tasks - //-> 4 more tasks ( 3 of type 1 and 1 of type 2) - Pose startPoseGoal1Set2 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot1Set2 = new Pose(14.5,28.0,Math.PI/2); - Task task1Set2 = new Task(8,startPoseGoal1Set2,goalPoseRobot1Set2,1); - /////////////////// - Pose startPoseGoal2Set2 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot2Set2 = new Pose(33.5,4.0,0.0); - Task task2Set2 = new Task(9,startPoseGoal2Set2,goalPoseRobot2Set2,1); - /////////////////// - Pose startPoseGoal3Set2 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot3Set2 = new Pose(33.7,2.0,0.0); - Task task3Set2 = new Task(10,startPoseGoal3Set2,goalPoseRobot3Set2,1); - /////////////////// - Pose startPoseGoal4Set2 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot4Set2 = new Pose(2.8,6.5,Math.PI); - Task task4Set2 = new Task(11,startPoseGoal4Set2,goalPoseRobot4Set2,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - //Third set of tasks - //-> 5 more tasks (4 of type 1 and 1 of type 2) - Pose startPoseGoal1Set3 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot1Set3 = new Pose(43.5,30.6,0.0); - Task task1Set3 = new Task(12,startPoseGoal1Set3,goalPoseRobot1Set3,2); - //////////////////////////////////////////// - Pose startPoseGoal2Set3 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot2Set3 = new Pose(2.4,3.0,Math.PI); - Task task2Set3 = new Task(13,startPoseGoal2Set3,goalPoseRobot2Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set3 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot3Set3 = new Pose(17.0,2.8,-Math.PI/2); - Task task3Set3 = new Task(14,startPoseGoal3Set3,goalPoseRobot3Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set3 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot4Set3 = new Pose(3.0,28.0,0.0); - Task task4Set3 = new Task(15,startPoseGoal4Set3,goalPoseRobot4Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set3 = new Pose(7.5,33.5,0.0); - Pose goalPoseRobot5Set3 = new Pose(43.5,33.6,0.0); - Task task5Set3 = new Task(16,startPoseGoal5Set3,goalPoseRobot5Set3,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - - - - - - - //Forth set of tasks - //-> 4 more tasks (4 of type 1 and 0 of type 2) - Pose startPoseGoal1Set4 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot1Set4 = new Pose(25.0,15.0,0.0); - Task task1Set4 = new Task(17,startPoseGoal1Set4,goalPoseRobot1Set4,1); - ////////////////////////////////////// - Pose startPoseGoal2Set4 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot2Set4 = new Pose(20.0,24.5,0.0); - Task task2Set4 = new Task(18,startPoseGoal2Set4,goalPoseRobot2Set4,1); - ////////////////////////////////////// - Pose startPoseGoal3Set4 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot3Set4 = new Pose(24.0,11.0,0.0); - Task task3Set4 = new Task(19,startPoseGoal3Set4,goalPoseRobot3Set4,1); - ////////////////////////////////////// - Pose startPoseGoal4Set4 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot4Set4 = new Pose(6.0,33.5,Math.PI/2); - Task task4Set4 = new Task(20,startPoseGoal4Set4,goalPoseRobot4Set4,1); - ////////////////////////////////////// - //Fifth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set5 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set5 = new Pose(6.0,10.0,Math.PI); - Task task1Set5 = new Task(21,startPoseGoal1Set5,goalPoseRobot1Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set5 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set5 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set5 = new Task(22,startPoseGoal2Set5,goalPoseRobot2Set5,2); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set5 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set5 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set5 = new Task(23,startPoseGoal3Set5,goalPoseRobot3Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set5 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set5 = new Pose(23.0,19.0,0.0); - Task task4Set5 = new Task(24,startPoseGoal4Set5,goalPoseRobot4Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set5 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set5 = new Pose(33.5,7.5,0.0); - Task task5Set5 = new Task(25,startPoseGoal5Set5,goalPoseRobot5Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal6Set5 = new Pose(20.0,3.0,0.0); - Pose goalPoseRobot6Set5 = new Pose(25.5,2.0,Math.PI/2); - Task task6Set5 = new Task(26,startPoseGoal6Set5,goalPoseRobot6Set5,2); - //////////////////////////////////////////////////// - Pose startPoseGoal7Set5 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot7Set5 = new Pose(3.0,33.5,Math.PI/2); - Task task7Set5 = new Task(27,startPoseGoal7Set5,goalPoseRobot7Set5,1); - //////////////////////////////////////////////////// - ////////////////////////////////////// - //Sixth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot1Set6 = new Pose(43.5,27.6,0.0); - Task task1Set6 = new Task(28,startPoseGoal1Set6,goalPoseRobot1Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot2Set6 = new Pose(43.5,24.6,0.0); - Task task2Set6 = new Task(29,startPoseGoal2Set6,goalPoseRobot2Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set6 = new Pose(43.5,21.6,0.0); - Task task3Set6 = new Task(30,startPoseGoal3Set6,goalPoseRobot3Set6,2); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot4Set6 = new Pose(43.5,18.6,0.0); - Task task4Set6 = new Task(31,startPoseGoal4Set6,goalPoseRobot4Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot5Set6 = new Pose(43.5,15.6,0.0); - Task task5Set6 = new Task(32,startPoseGoal5Set6,goalPoseRobot5Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot6Set6 = new Pose(43.5,12.6,0.0); - Task task6Set6 = new Task(33,startPoseGoal6Set6,goalPoseRobot6Set6,2); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set6 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot7Set6 = new Pose(3.0,26.0,0.0); - Task task7Set6 = new Task(34,startPoseGoal7Set6,goalPoseRobot7Set6,1); - ///////////////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////////////// - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set10 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot1Set10 = new Pose(43.5,27.6,0.0); - Task task1Set10 = new Task(35,startPoseGoal1Set10,goalPoseRobot1Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set10 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot2Set10 = new Pose(43.5,24.6,0.0); - Task task2Set10 = new Task(36,startPoseGoal2Set10,goalPoseRobot2Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set10 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot3Set10 = new Pose(43.5,21.6,0.0); - Task task3Set10 = new Task(37,startPoseGoal3Set10,goalPoseRobot3Set10,2); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot4Set10 = new Pose(43.5,18.6,0.0); - Task task4Set10 = new Task(38,startPoseGoal4Set10,goalPoseRobot4Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot5Set10 = new Pose(43.5,15.6,0.0); - Task task5Set10 = new Task(39,startPoseGoal5Set10,goalPoseRobot5Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot6Set10 = new Pose(43.5,12.6,0.0); - Task task6Set10 = new Task(40,startPoseGoal6Set10,goalPoseRobot6Set10,2); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set10 = new Pose(16.0,32.0,0.0); - Pose goalPoseRobot7Set10 = new Pose(43.5,9.6,0.0); - Task task7Set10 = new Task(41,startPoseGoal7Set10,goalPoseRobot7Set10,1); - - - - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set9 = new Pose(29.0,4.0,Math.PI); - Pose goalPoseRobot1Set9 = new Pose(33.0,4.0,Math.PI); - Task task1Set9 = new Task(42,startPoseGoal1Set9,goalPoseRobot1Set9,1); - //////////////////////////////////////////////////////////// - Pose startPoseGoal2Set9 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set9 = new Pose(3.0,28.0,0.0); - Task task2Set9 = new Task(43,startPoseGoal2Set9,goalPoseRobot2Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal3Set9 = new Pose(6.0,20.0,0.0); - Pose goalPoseRobot3Set9 = new Pose(3.0,20.0,0.0); - Task task3Set9 = new Task(44,startPoseGoal3Set9,goalPoseRobot3Set9,2); - /////////////////////////////////////////////////// - Pose startPoseGoal4Set9 = new Pose(7.0,25.0,0.0); - Pose goalPoseRobot4Set9 = new Pose(3.0,25.0,0.0); - Task task4Set9 = new Task(45,startPoseGoal4Set9,goalPoseRobot4Set9,2); - /////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set9 = new Pose(8.0,5.8,Math.PI/2); - Pose goalPoseRobot5Set9 = new Pose(8.0,2.8,Math.PI/2); - Task task5Set9 = new Task(46,startPoseGoal5Set9,goalPoseRobot5Set9,1); - /////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set9 = new Pose(11.0,5.8,Math.PI/2); - Pose goalPoseRobot6Set9 = new Pose(11.0,2.8,Math.PI/2); - Task task6Set9 = new Task(47,startPoseGoal6Set9,goalPoseRobot6Set9,1); - /////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set9 = new Pose(18.0,7.8,Math.PI/2); - Pose goalPoseRobot7Set9 = new Pose(20.0,2.5,Math.PI/2); - Task task7Set9 = new Task(48,startPoseGoal7Set9,goalPoseRobot7Set9,1); - - - /////// - Thread.sleep(44000); - //Add the second set of tasks - assignmentProblem.addTask(task1Set2); - assignmentProblem.addTask(task2Set2); - assignmentProblem.addTask(task3Set2); - assignmentProblem.addTask(task4Set2); - ///Sleep for a while - - Thread.sleep(40000); - assignmentProblem.addTask(task1Set3); - assignmentProblem.addTask(task2Set3); - assignmentProblem.addTask(task3Set3); - assignmentProblem.addTask(task4Set3); - assignmentProblem.addTask(task5Set3); - - - Thread.sleep(60000); - - assignmentProblem.addTask(task1Set6); - assignmentProblem.addTask(task2Set6); - assignmentProblem.addTask(task3Set6); - assignmentProblem.addTask(task4Set6); - assignmentProblem.addTask(task5Set6); - assignmentProblem.addTask(task6Set6); - assignmentProblem.addTask(task7Set6); - - Thread.sleep(60000); - - assignmentProblem.addTask(task1Set4); - assignmentProblem.addTask(task2Set4); - assignmentProblem.addTask(task3Set4); - assignmentProblem.addTask(task4Set4); - - Thread.sleep(80000); - - assignmentProblem.addTask(task1Set5); - assignmentProblem.addTask(task2Set5); - assignmentProblem.addTask(task3Set5); - assignmentProblem.addTask(task4Set5); - assignmentProblem.addTask(task5Set5); - assignmentProblem.addTask(task6Set5); - assignmentProblem.addTask(task7Set5); - - - Thread.sleep(80000); - - assignmentProblem.addTask(task1Set10); - assignmentProblem.addTask(task2Set10); - assignmentProblem.addTask(task3Set10); - assignmentProblem.addTask(task4Set10); - assignmentProblem.addTask(task5Set10); - assignmentProblem.addTask(task6Set10); - assignmentProblem.addTask(task7Set10); - - Thread.sleep(80000); - - assignmentProblem.addTask(task1Set9); - assignmentProblem.addTask(task2Set9); - assignmentProblem.addTask(task3Set9); - assignmentProblem.addTask(task4Set9); - assignmentProblem.addTask(task5Set9); - assignmentProblem.addTask(task6Set9); - assignmentProblem.addTask(task7Set9); - - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseOld.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseOld.java deleted file mode 100644 index 7ebbd81..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseOld.java +++ /dev/null @@ -1,209 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentOrebroWarehouse { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - //Coordinate footprint1 = new Coordinate(-1.0,0.5); - //Coordinate footprint2 = new Coordinate(1.0,0.5); - //Coordinate footprint3 = new Coordinate(1.0,-0.5); - //Coordinate footprint4 = new Coordinate(-1.0,-0.5); - Coordinate footprint1 = new Coordinate(-0.5,0.5); - Coordinate footprint2 = new Coordinate(0.5,0.5); - Coordinate footprint3 = new Coordinate(0.5,-0.5); - Coordinate footprint4 = new Coordinate(-0.5,-0.5); - - Coordinate[] fp2 = new Coordinate[] { - new Coordinate(-1.0,0.0), - new Coordinate(0.0,1.0), - new Coordinate(1.0,0.0), - new Coordinate(-0.0,-1.0), - }; - - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(15, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - viz.setMap("maps/icra2016-basement-mod.yaml"); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp2 = new ReedsSheppCarPlanner(); - rsp2.setRadius(0.2); - rsp2.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp2.setTurningRadius(4.0); - rsp2.setDistanceBetweenPathPoints(0.5); - rsp2.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/icra2016-basement-mod.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/icra2016-basement-mod.yaml")); - //rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-corridors-vi.yaml")); - //double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-corridors-vi.yaml")); - rsp2.setMapResolution(res); - rsp2.setPlanningTimeInSecs(5); - - Pose startPoseRobot1 = new Pose(33.0,6.0,Math.PI); - Pose startPoseRobot2 = new Pose(3.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(3.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(3.0,25.0,0.0); - Pose startPoseRobot5 = new Pose(8.0,1.0,Math.PI/2); - Pose startPoseRobot6 = new Pose(11.0,1.0,Math.PI/2); - Pose startPoseRobot7 = new Pose(20.0,1.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2,2,fp2 ); - Robot robot3 = new Robot(3,1 ); - Robot robot4 = new Robot(4,1); - Robot robot5 = new Robot(5,1); - - Robot robot6 = new Robot(6,2,fp2); - Robot robot7 = new Robot(7,1); - - - tec.setFootprint(robot2.getRobotID(),fp2); - tec.setFootprint(robot6.getRobotID(),fp2); - - Pose startPoseGoal1 = new Pose(7.0,4.0,Math.PI/2); - Pose startPoseGoal2 = new Pose(13.0,20.0,0.0); - Pose startPoseGoal3 = new Pose(13.0,23.0,0.0); - Pose startPoseGoal4 = new Pose(13.0,20.0,0.0); - Pose startPoseGoal5 = new Pose(13.0,20.0,0.0); - Pose startPoseGoal6 = new Pose(11.0,5.0,Math.PI/2); - Pose startPoseGoal7 = new Pose(18.0,5.0,Math.PI/2); - - Pose goalPoseRobot1 = new Pose(7.0,15.5,Math.PI/2); - Pose goalPoseRobot2 = new Pose(25.0,18.0,0.0); - Pose goalPoseRobot3 = new Pose(22.0,23.0,0.0); - Pose goalPoseRobot4 = new Pose(31.5,6.0,Math.PI/2); - Pose goalPoseRobot5 = new Pose(26.0,15.0,0.0); - Pose goalPoseRobot6 = new Pose(3.0,23.0,0.0); - Pose goalPoseRobot7 = new Pose(25.0,10.0,0.0); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - tec.addRobot(robot6,startPoseRobot6); - tec.addRobot(robot7,startPoseRobot7); - - - - - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,2); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - Task task6 = new Task(4,startPoseGoal6,goalPoseRobot6,2); - Task task7 = new Task(5,startPoseGoal7,goalPoseRobot7,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 1.0; - TaskAssignmentSimple assignmentProblem = new TaskAssignmentSimple(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - assignmentProblem.addTask(task6); - assignmentProblem.addTask(task7); - assignmentProblem.setFleetVisualization(viz); - - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setDefaultMotionPlanner(rsp2); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.setLinearWeight(alpha); - //assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - - tec.setDefaultMotionPlanner(assignmentProblem.getDefaultMotionPlanner()); - - assignmentProblem.startTaskAssignment(tec); - //assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseOnline2.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseOnline2.java deleted file mode 100644 index 642df71..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseOnline2.java +++ /dev/null @@ -1,850 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Random; -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; - -import com.vividsolutions.jts.geom.Coordinate; - -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.NetworkConfiguration; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner.PLANNING_ALGORITHM; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimulatedAnnealing; -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; -import com.google.ortools.linearsolver.*; - - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentOrebroWarehouseOnline2 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - - //Maximum acceleration/deceleration and speed for all robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - - //Create a coordinator with interfaces to robots - //in the built-in 2D simulator - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Define a network with uncertainties (see Mannucci et al., 2019) - NetworkConfiguration.setDelays(0, 0); - NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS = 0.0; - - //Tell the coordinator - // (1) what is known about the communication channel, and - // (2) the accepted probability of constraint violation - tec.setNetworkParameters(NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS, NetworkConfiguration.getMaximumTxDelay(), 0.01); - - //Avoid deadlocks via global re-ordering - tec.setBreakDeadlocks(true, false, false); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - - //Set up infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - - //Start the thread that revises precedences at every period - tec.startInference(); - - //set the map yalm file - String yamlFile = new String("maps/basement.yaml"); - - //Start a visualization (will open a new browser tab) - BrowserVisualization viz = new BrowserVisualization(); - viz.setMap(yamlFile); - viz.setInitialTransform(13, 6.1, 6.8); - tec.setVisualization(viz); - tec.setBreakDeadlocks(true, false, false); - //Robot IDs can be non-sequential (but must be unique) - double xl = 1.0; - double yl = .5; - Coordinate footprint1 = new Coordinate(-xl,yl); - Coordinate footprint2 = new Coordinate(xl,yl); - Coordinate footprint3 = new Coordinate(xl,-yl); - Coordinate footprint4 = new Coordinate(-xl,-yl); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - - Coordinate[] fp2 = new Coordinate[] { - new Coordinate(-0.7,0.7), - new Coordinate(0.7,0.7), - new Coordinate(0.7,-0.7), - new Coordinate(-0.7,-0.7), - }; - - Robot robot1 = new Robot(1,1,tec.getDefaultFootprint()); - Robot robot2 = new Robot(2,1,tec.getDefaultFootprint()); - Robot robot3 = new Robot(3,1,tec.getDefaultFootprint()); - Robot robot4 = new Robot(4,1,tec.getDefaultFootprint()); - Robot robot5 = new Robot(5,1,tec.getDefaultFootprint()); - Robot robot6 = new Robot(6,1,tec.getDefaultFootprint()); - Robot robot7 = new Robot(7,1,tec.getDefaultFootprint()); - - //tec.setFootprint(2, fp2); - //tec.setFootprint(6, fp2); - //Define start and goal poses for each robot - //Pose startPoseRobot1 = new Pose(33.0,5.0,Math.PI); - Pose startPoseRobot1 = new Pose(33.0,6.0,Math.PI); //greedy - Pose startPoseRobot2 = new Pose(3.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(3.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(3.0,25.0,0.0); - Pose startPoseRobot5 = new Pose(8.0,2.8,Math.PI/2); - Pose startPoseRobot6 = new Pose(11.0,2.8,Math.PI/2); - Pose startPoseRobot7 = new Pose(20.0,2.8,Math.PI/2); - - - - - Pose startPoseGoal1 = new Pose(7.0,7.0,Math.PI/2); - Pose startPoseGoal2 = new Pose(13.0,20.0,0.0); - Pose startPoseGoal3 = new Pose(13.0,24.25,0.0); - Pose startPoseGoal4 = new Pose(25.0,5.0,0.0); - Pose startPoseGoal5 = new Pose(18.0,15.0,0.0); - Pose startPoseGoal6 = new Pose(11.0,5.0,Math.PI/2); - Pose startPoseGoal7 = new Pose(20.0,11.0,0.0); - - Pose goalPoseRobot1 = new Pose(6.0,15.5,Math.PI/2); - Pose goalPoseRobot2 = new Pose(3.0,23.0,0.0); - Pose goalPoseRobot3 = new Pose(20.0,24.25,0.0); - //Pose goalPoseRobot4 = new Pose(30.5,7.0,Math.PI/2); - Pose goalPoseRobot4 = new Pose(28.5,7.0,Math.PI/2); - Pose goalPoseRobot5 = new Pose(25.0,15.0,0.0); - Pose goalPoseRobot6 = new Pose(3.0,26.0,0.0); - Pose goalPoseRobot7 = new Pose(24.0,11.0,0.0); - - - /////////////////////////////////////////////// - - //Set the default motion planner - ReedsSheppCarPlanner rsp1 = new ReedsSheppCarPlanner(PLANNING_ALGORITHM.RRTstar); - rsp1.setRadius(0.1); - rsp1.setTurningRadius(4.0); - rsp1.setFootprint(tec.getDefaultFootprint()); - rsp1.setDistanceBetweenPathPoints(0.5); - rsp1.setMap(yamlFile); - rsp1.setPlanningTimeInSecs(10); - - tec.addRobot(robot1, startPoseRobot1); - tec.addRobot(robot2, startPoseRobot2); - tec.addRobot(robot3, startPoseRobot3); - tec.addRobot(robot4, startPoseRobot4); - tec.addRobot(robot5, startPoseRobot5); - tec.addRobot(robot6, startPoseRobot6); - tec.addRobot(robot7, startPoseRobot7); - - //Primo set di tasks - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - Task task6 = new Task(6,startPoseGoal6,goalPoseRobot6,1); - Task task7 = new Task(7,startPoseGoal7,goalPoseRobot7,1); - - - - - - int a = 1; - //FOLDER NUMBER - int b = 2; - tec.setTestNumber(a); - tec.setFolderNumber(b); - - TaskAssignment assignmentProblem = new TaskAssignment(); - - //TaskAssignmentSimulatedAnnealing assignmentProblem = new TaskAssignmentSimulatedAnnealing(); - - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - assignmentProblem.setFleetVisualization(viz); - int numPaths = 1; - - for (int robotID : tec.getIdleRobots()) { - - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = (ReedsSheppCarPlanner) rsp1.getCopy(false); - tec.setMotionPlanner(robotID, rsp); - } - - - ///////////////////////// ////////////////////////////// - //Solve the problem to find some feasible solution - assignmentProblem.setTimeOutinMin(1.5); - double alpha = 0.5; - double [][][]optimalAllocation = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - - }; - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario1"); - //assignmentProblem.LoadScenarioAllocation(optimalAllocation); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - //tec.setFakeCoordinator(true); - //assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - //assignmentProblem.setSaveFutureAllocation(true); - - //assignmentProblem.startTaskAssignment(tec); - assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - //assignmentProblem.startTaskAssignmentLocalSearchAlgorithm(tec, -1); - //assignmentProblem.startTaskAssignment(tec); - - - //New tasks real - //SET 2 - //Second set of tasks - //-> 4 more tasks ( 3 of type 1 and 1 of type 2) - Pose startPoseGoal1Set2 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot1Set2 = new Pose(15.0,28.0,Math.PI/2); - Task task1Set2 = new Task(8,startPoseGoal1Set2,goalPoseRobot1Set2,1); - /////////////////// - Pose startPoseGoal2Set2 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot2Set2 = new Pose(33.5,4.0,0.0); - Task task2Set2 = new Task(9,startPoseGoal2Set2,goalPoseRobot2Set2,1); - /////////////////// - Pose startPoseGoal3Set2 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot3Set2 = new Pose(33.7,2.0,0.0); - Task task3Set2 = new Task(10,startPoseGoal3Set2,goalPoseRobot3Set2,1); - /////////////////// - Pose startPoseGoal4Set2 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot4Set2 = new Pose(2.8,6.5,Math.PI); - Task task4Set2 = new Task(11,startPoseGoal4Set2,goalPoseRobot4Set2,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - //Third set of tasks - //-> 5 more tasks (4 of type 1 and 1 of type 2) - Pose startPoseGoal1Set3 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot1Set3 = new Pose(43.5,30.6,0.0); - Task task1Set3 = new Task(12,startPoseGoal1Set3,goalPoseRobot1Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal2Set3 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot2Set3 = new Pose(2.4,3.0,Math.PI); - Task task2Set3 = new Task(13,startPoseGoal2Set3,goalPoseRobot2Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set3 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot3Set3 = new Pose(17.0,2.8,-Math.PI/2); - Task task3Set3 = new Task(14,startPoseGoal3Set3,goalPoseRobot3Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set3 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot4Set3 = new Pose(3.0,28.0,0.0); - Task task4Set3 = new Task(15,startPoseGoal4Set3,goalPoseRobot4Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set3 = new Pose(23.5,33.5,0.0); - Pose goalPoseRobot5Set3 = new Pose(43.5,33.6,0.0); - Task task5Set3 = new Task(16,startPoseGoal5Set3,goalPoseRobot5Set3,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - - - - - - - //Forth set of tasks - //-> 4 more tasks (4 of type 1 and 0 of type 2) - Pose startPoseGoal1Set4 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot1Set4 = new Pose(25.0,15.0,0.0); - Task task1Set4 = new Task(17,startPoseGoal1Set4,goalPoseRobot1Set4,1); - ////////////////////////////////////// - Pose startPoseGoal2Set4 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot2Set4 = new Pose(20.0,24.5,0.0); - Task task2Set4 = new Task(18,startPoseGoal2Set4,goalPoseRobot2Set4,1); - ////////////////////////////////////// - Pose startPoseGoal3Set4 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot3Set4 = new Pose(24.0,11.0,0.0); - Task task3Set4 = new Task(19,startPoseGoal3Set4,goalPoseRobot3Set4,1); - ////////////////////////////////////// - Pose startPoseGoal4Set4 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot4Set4 = new Pose(6.0,33.5,Math.PI/2); - Task task4Set4 = new Task(20,startPoseGoal4Set4,goalPoseRobot4Set4,1); - ////////////////////////////////////// - //Fifth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set5 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set5 = new Pose(6.0,10.0,Math.PI); - Task task1Set5 = new Task(21,startPoseGoal1Set5,goalPoseRobot1Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set5 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set5 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set5 = new Task(22,startPoseGoal2Set5,goalPoseRobot2Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set5 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set5 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set5 = new Task(23,startPoseGoal3Set5,goalPoseRobot3Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set5 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set5 = new Pose(23.0,19.0,0.0); - Task task4Set5 = new Task(24,startPoseGoal4Set5,goalPoseRobot4Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set5 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set5 = new Pose(33.5,7.5,0.0); - Task task5Set5 = new Task(25,startPoseGoal5Set5,goalPoseRobot5Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal6Set5 = new Pose(20.0,4.0,0.0); - Pose goalPoseRobot6Set5 = new Pose(25.5,3.0,Math.PI/2); - Task task6Set5 = new Task(26,startPoseGoal6Set5,goalPoseRobot6Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal7Set5 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot7Set5 = new Pose(3.0,33.5,Math.PI/2); - Task task7Set5 = new Task(27,startPoseGoal7Set5,goalPoseRobot7Set5,1); - //////////////////////////////////////////////////// - ////////////////////////////////////// - //Sixth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set6 = new Pose(18.0,33.0,0.0); - Pose goalPoseRobot1Set6 = new Pose(43.5,27.6,0.0); - Task task1Set6 = new Task(28,startPoseGoal1Set6,goalPoseRobot1Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set6 = new Pose(19.0,33.0,0.0); - Pose goalPoseRobot2Set6 = new Pose(43.5,24.6,0.0); - Task task2Set6 = new Task(29,startPoseGoal2Set6,goalPoseRobot2Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set6 = new Pose(43.5,21.6,0.0); - Task task3Set6 = new Task(30,startPoseGoal3Set6,goalPoseRobot3Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set6 = new Pose(22.0,33.0,0.0); - Pose goalPoseRobot4Set6 = new Pose(43.5,18.6,0.0); - Task task4Set6 = new Task(31,startPoseGoal4Set6,goalPoseRobot4Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set6 = new Pose(21.0,33.0,0.0); - Pose goalPoseRobot5Set6 = new Pose(43.5,15.6,0.0); - Task task5Set6 = new Task(32,startPoseGoal5Set6,goalPoseRobot5Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set6 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot6Set6 = new Pose(43.5,12.6,0.0); - Task task6Set6 = new Task(33,startPoseGoal6Set6,goalPoseRobot6Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set6 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot7Set6 = new Pose(3.0,26.0,0.0); - Task task7Set6 = new Task(34,startPoseGoal7Set6,goalPoseRobot7Set6,1); - ///////////////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////////////// - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set10 = new Pose(22.0,33.0,0.0); - Pose goalPoseRobot1Set10 = new Pose(43.5,27.6,0.0); - Task task1Set10 = new Task(35,startPoseGoal1Set10,goalPoseRobot1Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set10 = new Pose(24.0,33.0,0.0); - Pose goalPoseRobot2Set10 = new Pose(43.5,24.6,0.0); - Task task2Set10 = new Task(36,startPoseGoal2Set10,goalPoseRobot2Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set10 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot3Set10 = new Pose(43.5,21.6,0.0); - Task task3Set10 = new Task(37,startPoseGoal3Set10,goalPoseRobot3Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot4Set10 = new Pose(43.5,18.6,0.0); - Task task4Set10 = new Task(38,startPoseGoal4Set10,goalPoseRobot4Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set10 = new Pose(18.0,32.0,0.0); - Pose goalPoseRobot5Set10 = new Pose(43.5,15.6,0.0); - Task task5Set10 = new Task(39,startPoseGoal5Set10,goalPoseRobot5Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot6Set10 = new Pose(43.5,12.6,0.0); - Task task6Set10 = new Task(40,startPoseGoal6Set10,goalPoseRobot6Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set10 = new Pose(16.0,32.0,0.0); - Pose goalPoseRobot7Set10 = new Pose(43.5,9.6,0.0); - Task task7Set10 = new Task(41,startPoseGoal7Set10,goalPoseRobot7Set10,1); - - - - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set9 = new Pose(29.0,4.0,Math.PI); - //Pose goalPoseRobot1Set9 = new Pose(33.0,4.0,Math.PI); - Pose goalPoseRobot1Set9 = new Pose(33.0,6.0,Math.PI); //greedy - Task task1Set9 = new Task(42,startPoseGoal1Set9,goalPoseRobot1Set9,1); - //////////////////////////////////////////////////////////// - Pose startPoseGoal2Set9 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set9 = new Pose(3.0,28.0,0.0); - Task task2Set9 = new Task(43,startPoseGoal2Set9,goalPoseRobot2Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal3Set9 = new Pose(6.0,20.0,0.0); - Pose goalPoseRobot3Set9 = new Pose(3.0,20.0,0.0); - Task task3Set9 = new Task(44,startPoseGoal3Set9,goalPoseRobot3Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal4Set9 = new Pose(7.0,25.0,0.0); - Pose goalPoseRobot4Set9 = new Pose(3.0,25.0,0.0); - Task task4Set9 = new Task(45,startPoseGoal4Set9,goalPoseRobot4Set9,1); - ////////////////////////////////////////////////////////////////// - ////////////////////////GIVE AGAIN THE SAME TASKS TO HAVE 75 TOTAL TASKS/////////////////////////////////////////// - ////// - //SET 11 - Pose startPoseGoal1Set11 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot1Set11 = new Pose(3.0,26.0,0.0); - Task task1Set11 = new Task(46,startPoseGoal1Set11,goalPoseRobot1Set11,1); - /////////////////// - Pose startPoseGoal2Set11 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot2Set11 = new Pose(24.0,11.0,0.0); - Task task2Set11 = new Task(47,startPoseGoal2Set11,goalPoseRobot2Set11,1); - //////////////////////////////////////////// - - Pose startPoseGoal3Set11 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot3Set11 = new Pose(14.5,28.0,Math.PI/2); - Task task3Set11 = new Task(48,startPoseGoal3Set11,goalPoseRobot3Set11,1); - /////////////////// - Pose startPoseGoal4Set11 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot4Set11 = new Pose(33.5,4.0,0.0); - Task task4Set11 = new Task(49,startPoseGoal4Set11,goalPoseRobot4Set11,1); - /////////////////// - Pose startPoseGoal5Set11 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot5Set11 = new Pose(33.7,2.0,0.0); - Task task5Set11 = new Task(50,startPoseGoal5Set11,goalPoseRobot5Set11,1); - //////////////////////////////////////// - /////////////////////////////////////// - //SET 12 - /////////////////// - Pose startPoseGoal1Set12 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set12 = new Pose(2.8,6.5,Math.PI); - Task task1Set12 = new Task(51,startPoseGoal1Set12,goalPoseRobot1Set12,1); - ////////////////////////////////////////////// - Pose startPoseGoal2Set12 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot2Set12 = new Pose(43.5,30.6,0.0); - Task task2Set12 = new Task(52,startPoseGoal2Set12,goalPoseRobot2Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set12 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot3Set12 = new Pose(2.4,3.0,Math.PI); - Task task3Set12 = new Task(53,startPoseGoal3Set12,goalPoseRobot3Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set12 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot4Set12 = new Pose(17.0,2.8,-Math.PI/2); - Task task4Set12 = new Task(54,startPoseGoal4Set12,goalPoseRobot4Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set12 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot5Set12 = new Pose(3.0,28.0,0.0); - Task task5Set12 = new Task(55,startPoseGoal5Set12,goalPoseRobot5Set12,1); - //////////////////////////////////////////// - /////////////////////////////////////////// - //SET 13 - Pose startPoseGoal1Set13 = new Pose(23.5,33.5,0.0); - Pose goalPoseRobot1Set13 = new Pose(43.5,33.6,0.0); - Task task1Set13 = new Task(56,startPoseGoal1Set13,goalPoseRobot1Set13,1); - ////////////////////////////////////////////// - Pose startPoseGoal2Set13 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot2Set13 = new Pose(25.0,15.0,0.0); - Task task2Set13 = new Task(57,startPoseGoal2Set13,goalPoseRobot2Set13,1); - ////////////////////////////////////// - Pose startPoseGoal3Set13 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot3Set13 = new Pose(20.0,24.5,0.0); - Task task3Set13 = new Task(58,startPoseGoal3Set13,goalPoseRobot3Set13,1); - ////////////////////////////////////// - Pose startPoseGoal4Set13 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot4Set13 = new Pose(24.0,11.0,0.0); - Task task4Set13 = new Task(59,startPoseGoal4Set13,goalPoseRobot4Set13,1); - ////////////////////////////////////// - Pose startPoseGoal5Set13 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot5Set13 = new Pose(6.0,33.5,Math.PI/2); - Task task5Set13 = new Task(60,startPoseGoal5Set13,goalPoseRobot5Set13,1); - ////////////////////////////////////// - ///////////////////////////////////////////////////////// - //SET 14 - Pose startPoseGoal1Set14 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set14 = new Pose(6.0,10.0,Math.PI); - Task task1Set14 = new Task(61,startPoseGoal1Set14,goalPoseRobot1Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set14 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set14 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set14 = new Task(62,startPoseGoal2Set14,goalPoseRobot2Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set14 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set14 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set14 = new Task(63,startPoseGoal3Set14,goalPoseRobot3Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set14 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set14 = new Pose(23.0,19.0,0.0); - Task task4Set14 = new Task(64,startPoseGoal4Set14,goalPoseRobot4Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set14 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set14 = new Pose(33.5,7.5,0.0); - Task task5Set14 = new Task(65,startPoseGoal5Set14,goalPoseRobot5Set14,1); - //////////////////////////////////////////////////// - /////////////////////////////////////////////////// - //SET 15 - Pose startPoseGoal1Set15 = new Pose(20.0,4.0,0.0); - Pose goalPoseRobot1Set15 = new Pose(25.5,3.0,Math.PI/2); - Task task1Set15 = new Task(66,startPoseGoal1Set15,goalPoseRobot1Set15,1); - /////////////////////////////////////// - Pose startPoseGoal2Set15 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set15 = new Pose(3.0,33.5,Math.PI/2); - Task task2Set15 = new Task(67,startPoseGoal2Set15,goalPoseRobot2Set15,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set15 = new Pose(43.5,27.6,0.0); - Task task3Set15 = new Task(68,startPoseGoal3Set15,goalPoseRobot3Set15,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot4Set15 = new Pose(43.5,24.6,0.0); - Task task4Set15 = new Task(69,startPoseGoal4Set15,goalPoseRobot4Set15,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot5Set15 = new Pose(43.5,21.6,0.0); - Task task5Set15 = new Task(70,startPoseGoal5Set15,goalPoseRobot5Set15,1); - ///////////////////////////////////////////////////////////////////// - //////////////////////////// - //SET 16 - - Pose startPoseGoal1Set16 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot1Set16 = new Pose(43.5,18.6,0.0); - Task task1Set16 = new Task(71,startPoseGoal1Set16,goalPoseRobot1Set16,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set16 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot2Set16 = new Pose(43.5,15.6,0.0); - Task task2Set16 = new Task(72,startPoseGoal2Set16,goalPoseRobot2Set16,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set16 = new Pose(8.0,5.8,Math.PI/2); - Pose goalPoseRobot3Set16 = new Pose(8.0,2.8,Math.PI/2); - Task task3Set16 = new Task(73,startPoseGoal3Set16,goalPoseRobot3Set16,1); - //////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set16 = new Pose(11.0,5.8,Math.PI/2); - Pose goalPoseRobot4Set16 = new Pose(11.0,2.8,Math.PI/2); - Task task4Set16 = new Task(74,startPoseGoal4Set16,goalPoseRobot4Set16,1); - /////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set16 = new Pose(18.0,7.8,Math.PI/2); - Pose goalPoseRobot5Set16 = new Pose(20.0,2.5,Math.PI/2); - Task task5Set16 = new Task(75,startPoseGoal5Set16,goalPoseRobot5Set16,1); - - - - - - - - - - - - //TEST ZONE - Pose startPoseGoal1New = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1New = new Pose(2.0,5.0,Math.PI); - - Robot robot1New = new Robot(8, 1,tec.getDefaultFootprint() ); - Robot robot2New = new Robot(9,2,tec.getDefaultFootprint() ); - Robot robot3New = new Robot(10,1,tec.getDefaultFootprint() ); - Robot robot4New = new Robot(11,1,tec.getDefaultFootprint()); - Robot robot5New = new Robot(12,1,tec.getDefaultFootprint()); - Robot robot6New = new Robot(13,2,fp2); - - //tec.addRobot(robot1New,startPoseGoal1Set3); - //tec.addRobot(robot2New,goalPoseRobot1Set3); - //tec.addRobot(robot3New,startPoseGoal1Set7); - //tec.addRobot(robot4New, startPoseGoal7Set5); - //tec.addRobot(robot5New, startPoseGoal6Set5); - //tec.addRobot(robot6New, goalPoseRobot6Set5); - /////// - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(55000); //sys alpha != 1 synchronous - //Thread.sleep(550000); //sys alpha = 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(120000); //GA alpha != 1 asynchronous - Thread.sleep(50000); //greedy - //Thread.sleep(450000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - //Add the second set of tasks - assignmentProblem.addTask(task6); - assignmentProblem.addTask(task1Set10); - assignmentProblem.addTask(task1Set2); - assignmentProblem.addTask(task2Set2); - assignmentProblem.addTask(task3Set2); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario2"); - double [][][]optimalAllocation2 = {{{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - - }; - //assignmentProblem.LoadScenarioAllocation(optimalAllocation2); - - - ///Sleep for a while - - //Thread.sleep(550000); //sys alpha = 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 synchronous - //Thread.sleep(170000); //SA alpha != 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 asynchronous - //Thread.sleep(100000); //sys - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set3); - assignmentProblem.addTask(task2Set3); - assignmentProblem.addTask(task3Set3); - assignmentProblem.addTask(task4Set3); - assignmentProblem.addTask(task5Set3); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario3"); - double [][][]optimalAllocation3 = {{{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - //assignmentProblem.LoadScenarioAllocation(optimalAllocation3); - - - //Thread.sleep(150000); //sys synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(250000); //sys alpha != 1 asynchronous - //Thread.sleep(250000); - //Thread.sleep(500000); - //Thread.sleep(75000); - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task4Set2); - assignmentProblem.addTask(task1Set6); - assignmentProblem.addTask(task2Set6); - assignmentProblem.addTask(task3Set6); - assignmentProblem.addTask(task4Set6); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario4"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(130000); //sys alpha != 1 asynchronous - //Thread.sleep(160000); //sys alpha != 1 asynchronous - //Thread.sleep(600000); //sys alpha _=! 1 - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task5Set6); - assignmentProblem.addTask(task6Set6); - assignmentProblem.addTask(task7Set6); - assignmentProblem.addTask(task7); - assignmentProblem.addTask(task1Set4); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario5"); - - //Thread.sleep(300000); - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(180000); //sys alpha != 1 asynchronous - //Thread.sleep(250000); //sys alpha != 1 asynchronous - //Thread.sleep(500000); //sys - Thread.sleep(80000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task2Set4); - assignmentProblem.addTask(task3Set4); - assignmentProblem.addTask(task4Set4); - assignmentProblem.addTask(task1Set5); - assignmentProblem.addTask(task2Set5); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario6"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(500000); //sys - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - assignmentProblem.addTask(task3Set5); - assignmentProblem.addTask(task4Set5); - assignmentProblem.addTask(task5Set5); - assignmentProblem.addTask(task6Set5); - assignmentProblem.addTask(task7Set5); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario7"); - - - //Thread.sleep(150000);//sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(650000); //sys - Thread.sleep(80000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task2Set10); - assignmentProblem.addTask(task3Set10); - assignmentProblem.addTask(task4Set10); - assignmentProblem.addTask(task5Set10); - assignmentProblem.addTask(task6Set10); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario8"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(650000); //sys - Thread.sleep(80000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task7Set10); - assignmentProblem.addTask(task1Set9); - assignmentProblem.addTask(task2Set9); - assignmentProblem.addTask(task3Set9); - assignmentProblem.addTask(task4Set9); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario9"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(200000); //GD alpha != 1 asynchronous - //Thread.sleep(650000); //sys - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - assignmentProblem.addTask(task1Set11); - assignmentProblem.addTask(task2Set11); - assignmentProblem.addTask(task3Set11); - assignmentProblem.addTask(task4Set11); - assignmentProblem.addTask(task5Set11); - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario10"); - - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(650000); //sys - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set12); - assignmentProblem.addTask(task2Set12); - assignmentProblem.addTask(task3Set12); - assignmentProblem.addTask(task4Set12); - assignmentProblem.addTask(task5Set12); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario11"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(650000); //sys - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set13); - assignmentProblem.addTask(task2Set13); - assignmentProblem.addTask(task3Set13); - assignmentProblem.addTask(task4Set13); - assignmentProblem.addTask(task5Set13); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario12"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(650000); //sys - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task1Set14); - assignmentProblem.addTask(task2Set14); - assignmentProblem.addTask(task3Set14); - assignmentProblem.addTask(task4Set14); - assignmentProblem.addTask(task5Set14); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario13"); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(650000); //sys - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set15); - assignmentProblem.addTask(task2Set15); - assignmentProblem.addTask(task3Set15); - assignmentProblem.addTask(task4Set15); - assignmentProblem.addTask(task5Set15); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario14"); - - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - //Thread.sleep(120000); //sys alpha != 1 asynchronous - //Thread.sleep(650000); //sys - Thread.sleep(50000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task1Set16); - assignmentProblem.addTask(task2Set16); - assignmentProblem.addTask(task3Set16); - assignmentProblem.addTask(task4Set16); - assignmentProblem.addTask(task5Set16); - - //assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario15"); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseWithDistance.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseWithDistance.java deleted file mode 100644 index cf87c8b..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseWithDistance.java +++ /dev/null @@ -1,956 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Random; -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; - -import com.vividsolutions.jts.geom.Coordinate; - -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.NetworkConfiguration; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner.PLANNING_ALGORITHM; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; -import com.google.ortools.linearsolver.*; - - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentOrebroWarehouseWithDistance { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - - //Maximum acceleration/deceleration and speed for all robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - - //Create a coordinator with interfaces to robots - //in the built-in 2D simulator - //final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Define a network with uncertainties (see Mannucci et al., 2019) - NetworkConfiguration.setDelays(0, 0); - NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS = 0.0; - - //Tell the coordinator - // (1) what is known about the communication channel, and - // (2) the accepted probability of constraint violation - tec.setNetworkParameters(NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS, NetworkConfiguration.getMaximumTxDelay(), 0.01); - - //Avoid deadlocks via global re-ordering - tec.setBreakDeadlocks(true, false, false); - - //Need to instantiate the fleetmaster interface - //tec.instantiateFleetMaster(0.1, true); - - //Set up infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - - //Start the thread that revises precedences at every period - tec.startInference(); - - //set the map yalm file - String yamlFile = new String("maps/basement.yaml"); - - //Start a visualization (will open a new browser tab) - BrowserVisualization viz = new BrowserVisualization(); - viz.setMap(yamlFile); - viz.setInitialTransform(13, 6.1, 6.8); - tec.setVisualization(viz); - tec.setBreakDeadlocks(true, false, false); - //Robot IDs can be non-sequential (but must be unique) - double xl = 1.0; - double yl = .5; - Coordinate footprint1 = new Coordinate(-xl,yl); - Coordinate footprint2 = new Coordinate(xl,yl); - Coordinate footprint3 = new Coordinate(xl,-yl); - Coordinate footprint4 = new Coordinate(-xl,-yl); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - - Coordinate[] fp2 = new Coordinate[] { - new Coordinate(-0.7,0.7), - new Coordinate(0.7,0.7), - new Coordinate(0.7,-0.7), - new Coordinate(-0.7,-0.7), - }; - - Robot robot1 = new Robot(1,1,tec.getDefaultFootprint()); - Robot robot2 = new Robot(2,1,tec.getDefaultFootprint()); - Robot robot3 = new Robot(3,1,tec.getDefaultFootprint()); - Robot robot4 = new Robot(4,1,tec.getDefaultFootprint()); - Robot robot5 = new Robot(5,1,tec.getDefaultFootprint()); - Robot robot6 = new Robot(6,1,tec.getDefaultFootprint()); - Robot robot7 = new Robot(7,1,tec.getDefaultFootprint()); - - //tec.setFootprint(2, fp2); - //tec.setFootprint(6, fp2); - //Define start and goal poses for each robot - Pose startPoseRobot1 = new Pose(33.0,5.0,Math.PI); - Pose startPoseRobot2 = new Pose(3.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(3.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(3.0,25.0,0.0); - Pose startPoseRobot5 = new Pose(8.0,2.8,Math.PI/2); - Pose startPoseRobot6 = new Pose(11.0,2.8,Math.PI/2); - Pose startPoseRobot7 = new Pose(20.0,2.8,Math.PI/2); - - - - - Pose startPoseGoal1 = new Pose(7.0,7.0,Math.PI/2); - Pose startPoseGoal2 = new Pose(13.0,20.0,0.0); - Pose startPoseGoal3 = new Pose(13.0,24.25,0.0); - Pose startPoseGoal4 = new Pose(25.0,5.0,0.0); - Pose startPoseGoal5 = new Pose(18.0,15.0,0.0); - Pose startPoseGoal6 = new Pose(11.0,5.0,Math.PI/2); - Pose startPoseGoal7 = new Pose(20.0,11.0,0.0); - - Pose goalPoseRobot1 = new Pose(6.0,15.5,Math.PI/2); - Pose goalPoseRobot2 = new Pose(3.0,23.0,0.0); - Pose goalPoseRobot3 = new Pose(20.0,24.25,0.0); - Pose goalPoseRobot4 = new Pose(30.5,7.0,Math.PI/2); - Pose goalPoseRobot5 = new Pose(25.0,15.0,0.0); - Pose goalPoseRobot6 = new Pose(3.0,26.0,0.0); - Pose goalPoseRobot7 = new Pose(24.0,11.0,0.0); - - - /////////////////////////////////////////////// - - //Set the default motion planner - ReedsSheppCarPlanner rsp1 = new ReedsSheppCarPlanner(PLANNING_ALGORITHM.RRTstar); - rsp1.setRadius(0.1); - rsp1.setTurningRadius(4.0); - rsp1.setFootprint(tec.getDefaultFootprint()); - rsp1.setDistanceBetweenPathPoints(0.5); - rsp1.setMap(yamlFile); - rsp1.setPlanningTimeInSecs(10); - - tec.addRobot(robot1, startPoseRobot1); - tec.addRobot(robot2, startPoseRobot2); - tec.addRobot(robot3, startPoseRobot3); - tec.addRobot(robot4, startPoseRobot4); - tec.addRobot(robot5, startPoseRobot5); - tec.addRobot(robot6, startPoseRobot6); - tec.addRobot(robot7, startPoseRobot7); - - //Primo set di tasks - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - Task task6 = new Task(6,startPoseGoal6,goalPoseRobot6,1); - Task task7 = new Task(7,startPoseGoal7,goalPoseRobot7,1); - - - - - - int a = 1; - //FOLDER NUMBER - int b = 2; - tec.setTestNumber(a); - tec.setFolderNumber(b); - - TaskAssignment assignmentProblem = new TaskAssignment(); - - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - assignmentProblem.setFleetVisualization(viz); - int numPaths = 1; - - for (int robotID : tec.getIdleRobots()) { - - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = (ReedsSheppCarPlanner) rsp1.getCopy(false); - tec.setMotionPlanner(robotID, rsp); - } - - - ///////////////////////// ////////////////////////////// - //Solve the problem to find some feasible solution - assignmentProblem.setTimeOutinMin(6.5); - double alpha = 0.5; - double [][][]optimalAllocation = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - }; - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario1"); - assignmentProblem.LoadScenarioAllocation(optimalAllocation); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - //tec.setFakeCoordinator(true); - //assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - - assignmentProblem.startTaskAssignment(tec); - //assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - //assignmentProblem.startTaskAssignmentLocalSearchAlgorithm(tec, -1); - - - //New tasks real - //SET 2 - //Second set of tasks - //-> 4 more tasks ( 3 of type 1 and 1 of type 2) - Pose startPoseGoal1Set2 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot1Set2 = new Pose(15.0,28.0,Math.PI/2); - Task task1Set2 = new Task(8,startPoseGoal1Set2,goalPoseRobot1Set2,1); - /////////////////// - Pose startPoseGoal2Set2 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot2Set2 = new Pose(33.5,4.0,0.0); - Task task2Set2 = new Task(9,startPoseGoal2Set2,goalPoseRobot2Set2,1); - /////////////////// - Pose startPoseGoal3Set2 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot3Set2 = new Pose(33.7,2.0,0.0); - Task task3Set2 = new Task(10,startPoseGoal3Set2,goalPoseRobot3Set2,1); - /////////////////// - Pose startPoseGoal4Set2 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot4Set2 = new Pose(2.8,6.5,Math.PI); - Task task4Set2 = new Task(11,startPoseGoal4Set2,goalPoseRobot4Set2,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - //Third set of tasks - //-> 5 more tasks (4 of type 1 and 1 of type 2) - Pose startPoseGoal1Set3 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot1Set3 = new Pose(43.5,30.6,0.0); - Task task1Set3 = new Task(12,startPoseGoal1Set3,goalPoseRobot1Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal2Set3 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot2Set3 = new Pose(2.4,3.0,Math.PI); - Task task2Set3 = new Task(13,startPoseGoal2Set3,goalPoseRobot2Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set3 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot3Set3 = new Pose(17.0,2.8,-Math.PI/2); - Task task3Set3 = new Task(14,startPoseGoal3Set3,goalPoseRobot3Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set3 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot4Set3 = new Pose(3.0,28.0,0.0); - Task task4Set3 = new Task(15,startPoseGoal4Set3,goalPoseRobot4Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set3 = new Pose(23.5,33.5,0.0); - Pose goalPoseRobot5Set3 = new Pose(43.5,33.6,0.0); - Task task5Set3 = new Task(16,startPoseGoal5Set3,goalPoseRobot5Set3,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - - - - - - - //Forth set of tasks - //-> 4 more tasks (4 of type 1 and 0 of type 2) - Pose startPoseGoal1Set4 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot1Set4 = new Pose(25.0,15.0,0.0); - Task task1Set4 = new Task(17,startPoseGoal1Set4,goalPoseRobot1Set4,1); - ////////////////////////////////////// - Pose startPoseGoal2Set4 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot2Set4 = new Pose(20.0,24.5,0.0); - Task task2Set4 = new Task(18,startPoseGoal2Set4,goalPoseRobot2Set4,1); - ////////////////////////////////////// - Pose startPoseGoal3Set4 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot3Set4 = new Pose(24.0,11.0,0.0); - Task task3Set4 = new Task(19,startPoseGoal3Set4,goalPoseRobot3Set4,1); - ////////////////////////////////////// - Pose startPoseGoal4Set4 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot4Set4 = new Pose(6.0,33.5,Math.PI/2); - Task task4Set4 = new Task(20,startPoseGoal4Set4,goalPoseRobot4Set4,1); - ////////////////////////////////////// - //Fifth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set5 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set5 = new Pose(6.0,10.0,Math.PI); - Task task1Set5 = new Task(21,startPoseGoal1Set5,goalPoseRobot1Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set5 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set5 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set5 = new Task(22,startPoseGoal2Set5,goalPoseRobot2Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set5 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set5 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set5 = new Task(23,startPoseGoal3Set5,goalPoseRobot3Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set5 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set5 = new Pose(23.0,19.0,0.0); - Task task4Set5 = new Task(24,startPoseGoal4Set5,goalPoseRobot4Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set5 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set5 = new Pose(33.5,7.5,0.0); - Task task5Set5 = new Task(25,startPoseGoal5Set5,goalPoseRobot5Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal6Set5 = new Pose(20.0,4.0,0.0); - Pose goalPoseRobot6Set5 = new Pose(25.5,3.0,Math.PI/2); - Task task6Set5 = new Task(26,startPoseGoal6Set5,goalPoseRobot6Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal7Set5 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot7Set5 = new Pose(3.0,33.5,Math.PI/2); - Task task7Set5 = new Task(27,startPoseGoal7Set5,goalPoseRobot7Set5,1); - //////////////////////////////////////////////////// - ////////////////////////////////////// - //Sixth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set6 = new Pose(18.0,33.0,0.0); - Pose goalPoseRobot1Set6 = new Pose(43.5,27.6,0.0); - Task task1Set6 = new Task(28,startPoseGoal1Set6,goalPoseRobot1Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set6 = new Pose(19.0,33.0,0.0); - Pose goalPoseRobot2Set6 = new Pose(43.5,24.6,0.0); - Task task2Set6 = new Task(29,startPoseGoal2Set6,goalPoseRobot2Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set6 = new Pose(43.5,21.6,0.0); - Task task3Set6 = new Task(30,startPoseGoal3Set6,goalPoseRobot3Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set6 = new Pose(22.0,33.0,0.0); - Pose goalPoseRobot4Set6 = new Pose(43.5,18.6,0.0); - Task task4Set6 = new Task(31,startPoseGoal4Set6,goalPoseRobot4Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set6 = new Pose(21.0,33.0,0.0); - Pose goalPoseRobot5Set6 = new Pose(43.5,15.6,0.0); - Task task5Set6 = new Task(32,startPoseGoal5Set6,goalPoseRobot5Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set6 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot6Set6 = new Pose(43.5,12.6,0.0); - Task task6Set6 = new Task(33,startPoseGoal6Set6,goalPoseRobot6Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set6 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot7Set6 = new Pose(3.0,26.0,0.0); - Task task7Set6 = new Task(34,startPoseGoal7Set6,goalPoseRobot7Set6,1); - ///////////////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////////////// - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set10 = new Pose(22.0,33.0,0.0); - Pose goalPoseRobot1Set10 = new Pose(43.5,27.6,0.0); - Task task1Set10 = new Task(35,startPoseGoal1Set10,goalPoseRobot1Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set10 = new Pose(24.0,33.0,0.0); - Pose goalPoseRobot2Set10 = new Pose(43.5,24.6,0.0); - Task task2Set10 = new Task(36,startPoseGoal2Set10,goalPoseRobot2Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set10 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot3Set10 = new Pose(43.5,21.6,0.0); - Task task3Set10 = new Task(37,startPoseGoal3Set10,goalPoseRobot3Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot4Set10 = new Pose(43.5,18.6,0.0); - Task task4Set10 = new Task(38,startPoseGoal4Set10,goalPoseRobot4Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set10 = new Pose(18.0,32.0,0.0); - Pose goalPoseRobot5Set10 = new Pose(43.5,15.6,0.0); - Task task5Set10 = new Task(39,startPoseGoal5Set10,goalPoseRobot5Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot6Set10 = new Pose(43.5,12.6,0.0); - Task task6Set10 = new Task(40,startPoseGoal6Set10,goalPoseRobot6Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set10 = new Pose(16.0,32.0,0.0); - Pose goalPoseRobot7Set10 = new Pose(43.5,9.6,0.0); - Task task7Set10 = new Task(41,startPoseGoal7Set10,goalPoseRobot7Set10,1); - - - - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set9 = new Pose(29.0,4.0,Math.PI); - Pose goalPoseRobot1Set9 = new Pose(33.0,4.0,Math.PI); - Task task1Set9 = new Task(42,startPoseGoal1Set9,goalPoseRobot1Set9,1); - //////////////////////////////////////////////////////////// - Pose startPoseGoal2Set9 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set9 = new Pose(3.0,28.0,0.0); - Task task2Set9 = new Task(43,startPoseGoal2Set9,goalPoseRobot2Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal3Set9 = new Pose(6.0,20.0,0.0); - Pose goalPoseRobot3Set9 = new Pose(3.0,20.0,0.0); - Task task3Set9 = new Task(44,startPoseGoal3Set9,goalPoseRobot3Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal4Set9 = new Pose(7.0,25.0,0.0); - Pose goalPoseRobot4Set9 = new Pose(3.0,25.0,0.0); - Task task4Set9 = new Task(45,startPoseGoal4Set9,goalPoseRobot4Set9,1); - ////////////////////////////////////////////////////////////////// - ////////////////////////GIVE AGAIN THE SAME TASKS TO HAVE 75 TOTAL TASKS/////////////////////////////////////////// - ////// - //SET 11 - Pose startPoseGoal1Set11 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot1Set11 = new Pose(3.0,26.0,0.0); - Task task1Set11 = new Task(46,startPoseGoal1Set11,goalPoseRobot1Set11,1); - /////////////////// - Pose startPoseGoal2Set11 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot2Set11 = new Pose(24.0,11.0,0.0); - Task task2Set11 = new Task(47,startPoseGoal2Set11,goalPoseRobot2Set11,1); - //////////////////////////////////////////// - - Pose startPoseGoal3Set11 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot3Set11 = new Pose(14.5,28.0,Math.PI/2); - Task task3Set11 = new Task(48,startPoseGoal3Set11,goalPoseRobot3Set11,1); - /////////////////// - Pose startPoseGoal4Set11 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot4Set11 = new Pose(33.5,4.0,0.0); - Task task4Set11 = new Task(49,startPoseGoal4Set11,goalPoseRobot4Set11,1); - /////////////////// - Pose startPoseGoal5Set11 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot5Set11 = new Pose(33.7,2.0,0.0); - Task task5Set11 = new Task(50,startPoseGoal5Set11,goalPoseRobot5Set11,1); - //////////////////////////////////////// - /////////////////////////////////////// - //SET 12 - /////////////////// - Pose startPoseGoal1Set12 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set12 = new Pose(2.8,6.5,Math.PI); - Task task1Set12 = new Task(51,startPoseGoal1Set12,goalPoseRobot1Set12,1); - ////////////////////////////////////////////// - Pose startPoseGoal2Set12 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot2Set12 = new Pose(43.5,30.6,0.0); - Task task2Set12 = new Task(52,startPoseGoal2Set12,goalPoseRobot2Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set12 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot3Set12 = new Pose(2.4,3.0,Math.PI); - Task task3Set12 = new Task(53,startPoseGoal3Set12,goalPoseRobot3Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set12 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot4Set12 = new Pose(17.0,2.8,-Math.PI/2); - Task task4Set12 = new Task(54,startPoseGoal4Set12,goalPoseRobot4Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set12 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot5Set12 = new Pose(3.0,28.0,0.0); - Task task5Set12 = new Task(55,startPoseGoal5Set12,goalPoseRobot5Set12,1); - //////////////////////////////////////////// - /////////////////////////////////////////// - //SET 13 - Pose startPoseGoal1Set13 = new Pose(23.5,33.5,0.0); - Pose goalPoseRobot1Set13 = new Pose(43.5,33.6,0.0); - Task task1Set13 = new Task(56,startPoseGoal1Set13,goalPoseRobot1Set13,1); - ////////////////////////////////////////////// - Pose startPoseGoal2Set13 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot2Set13 = new Pose(25.0,15.0,0.0); - Task task2Set13 = new Task(57,startPoseGoal2Set13,goalPoseRobot2Set13,1); - ////////////////////////////////////// - Pose startPoseGoal3Set13 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot3Set13 = new Pose(20.0,24.5,0.0); - Task task3Set13 = new Task(58,startPoseGoal3Set13,goalPoseRobot3Set13,1); - ////////////////////////////////////// - Pose startPoseGoal4Set13 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot4Set13 = new Pose(24.0,11.0,0.0); - Task task4Set13 = new Task(59,startPoseGoal4Set13,goalPoseRobot4Set13,1); - ////////////////////////////////////// - Pose startPoseGoal5Set13 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot5Set13 = new Pose(6.0,33.5,Math.PI/2); - Task task5Set13 = new Task(60,startPoseGoal5Set13,goalPoseRobot5Set13,1); - ////////////////////////////////////// - ///////////////////////////////////////////////////////// - //SET 14 - Pose startPoseGoal1Set14 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set14 = new Pose(6.0,10.0,Math.PI); - Task task1Set14 = new Task(61,startPoseGoal1Set14,goalPoseRobot1Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set14 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set14 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set14 = new Task(62,startPoseGoal2Set14,goalPoseRobot2Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set14 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set14 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set14 = new Task(63,startPoseGoal3Set14,goalPoseRobot3Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set14 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set14 = new Pose(23.0,19.0,0.0); - Task task4Set14 = new Task(64,startPoseGoal4Set14,goalPoseRobot4Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set14 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set14 = new Pose(33.5,7.5,0.0); - Task task5Set14 = new Task(65,startPoseGoal5Set14,goalPoseRobot5Set14,1); - //////////////////////////////////////////////////// - /////////////////////////////////////////////////// - //SET 15 - Pose startPoseGoal1Set15 = new Pose(20.0,4.0,0.0); - Pose goalPoseRobot1Set15 = new Pose(25.5,3.0,Math.PI/2); - Task task1Set15 = new Task(66,startPoseGoal1Set15,goalPoseRobot1Set15,1); - /////////////////////////////////////// - Pose startPoseGoal2Set15 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set15 = new Pose(3.0,33.5,Math.PI/2); - Task task2Set15 = new Task(67,startPoseGoal2Set15,goalPoseRobot2Set15,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set15 = new Pose(43.5,27.6,0.0); - Task task3Set15 = new Task(68,startPoseGoal3Set15,goalPoseRobot3Set15,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot4Set15 = new Pose(43.5,24.6,0.0); - Task task4Set15 = new Task(69,startPoseGoal4Set15,goalPoseRobot4Set15,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot5Set15 = new Pose(43.5,21.6,0.0); - Task task5Set15 = new Task(70,startPoseGoal5Set15,goalPoseRobot5Set15,1); - ///////////////////////////////////////////////////////////////////// - //////////////////////////// - //SET 16 - - Pose startPoseGoal1Set16 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot1Set16 = new Pose(43.5,18.6,0.0); - Task task1Set16 = new Task(71,startPoseGoal1Set16,goalPoseRobot1Set16,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set16 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot2Set16 = new Pose(43.5,15.6,0.0); - Task task2Set16 = new Task(72,startPoseGoal2Set16,goalPoseRobot2Set16,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set16 = new Pose(8.0,5.8,Math.PI/2); - Pose goalPoseRobot3Set16 = new Pose(8.0,2.8,Math.PI/2); - Task task3Set16 = new Task(73,startPoseGoal3Set16,goalPoseRobot3Set16,1); - //////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set16 = new Pose(11.0,5.8,Math.PI/2); - Pose goalPoseRobot4Set16 = new Pose(11.0,2.8,Math.PI/2); - Task task4Set16 = new Task(74,startPoseGoal4Set16,goalPoseRobot4Set16,1); - /////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set16 = new Pose(18.0,7.8,Math.PI/2); - Pose goalPoseRobot5Set16 = new Pose(20.0,2.5,Math.PI/2); - Task task5Set16 = new Task(75,startPoseGoal5Set16,goalPoseRobot5Set16,1); - - - - - - - - - - - - //TEST ZONE - Pose startPoseGoal1New = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1New = new Pose(2.0,5.0,Math.PI); - - Robot robot1New = new Robot(8, 1,tec.getDefaultFootprint() ); - Robot robot2New = new Robot(9,2,tec.getDefaultFootprint() ); - Robot robot3New = new Robot(10,1,tec.getDefaultFootprint() ); - Robot robot4New = new Robot(11,1,tec.getDefaultFootprint()); - Robot robot5New = new Robot(12,1,tec.getDefaultFootprint()); - Robot robot6New = new Robot(13,2,fp2); - - //tec.addRobot(robot1New,startPoseGoal1Set3); - //tec.addRobot(robot2New,goalPoseRobot1Set3); - //tec.addRobot(robot3New,startPoseGoal1Set7); - //tec.addRobot(robot4New, startPoseGoal7Set5); - //tec.addRobot(robot5New, startPoseGoal2Set3); - //tec.addRobot(robot6New, goalPoseRobot2Set3); - /////// - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(55000); //sys alpha != 1 synchronous real - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(450000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - //Add the second set of tasks - assignmentProblem.addTask(task6); - assignmentProblem.addTask(task1Set10); - assignmentProblem.addTask(task1Set2); - assignmentProblem.addTask(task2Set2); - assignmentProblem.addTask(task3Set2); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario2"); - double [][][]optimalAllocation2 = {{{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation2); - - - ///Sleep for a while - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous real - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(100000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set3); - assignmentProblem.addTask(task2Set3); - assignmentProblem.addTask(task3Set3); - assignmentProblem.addTask(task4Set3); - assignmentProblem.addTask(task5Set3); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario3"); - double [][][]optimalAllocation3 = {{{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation3); - - - //Thread.sleep(150000); //sys synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous real - Thread.sleep(150000); //sys alpha != 1 synchronous nominal - //Thread.sleep(250000); - //Thread.sleep(500000); - //Thread.sleep(75000); - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task4Set2); - assignmentProblem.addTask(task1Set6); - assignmentProblem.addTask(task2Set6); - assignmentProblem.addTask(task3Set6); - assignmentProblem.addTask(task4Set6); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario4"); - double [][][]optimalAllocation4 = {{{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation4); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(600000); //sys alpha _=! 1 - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task5Set6); - assignmentProblem.addTask(task6Set6); - assignmentProblem.addTask(task7Set6); - assignmentProblem.addTask(task7); - assignmentProblem.addTask(task1Set4); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario5"); - double [][][]optimalAllocation5 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation5); - - //Thread.sleep(300000); - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(500000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task2Set4); - assignmentProblem.addTask(task3Set4); - assignmentProblem.addTask(task4Set4); - assignmentProblem.addTask(task1Set5); - assignmentProblem.addTask(task2Set5); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario6"); - double [][][]optimalAllocation6 = {{{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation6); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(500000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - assignmentProblem.addTask(task3Set5); - assignmentProblem.addTask(task4Set5); - assignmentProblem.addTask(task5Set5); - assignmentProblem.addTask(task6Set5); - assignmentProblem.addTask(task7Set5); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario7"); - double [][][]optimalAllocation7 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation7); - - - //Thread.sleep(150000);//sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task2Set10); - assignmentProblem.addTask(task3Set10); - assignmentProblem.addTask(task4Set10); - assignmentProblem.addTask(task5Set10); - assignmentProblem.addTask(task6Set10); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario8"); - double [][][]optimalAllocation8 = {{{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation8); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task7Set10); - assignmentProblem.addTask(task1Set9); - assignmentProblem.addTask(task2Set9); - assignmentProblem.addTask(task3Set9); - assignmentProblem.addTask(task4Set9); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario9"); - double [][][]optimalAllocation9 = {{{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation9); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - assignmentProblem.addTask(task1Set11); - assignmentProblem.addTask(task2Set11); - assignmentProblem.addTask(task3Set11); - assignmentProblem.addTask(task4Set11); - assignmentProblem.addTask(task5Set11); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario10"); - double [][][]optimalAllocation10 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation10); - - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set12); - assignmentProblem.addTask(task2Set12); - assignmentProblem.addTask(task3Set12); - assignmentProblem.addTask(task4Set12); - assignmentProblem.addTask(task5Set12); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario11"); - double [][][]optimalAllocation11 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation11); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set13); - assignmentProblem.addTask(task2Set13); - assignmentProblem.addTask(task3Set13); - assignmentProblem.addTask(task4Set13); - assignmentProblem.addTask(task5Set13); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario12"); - double [][][]optimalAllocation12 = {{{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation12); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task1Set14); - assignmentProblem.addTask(task2Set14); - assignmentProblem.addTask(task3Set14); - assignmentProblem.addTask(task4Set14); - assignmentProblem.addTask(task5Set14); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario13"); - double [][][]optimalAllocation13 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation13); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set15); - assignmentProblem.addTask(task2Set15); - assignmentProblem.addTask(task3Set15); - assignmentProblem.addTask(task4Set15); - assignmentProblem.addTask(task5Set15); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario14"); - double [][][]optimalAllocation14 = {{{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation14); - - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task1Set16); - assignmentProblem.addTask(task2Set16); - assignmentProblem.addTask(task3Set16); - assignmentProblem.addTask(task4Set16); - assignmentProblem.addTask(task5Set16); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario15"); - double [][][]optimalAllocation15 = {{{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation15); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseWithPropagation.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseWithPropagation.java deleted file mode 100644 index 0232de1..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentOrebroWarehouseWithPropagation.java +++ /dev/null @@ -1,955 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Random; -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; - -import com.vividsolutions.jts.geom.Coordinate; - -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.NetworkConfiguration; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner.PLANNING_ALGORITHM; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.Missions; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; -import com.google.ortools.linearsolver.*; - - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentOrebroWarehouseWithPropagation { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - - //Maximum acceleration/deceleration and speed for all robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - - //Create a coordinator with interfaces to robots - //in the built-in 2D simulator - //final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Define a network with uncertainties (see Mannucci et al., 2019) - NetworkConfiguration.setDelays(0, 0); - NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS = 0.0; - - //Tell the coordinator - // (1) what is known about the communication channel, and - // (2) the accepted probability of constraint violation - tec.setNetworkParameters(NetworkConfiguration.PROBABILITY_OF_PACKET_LOSS, NetworkConfiguration.getMaximumTxDelay(), 0.01); - - //Avoid deadlocks via global re-ordering - tec.setBreakDeadlocks(true, false, false); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, true); - - //Set up infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - - //Start the thread that revises precedences at every period - tec.startInference(); - - //set the map yalm file - String yamlFile = new String("maps/basement.yaml"); - - //Start a visualization (will open a new browser tab) - BrowserVisualization viz = new BrowserVisualization(); - viz.setMap(yamlFile); - viz.setInitialTransform(13, 6.1, 6.8); - tec.setVisualization(viz); - tec.setBreakDeadlocks(true, false, false); - //Robot IDs can be non-sequential (but must be unique) - double xl = 1.0; - double yl = .5; - Coordinate footprint1 = new Coordinate(-xl,yl); - Coordinate footprint2 = new Coordinate(xl,yl); - Coordinate footprint3 = new Coordinate(xl,-yl); - Coordinate footprint4 = new Coordinate(-xl,-yl); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - - Coordinate[] fp2 = new Coordinate[] { - new Coordinate(-0.7,0.7), - new Coordinate(0.7,0.7), - new Coordinate(0.7,-0.7), - new Coordinate(-0.7,-0.7), - }; - - Robot robot1 = new Robot(1,1,tec.getDefaultFootprint()); - Robot robot2 = new Robot(2,1,tec.getDefaultFootprint()); - Robot robot3 = new Robot(3,1,tec.getDefaultFootprint()); - Robot robot4 = new Robot(4,1,tec.getDefaultFootprint()); - Robot robot5 = new Robot(5,1,tec.getDefaultFootprint()); - Robot robot6 = new Robot(6,1,tec.getDefaultFootprint()); - Robot robot7 = new Robot(7,1,tec.getDefaultFootprint()); - - //tec.setFootprint(2, fp2); - //tec.setFootprint(6, fp2); - //Define start and goal poses for each robot - Pose startPoseRobot1 = new Pose(33.0,5.0,Math.PI); - Pose startPoseRobot2 = new Pose(3.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(3.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(3.0,25.0,0.0); - Pose startPoseRobot5 = new Pose(8.0,2.8,Math.PI/2); - Pose startPoseRobot6 = new Pose(11.0,2.8,Math.PI/2); - Pose startPoseRobot7 = new Pose(20.0,2.8,Math.PI/2); - - - - - Pose startPoseGoal1 = new Pose(7.0,7.0,Math.PI/2); - Pose startPoseGoal2 = new Pose(13.0,20.0,0.0); - Pose startPoseGoal3 = new Pose(13.0,24.25,0.0); - Pose startPoseGoal4 = new Pose(25.0,5.0,0.0); - Pose startPoseGoal5 = new Pose(18.0,15.0,0.0); - Pose startPoseGoal6 = new Pose(11.0,5.0,Math.PI/2); - Pose startPoseGoal7 = new Pose(20.0,11.0,0.0); - - Pose goalPoseRobot1 = new Pose(6.0,15.5,Math.PI/2); - Pose goalPoseRobot2 = new Pose(3.0,23.0,0.0); - Pose goalPoseRobot3 = new Pose(20.0,24.25,0.0); - Pose goalPoseRobot4 = new Pose(30.5,7.0,Math.PI/2); - Pose goalPoseRobot5 = new Pose(25.0,15.0,0.0); - Pose goalPoseRobot6 = new Pose(3.0,26.0,0.0); - Pose goalPoseRobot7 = new Pose(24.0,11.0,0.0); - - - /////////////////////////////////////////////// - - //Set the default motion planner - ReedsSheppCarPlanner rsp1 = new ReedsSheppCarPlanner(PLANNING_ALGORITHM.RRTstar); - rsp1.setRadius(0.1); - rsp1.setTurningRadius(4.0); - rsp1.setFootprint(tec.getDefaultFootprint()); - rsp1.setDistanceBetweenPathPoints(0.5); - rsp1.setMap(yamlFile); - rsp1.setPlanningTimeInSecs(10); - - tec.addRobot(robot1, startPoseRobot1); - tec.addRobot(robot2, startPoseRobot2); - tec.addRobot(robot3, startPoseRobot3); - tec.addRobot(robot4, startPoseRobot4); - tec.addRobot(robot5, startPoseRobot5); - tec.addRobot(robot6, startPoseRobot6); - tec.addRobot(robot7, startPoseRobot7); - - //Primo set di tasks - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - Task task6 = new Task(6,startPoseGoal6,goalPoseRobot6,1); - Task task7 = new Task(7,startPoseGoal7,goalPoseRobot7,1); - - - - - - int a = 1; - //FOLDER NUMBER - int b = 2; - tec.setTestNumber(a); - tec.setFolderNumber(b); - - TaskAssignment assignmentProblem = new TaskAssignment(); - - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - assignmentProblem.setFleetVisualization(viz); - int numPaths = 1; - - for (int robotID : tec.getIdleRobots()) { - - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = (ReedsSheppCarPlanner) rsp1.getCopy(false); - tec.setMotionPlanner(robotID, rsp); - } - - - ///////////////////////// ////////////////////////////// - //Solve the problem to find some feasible solution - assignmentProblem.setTimeOutinMin(6.5); - double alpha = 0.5; - double [][][]optimalAllocation = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - }; - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario1"); - assignmentProblem.LoadScenarioAllocation(optimalAllocation); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - //tec.setFakeCoordinator(true); - //assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - - assignmentProblem.startTaskAssignment(tec); - //assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - //assignmentProblem.startTaskAssignmentLocalSearchAlgorithm(tec, -1); - - - //New tasks real - //SET 2 - //Second set of tasks - //-> 4 more tasks ( 3 of type 1 and 1 of type 2) - Pose startPoseGoal1Set2 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot1Set2 = new Pose(15.0,28.0,Math.PI/2); - Task task1Set2 = new Task(8,startPoseGoal1Set2,goalPoseRobot1Set2,1); - /////////////////// - Pose startPoseGoal2Set2 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot2Set2 = new Pose(33.5,4.0,0.0); - Task task2Set2 = new Task(9,startPoseGoal2Set2,goalPoseRobot2Set2,1); - /////////////////// - Pose startPoseGoal3Set2 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot3Set2 = new Pose(33.7,2.0,0.0); - Task task3Set2 = new Task(10,startPoseGoal3Set2,goalPoseRobot3Set2,1); - /////////////////// - Pose startPoseGoal4Set2 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot4Set2 = new Pose(2.8,6.5,Math.PI); - Task task4Set2 = new Task(11,startPoseGoal4Set2,goalPoseRobot4Set2,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - //Third set of tasks - //-> 5 more tasks (4 of type 1 and 1 of type 2) - Pose startPoseGoal1Set3 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot1Set3 = new Pose(43.5,30.6,0.0); - Task task1Set3 = new Task(12,startPoseGoal1Set3,goalPoseRobot1Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal2Set3 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot2Set3 = new Pose(2.4,3.0,Math.PI); - Task task2Set3 = new Task(13,startPoseGoal2Set3,goalPoseRobot2Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set3 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot3Set3 = new Pose(17.0,2.8,-Math.PI/2); - Task task3Set3 = new Task(14,startPoseGoal3Set3,goalPoseRobot3Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set3 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot4Set3 = new Pose(3.0,28.0,0.0); - Task task4Set3 = new Task(15,startPoseGoal4Set3,goalPoseRobot4Set3,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set3 = new Pose(23.5,33.5,0.0); - Pose goalPoseRobot5Set3 = new Pose(43.5,33.6,0.0); - Task task5Set3 = new Task(16,startPoseGoal5Set3,goalPoseRobot5Set3,1); - ////////////////////////////////////////////// - ////////////////////////////////////////////// - - - - - - - //Forth set of tasks - //-> 4 more tasks (4 of type 1 and 0 of type 2) - Pose startPoseGoal1Set4 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot1Set4 = new Pose(25.0,15.0,0.0); - Task task1Set4 = new Task(17,startPoseGoal1Set4,goalPoseRobot1Set4,1); - ////////////////////////////////////// - Pose startPoseGoal2Set4 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot2Set4 = new Pose(20.0,24.5,0.0); - Task task2Set4 = new Task(18,startPoseGoal2Set4,goalPoseRobot2Set4,1); - ////////////////////////////////////// - Pose startPoseGoal3Set4 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot3Set4 = new Pose(24.0,11.0,0.0); - Task task3Set4 = new Task(19,startPoseGoal3Set4,goalPoseRobot3Set4,1); - ////////////////////////////////////// - Pose startPoseGoal4Set4 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot4Set4 = new Pose(6.0,33.5,Math.PI/2); - Task task4Set4 = new Task(20,startPoseGoal4Set4,goalPoseRobot4Set4,1); - ////////////////////////////////////// - //Fifth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set5 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set5 = new Pose(6.0,10.0,Math.PI); - Task task1Set5 = new Task(21,startPoseGoal1Set5,goalPoseRobot1Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set5 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set5 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set5 = new Task(22,startPoseGoal2Set5,goalPoseRobot2Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set5 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set5 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set5 = new Task(23,startPoseGoal3Set5,goalPoseRobot3Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set5 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set5 = new Pose(23.0,19.0,0.0); - Task task4Set5 = new Task(24,startPoseGoal4Set5,goalPoseRobot4Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set5 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set5 = new Pose(33.5,7.5,0.0); - Task task5Set5 = new Task(25,startPoseGoal5Set5,goalPoseRobot5Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal6Set5 = new Pose(20.0,4.0,0.0); - Pose goalPoseRobot6Set5 = new Pose(25.5,3.0,Math.PI/2); - Task task6Set5 = new Task(26,startPoseGoal6Set5,goalPoseRobot6Set5,1); - //////////////////////////////////////////////////// - Pose startPoseGoal7Set5 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot7Set5 = new Pose(3.0,33.5,Math.PI/2); - Task task7Set5 = new Task(27,startPoseGoal7Set5,goalPoseRobot7Set5,1); - //////////////////////////////////////////////////// - ////////////////////////////////////// - //Sixth set of tasks - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set6 = new Pose(18.0,33.0,0.0); - Pose goalPoseRobot1Set6 = new Pose(43.5,27.6,0.0); - Task task1Set6 = new Task(28,startPoseGoal1Set6,goalPoseRobot1Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set6 = new Pose(19.0,33.0,0.0); - Pose goalPoseRobot2Set6 = new Pose(43.5,24.6,0.0); - Task task2Set6 = new Task(29,startPoseGoal2Set6,goalPoseRobot2Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set6 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set6 = new Pose(43.5,21.6,0.0); - Task task3Set6 = new Task(30,startPoseGoal3Set6,goalPoseRobot3Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set6 = new Pose(22.0,33.0,0.0); - Pose goalPoseRobot4Set6 = new Pose(43.5,18.6,0.0); - Task task4Set6 = new Task(31,startPoseGoal4Set6,goalPoseRobot4Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set6 = new Pose(21.0,33.0,0.0); - Pose goalPoseRobot5Set6 = new Pose(43.5,15.6,0.0); - Task task5Set6 = new Task(32,startPoseGoal5Set6,goalPoseRobot5Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set6 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot6Set6 = new Pose(43.5,12.6,0.0); - Task task6Set6 = new Task(33,startPoseGoal6Set6,goalPoseRobot6Set6,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set6 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot7Set6 = new Pose(3.0,26.0,0.0); - Task task7Set6 = new Task(34,startPoseGoal7Set6,goalPoseRobot7Set6,1); - ///////////////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////////////// - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set10 = new Pose(22.0,33.0,0.0); - Pose goalPoseRobot1Set10 = new Pose(43.5,27.6,0.0); - Task task1Set10 = new Task(35,startPoseGoal1Set10,goalPoseRobot1Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set10 = new Pose(24.0,33.0,0.0); - Pose goalPoseRobot2Set10 = new Pose(43.5,24.6,0.0); - Task task2Set10 = new Task(36,startPoseGoal2Set10,goalPoseRobot2Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set10 = new Pose(23.0,33.0,0.0); - Pose goalPoseRobot3Set10 = new Pose(43.5,21.6,0.0); - Task task3Set10 = new Task(37,startPoseGoal3Set10,goalPoseRobot3Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot4Set10 = new Pose(43.5,18.6,0.0); - Task task4Set10 = new Task(38,startPoseGoal4Set10,goalPoseRobot4Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set10 = new Pose(18.0,32.0,0.0); - Pose goalPoseRobot5Set10 = new Pose(43.5,15.6,0.0); - Task task5Set10 = new Task(39,startPoseGoal5Set10,goalPoseRobot5Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal6Set10 = new Pose(20.0,32.0,0.0); - Pose goalPoseRobot6Set10 = new Pose(43.5,12.6,0.0); - Task task6Set10 = new Task(40,startPoseGoal6Set10,goalPoseRobot6Set10,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal7Set10 = new Pose(16.0,32.0,0.0); - Pose goalPoseRobot7Set10 = new Pose(43.5,9.6,0.0); - Task task7Set10 = new Task(41,startPoseGoal7Set10,goalPoseRobot7Set10,1); - - - - //-> 7 more tasks (5 of type 1 and 2 of type 2) - Pose startPoseGoal1Set9 = new Pose(29.0,4.0,Math.PI); - Pose goalPoseRobot1Set9 = new Pose(33.0,4.0,Math.PI); - Task task1Set9 = new Task(42,startPoseGoal1Set9,goalPoseRobot1Set9,1); - //////////////////////////////////////////////////////////// - Pose startPoseGoal2Set9 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set9 = new Pose(3.0,28.0,0.0); - Task task2Set9 = new Task(43,startPoseGoal2Set9,goalPoseRobot2Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal3Set9 = new Pose(6.0,20.0,0.0); - Pose goalPoseRobot3Set9 = new Pose(3.0,20.0,0.0); - Task task3Set9 = new Task(44,startPoseGoal3Set9,goalPoseRobot3Set9,1); - /////////////////////////////////////////////////// - Pose startPoseGoal4Set9 = new Pose(7.0,25.0,0.0); - Pose goalPoseRobot4Set9 = new Pose(3.0,25.0,0.0); - Task task4Set9 = new Task(45,startPoseGoal4Set9,goalPoseRobot4Set9,1); - ////////////////////////////////////////////////////////////////// - ////////////////////////GIVE AGAIN THE SAME TASKS TO HAVE 75 TOTAL TASKS/////////////////////////////////////////// - ////// - //SET 11 - Pose startPoseGoal1Set11 = new Pose(11.0,5.0,Math.PI/2); - Pose goalPoseRobot1Set11 = new Pose(3.0,26.0,0.0); - Task task1Set11 = new Task(46,startPoseGoal1Set11,goalPoseRobot1Set11,1); - /////////////////// - Pose startPoseGoal2Set11 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot2Set11 = new Pose(24.0,11.0,0.0); - Task task2Set11 = new Task(47,startPoseGoal2Set11,goalPoseRobot2Set11,1); - //////////////////////////////////////////// - - Pose startPoseGoal3Set11 = new Pose(9.0,24.0,0.0); - Pose goalPoseRobot3Set11 = new Pose(14.5,28.0,Math.PI/2); - Task task3Set11 = new Task(48,startPoseGoal3Set11,goalPoseRobot3Set11,1); - /////////////////// - Pose startPoseGoal4Set11 = new Pose(22.0,5.0,0.0); - Pose goalPoseRobot4Set11 = new Pose(33.5,4.0,0.0); - Task task4Set11 = new Task(49,startPoseGoal4Set11,goalPoseRobot4Set11,1); - /////////////////// - Pose startPoseGoal5Set11 = new Pose(22.0,3.0,0.0); - Pose goalPoseRobot5Set11 = new Pose(33.7,2.0,0.0); - Task task5Set11 = new Task(50,startPoseGoal5Set11,goalPoseRobot5Set11,1); - //////////////////////////////////////// - /////////////////////////////////////// - //SET 12 - /////////////////// - Pose startPoseGoal1Set12 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set12 = new Pose(2.8,6.5,Math.PI); - Task task1Set12 = new Task(51,startPoseGoal1Set12,goalPoseRobot1Set12,1); - ////////////////////////////////////////////// - Pose startPoseGoal2Set12 = new Pose(9.5,33.5,0.0); - Pose goalPoseRobot2Set12 = new Pose(43.5,30.6,0.0); - Task task2Set12 = new Task(52,startPoseGoal2Set12,goalPoseRobot2Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal3Set12 = new Pose(11.0,12.0,-Math.PI/2); - Pose goalPoseRobot3Set12 = new Pose(2.4,3.0,Math.PI); - Task task3Set12 = new Task(53,startPoseGoal3Set12,goalPoseRobot3Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal4Set12 = new Pose(18.0,8.0,-Math.PI/2); - Pose goalPoseRobot4Set12 = new Pose(17.0,2.8,-Math.PI/2); - Task task4Set12 = new Task(54,startPoseGoal4Set12,goalPoseRobot4Set12,1); - //////////////////////////////////////////// - Pose startPoseGoal5Set12 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot5Set12 = new Pose(3.0,28.0,0.0); - Task task5Set12 = new Task(55,startPoseGoal5Set12,goalPoseRobot5Set12,1); - //////////////////////////////////////////// - /////////////////////////////////////////// - //SET 13 - Pose startPoseGoal1Set13 = new Pose(23.5,33.5,0.0); - Pose goalPoseRobot1Set13 = new Pose(43.5,33.6,0.0); - Task task1Set13 = new Task(56,startPoseGoal1Set13,goalPoseRobot1Set13,1); - ////////////////////////////////////////////// - Pose startPoseGoal2Set13 = new Pose(18.0,15.0,0.0); - Pose goalPoseRobot2Set13 = new Pose(25.0,15.0,0.0); - Task task2Set13 = new Task(57,startPoseGoal2Set13,goalPoseRobot2Set13,1); - ////////////////////////////////////// - Pose startPoseGoal3Set13 = new Pose(15.0,24.0,0.0); - Pose goalPoseRobot3Set13 = new Pose(20.0,24.5,0.0); - Task task3Set13 = new Task(58,startPoseGoal3Set13,goalPoseRobot3Set13,1); - ////////////////////////////////////// - Pose startPoseGoal4Set13 = new Pose(20.0,11.0,0.0); - Pose goalPoseRobot4Set13 = new Pose(24.0,11.0,0.0); - Task task4Set13 = new Task(59,startPoseGoal4Set13,goalPoseRobot4Set13,1); - ////////////////////////////////////// - Pose startPoseGoal5Set13 = new Pose(9.0,22.0,Math.PI/2); - Pose goalPoseRobot5Set13 = new Pose(6.0,33.5,Math.PI/2); - Task task5Set13 = new Task(60,startPoseGoal5Set13,goalPoseRobot5Set13,1); - ////////////////////////////////////// - ///////////////////////////////////////////////////////// - //SET 14 - Pose startPoseGoal1Set14 = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1Set14 = new Pose(6.0,10.0,Math.PI); - Task task1Set14 = new Task(61,startPoseGoal1Set14,goalPoseRobot1Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal2Set14 = new Pose(2.0,16.0,-Math.PI/2); - Pose goalPoseRobot2Set14 = new Pose(2.0,11.0,-Math.PI/2); - Task task2Set14 = new Task(62,startPoseGoal2Set14,goalPoseRobot2Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set14 = new Pose(9.0,23.0,Math.PI); - Pose goalPoseRobot3Set14 = new Pose(5.0,20.0,-Math.PI/2); - Task task3Set14 = new Task(63,startPoseGoal3Set14,goalPoseRobot3Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal4Set14 = new Pose(18.0,19.0,0.0); - Pose goalPoseRobot4Set14 = new Pose(23.0,19.0,0.0); - Task task4Set14 = new Task(64,startPoseGoal4Set14,goalPoseRobot4Set14,1); - //////////////////////////////////////////////////// - Pose startPoseGoal5Set14 = new Pose(20.0,5.0,0.0); - Pose goalPoseRobot5Set14 = new Pose(33.5,7.5,0.0); - Task task5Set14 = new Task(65,startPoseGoal5Set14,goalPoseRobot5Set14,1); - //////////////////////////////////////////////////// - /////////////////////////////////////////////////// - //SET 15 - Pose startPoseGoal1Set15 = new Pose(20.0,4.0,0.0); - Pose goalPoseRobot1Set15 = new Pose(25.5,3.0,Math.PI/2); - Task task1Set15 = new Task(66,startPoseGoal1Set15,goalPoseRobot1Set15,1); - /////////////////////////////////////// - Pose startPoseGoal2Set15 = new Pose(6.0,28.0,0.0); - Pose goalPoseRobot2Set15 = new Pose(3.0,33.5,Math.PI/2); - Task task2Set15 = new Task(67,startPoseGoal2Set15,goalPoseRobot2Set15,1); - //////////////////////////////////////////////////// - Pose startPoseGoal3Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot3Set15 = new Pose(43.5,27.6,0.0); - Task task3Set15 = new Task(68,startPoseGoal3Set15,goalPoseRobot3Set15,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot4Set15 = new Pose(43.5,24.6,0.0); - Task task4Set15 = new Task(69,startPoseGoal4Set15,goalPoseRobot4Set15,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set15 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot5Set15 = new Pose(43.5,21.6,0.0); - Task task5Set15 = new Task(70,startPoseGoal5Set15,goalPoseRobot5Set15,1); - ///////////////////////////////////////////////////////////////////// - //////////////////////////// - //SET 16 - - Pose startPoseGoal1Set16 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot1Set16 = new Pose(43.5,18.6,0.0); - Task task1Set16 = new Task(71,startPoseGoal1Set16,goalPoseRobot1Set16,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal2Set16 = new Pose(20.0,33.0,0.0); - Pose goalPoseRobot2Set16 = new Pose(43.5,15.6,0.0); - Task task2Set16 = new Task(72,startPoseGoal2Set16,goalPoseRobot2Set16,1); - ///////////////////////////////////////////////////////////////////// - Pose startPoseGoal3Set16 = new Pose(8.0,5.8,Math.PI/2); - Pose goalPoseRobot3Set16 = new Pose(8.0,2.8,Math.PI/2); - Task task3Set16 = new Task(73,startPoseGoal3Set16,goalPoseRobot3Set16,1); - //////////////////////////////////////////////////////////////////// - Pose startPoseGoal4Set16 = new Pose(11.0,5.8,Math.PI/2); - Pose goalPoseRobot4Set16 = new Pose(11.0,2.8,Math.PI/2); - Task task4Set16 = new Task(74,startPoseGoal4Set16,goalPoseRobot4Set16,1); - /////////////////////////////////////////////////////////////////// - Pose startPoseGoal5Set16 = new Pose(18.0,7.8,Math.PI/2); - Pose goalPoseRobot5Set16 = new Pose(20.0,2.5,Math.PI/2); - Task task5Set16 = new Task(75,startPoseGoal5Set16,goalPoseRobot5Set16,1); - - - - - - - - - - - - //TEST ZONE - Pose startPoseGoal1New = new Pose(11.0,18.0,-Math.PI/2); - Pose goalPoseRobot1New = new Pose(2.0,5.0,Math.PI); - - Robot robot1New = new Robot(8, 1,tec.getDefaultFootprint() ); - Robot robot2New = new Robot(9,2,tec.getDefaultFootprint() ); - Robot robot3New = new Robot(10,1,tec.getDefaultFootprint() ); - Robot robot4New = new Robot(11,1,tec.getDefaultFootprint()); - Robot robot5New = new Robot(12,1,tec.getDefaultFootprint()); - Robot robot6New = new Robot(13,2,fp2); - - //tec.addRobot(robot1New,startPoseGoal1Set3); - //tec.addRobot(robot2New,goalPoseRobot1Set3); - //tec.addRobot(robot3New,startPoseGoal1Set7); - //tec.addRobot(robot4New, startPoseGoal7Set5); - //tec.addRobot(robot5New, startPoseGoal2Set3); - //tec.addRobot(robot6New, goalPoseRobot2Set3); - /////// - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(55000); //sys alpha != 1 synchronous real - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(450000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - //Add the second set of tasks - assignmentProblem.addTask(task6); - assignmentProblem.addTask(task1Set10); - assignmentProblem.addTask(task1Set2); - assignmentProblem.addTask(task2Set2); - assignmentProblem.addTask(task3Set2); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario2"); - double [][][]optimalAllocation2 = {{{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation2); - - - ///Sleep for a while - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous real - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(100000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set3); - assignmentProblem.addTask(task2Set3); - assignmentProblem.addTask(task3Set3); - assignmentProblem.addTask(task4Set3); - assignmentProblem.addTask(task5Set3); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario3"); - double [][][]optimalAllocation3 = {{{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation3); - - - //Thread.sleep(150000); //sys synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous real - Thread.sleep(150000); //sys alpha != 1 synchronous nominal - //Thread.sleep(250000); - //Thread.sleep(500000); - //Thread.sleep(75000); - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task4Set2); - assignmentProblem.addTask(task1Set6); - assignmentProblem.addTask(task2Set6); - assignmentProblem.addTask(task3Set6); - assignmentProblem.addTask(task4Set6); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario4"); - double [][][]optimalAllocation4 = {{{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation4); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(600000); //sys alpha _=! 1 - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task5Set6); - assignmentProblem.addTask(task6Set6); - assignmentProblem.addTask(task7Set6); - assignmentProblem.addTask(task7); - assignmentProblem.addTask(task1Set4); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario5"); - double [][][]optimalAllocation5 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation5); - - //Thread.sleep(300000); - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(500000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task2Set4); - assignmentProblem.addTask(task3Set4); - assignmentProblem.addTask(task4Set4); - assignmentProblem.addTask(task1Set5); - assignmentProblem.addTask(task2Set5); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario6"); - double [][][]optimalAllocation6 = {{{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation6); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(500000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - assignmentProblem.addTask(task3Set5); - assignmentProblem.addTask(task4Set5); - assignmentProblem.addTask(task5Set5); - assignmentProblem.addTask(task6Set5); - assignmentProblem.addTask(task7Set5); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario7"); - double [][][]optimalAllocation7 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation7); - - - //Thread.sleep(150000);//sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task2Set10); - assignmentProblem.addTask(task3Set10); - assignmentProblem.addTask(task4Set10); - assignmentProblem.addTask(task5Set10); - assignmentProblem.addTask(task6Set10); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario8"); - double [][][]optimalAllocation8 = {{{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation8); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task7Set10); - assignmentProblem.addTask(task1Set9); - assignmentProblem.addTask(task2Set9); - assignmentProblem.addTask(task3Set9); - assignmentProblem.addTask(task4Set9); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario9"); - double [][][]optimalAllocation9 = {{{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation9); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - assignmentProblem.addTask(task1Set11); - assignmentProblem.addTask(task2Set11); - assignmentProblem.addTask(task3Set11); - assignmentProblem.addTask(task4Set11); - assignmentProblem.addTask(task5Set11); - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario10"); - double [][][]optimalAllocation10 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation10); - - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set12); - assignmentProblem.addTask(task2Set12); - assignmentProblem.addTask(task3Set12); - assignmentProblem.addTask(task4Set12); - assignmentProblem.addTask(task5Set12); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario11"); - double [][][]optimalAllocation11 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation11); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set13); - assignmentProblem.addTask(task2Set13); - assignmentProblem.addTask(task3Set13); - assignmentProblem.addTask(task4Set13); - assignmentProblem.addTask(task5Set13); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario12"); - double [][][]optimalAllocation12 = {{{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation12); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task1Set14); - assignmentProblem.addTask(task2Set14); - assignmentProblem.addTask(task3Set14); - assignmentProblem.addTask(task4Set14); - assignmentProblem.addTask(task5Set14); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario13"); - double [][][]optimalAllocation13 = {{{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation13); - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - - assignmentProblem.addTask(task1Set15); - assignmentProblem.addTask(task2Set15); - assignmentProblem.addTask(task3Set15); - assignmentProblem.addTask(task4Set15); - assignmentProblem.addTask(task5Set15); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario14"); - double [][][]optimalAllocation14 = {{{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation14); - - - //Thread.sleep(150000); //sys alpha = 1 synchronous - //Thread.sleep(550000); //sys alpha != 1 synchronous - Thread.sleep(100000); //sys alpha != 1 synchronous nominal - //Thread.sleep(650000); //sys - //Thread.sleep(150000); //greedy - //Thread.sleep(200000); //Local - - - assignmentProblem.addTask(task1Set16); - assignmentProblem.addTask(task2Set16); - assignmentProblem.addTask(task3Set16); - assignmentProblem.addTask(task4Set16); - assignmentProblem.addTask(task5Set16); - - assignmentProblem.LoadScenario("ScenarioOrebroWarehouse/ProvaScenario15"); - double [][][]optimalAllocation15 = {{{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - - }; - assignmentProblem.LoadScenarioAllocation(optimalAllocation15); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGrid.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGrid.java deleted file mode 100644 index 2eb7524..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGrid.java +++ /dev/null @@ -1,255 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; -import java.util.Random; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.Geometry; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimulatedAnnealing; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentRobotsGrid { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - tec.startInference(); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 1.85, 2.80); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - viz.setMap(yamlFile); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ArrayList taskQueue = new ArrayList (); - - Random rand = new Random(); - - //TEST NUMBER - int a = 4; - //FOLDER NUMBER - int b = 1; - tec.setTestNumber(a); - tec.setFolderNumber(b); - - TaskAssignment assignmentProblem = new TaskAssignment(); - - //TaskAssignmentSimulatedAnnealing assignmentProblem = new TaskAssignmentSimulatedAnnealing(); - - - double [][][]optimalAllocation = {{{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0}}, - }; - - //DO NOT USE THE COORDINATOR (in order to evaluate the nominal arrival time) - //tec.setFakeCoordinator(true); - //LOAD Optimal Assignment - //assignmentProblem.LoadScenarioAllocation(optimalAllocation); - - //LOAD ROADMAP - assignmentProblem.LoadScenario("ProvaScenario1"); - int numPaths = 1; - double delta = 0; - for(int i = 1; i<= 6; i++) { - Pose startPoseRobot = new Pose((4.0 + delta),30.0,-Math.PI/2); - //Pose startPoseRobot = new Pose(4.0,(6.0 + delta),0.0); - int robotType = 1; - if (i==2) { - robotType = 2; - } - else if(i==4){ - robotType = 2; - } - else if(i==6){ - robotType = 2; - } - Robot robot = new Robot(i,robotType); - tec.addRobot(robot,startPoseRobot); - Pose startPoseGoal = new Pose(15.0,(6.0 + delta),0.0); - Pose goalPoseRobot = new Pose(30.0 ,(6.0 + delta) ,0.0); - - //int taskType = rand.nextInt(2)+1; - int taskType = 1; - if(i==1) { - taskType = 2; - } - else if(i==4){ - taskType = 2; - } - else if(i==6){ - taskType = 2; - } - Task task = new Task(i,startPoseGoal,goalPoseRobot,taskType); - assignmentProblem.addTask(task); - taskQueue.add(task); - delta += 3.0; - - } - - - PrintStream fileStream = null; - - try { - fileStream = new PrintStream(new File("ProvaTask.txt")); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - - - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - //rsp.setMap("maps/map-empty.yaml"); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - //tec.setFakeCoordinator(true); - //Solve the problem to find some feasible solution - double alpha = 1.0; - double timeOut = 15*1000; - - - - - //Solve the problem to find some feasible solution - - - - tec.setBreakDeadlocks(true, false, true); - - - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - //assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - //assignmentProblem.setTimeOutinMin(timeOut); - - //FOR S AND GD - //MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - //double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblem(solver,tec); - //double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemLocalSearch(tec,-1); - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemGreedyAlgorithm(tec); - //FOR SA - //double [][][] assignmentMatrix = assignmentProblem.simulatedAnnealingAlgorithm(tec,-1); - - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - - - } -} - diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGridExact.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGridExact.java deleted file mode 100644 index bf83082..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGridExact.java +++ /dev/null @@ -1,153 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; -import java.util.Random; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentRobotsGridExact { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - - - Random rand = new Random(); - TaskAssignment assignmentProblem = new TaskAssignment(); - int numPaths = 1; - double delta = 0; - for(int i = 1; i<= 11; i++) { - - Pose startPoseRobot = new Pose(4.0,(6.0 + delta),0.0); - int robotType = rand.nextInt(2)+1; - Robot robot = new Robot(i,1); - tec.addRobot(robot,startPoseRobot); - Pose startPoseGoal = new Pose(15.0,(6.0 + delta),0.0); - Pose goalPoseRobot = new Pose(30.0 ,(6.0 + delta) ,0.0); - int taskType = rand.nextInt(2)+1; - Task task = new Task(i,startPoseGoal,goalPoseRobot,1); - assignmentProblem.addTask(task); - delta += 6.0; - } - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - //Solve the problem to find some feasible solution - double alpha = 0.6; - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.startTaskAssignmentExactAlgorithm(tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGridGreedy.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGridGreedy.java deleted file mode 100644 index 898087f..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsGridGreedy.java +++ /dev/null @@ -1,151 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; -import java.util.Random; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentRobotsGridGreedy { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - - - Random rand = new Random(); - TaskAssignment assignmentProblem = new TaskAssignment(); - int numPaths = 1; - double delta = 0; - for(int i = 1; i<= 11; i++) { - - Pose startPoseRobot = new Pose(4.0,(6.0 + delta),0.0); - int robotType = rand.nextInt(2)+1; - Robot robot = new Robot(i,1); - tec.addRobot(robot,startPoseRobot); - Pose startPoseGoal = new Pose(15.0,(6.0 + delta),0.0); - Pose goalPoseRobot = new Pose(30.0 ,(6.0 + delta) ,0.0); - int taskType = rand.nextInt(2)+1; - Task task = new Task(i,startPoseGoal,goalPoseRobot,1); - assignmentProblem.addTask(task); - delta += 6.0; - } - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - //Solve the problem to find some feasible solution - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCoordinator(tec); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsInLine.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsInLine.java deleted file mode 100644 index 094b126..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsInLine.java +++ /dev/null @@ -1,209 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.ForwardModel; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentRobotsInLine { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - - - - //You can set a footprint that is specific for each robot - Coordinate[] fp1 = new Coordinate[] { - new Coordinate(-1.0,0.5), - new Coordinate(1.0,0.5), - new Coordinate(1.0,-0.5), - new Coordinate(-1.0,-0.5) - }; - Coordinate[] fp2 = new Coordinate[] { - new Coordinate(0.36, 0.0), - new Coordinate(0.18, 0.36), - new Coordinate(-0.18, 0.36), - new Coordinate(-0.36, 0.0), - new Coordinate(-0.18, -0.36), - new Coordinate(0.18, -0.36) - }; - Coordinate[] fp3 = new Coordinate[] { - new Coordinate(-2.0,0.9), - new Coordinate(2.0,0.9), - new Coordinate(2.0,-0.9), - new Coordinate(-2.0,-0.9) - }; - tec.setFootprint(1,fp1); - tec.setFootprint(2,fp2); - tec.setFootprint(3,fp3); - - - - tec.setupSolver(0, 100000000); - tec.startInference(); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20,1.53,10.51); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - - String yamlFile = "maps/map-empty.yaml"; - viz.setMap(yamlFile); - Pose startPoseRobot1 = new Pose(16.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,6.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,6.0,0.0); - - ForwardModel fm3 = new ConstantAccelerationForwardModel(2, 8, tec.getTemporalResolution(), tec.getControlPeriod(), tec.getTrackingPeriod()); - ForwardModel fm2 = new ConstantAccelerationForwardModel(4, 16, tec.getTemporalResolution(), tec.getControlPeriod(), tec.getTrackingPeriod()); - ForwardModel fm1 = new ConstantAccelerationForwardModel(MAX_ACCEL, MAX_VEL, tec.getTemporalResolution(), tec.getControlPeriod(), tec.getTrackingPeriod()); - Robot robot1 = new Robot(1,1,fp3,fm1); - Robot robot2 = new Robot(2,1,fp2,fm2); - Robot robot3 = new Robot(3,1,fp1,fm3); - - - tec.addRobot(robot1, startPoseRobot1); - tec.addRobot(robot2, startPoseRobot2); - tec.addRobot(robot3, startPoseRobot3); - - - /* - Pose startPoseGoal1 = new Pose(20.0,6.0,0.0); - Pose startPoseGoal2 = new Pose(20.0,6.0,0.0); - Pose startPoseGoal3 = new Pose(20.0,6.0,0.0); - Pose goalPoseRobot1 = new Pose(30.0,6.0,0.0); - Pose goalPoseRobot2 = new Pose(26.0,6.0,0.0); - Pose goalPoseRobot3 = new Pose(22.0,6.0,0.0); - */ - - Pose startPoseGoal1 = new Pose(30.0,6.0,0.0); - Pose startPoseGoal2 = new Pose(30.0,6.0,0.0); - Pose startPoseGoal3 = new Pose(30.0,6.0,0.0); - Pose goalPoseRobot1 = new Pose(50.0,6.0,0.0); - Pose goalPoseRobot2 = new Pose(44.0,6.0,0.0); - Pose goalPoseRobot3 = new Pose(38.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - - - - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 1.0; - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(1.0, 0.0, 0.0); - assignmentProblem.startTaskAssignment(tec); - } -} - diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsInLineGreedy.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsInLineGreedy.java deleted file mode 100644 index 997c27c..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentRobotsInLineGreedy.java +++ /dev/null @@ -1,207 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.ForwardModel; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentRobotsInLineGreedy { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - - - //You can set a footprint that is specific for each robot - Coordinate[] fp1 = new Coordinate[] { - new Coordinate(-1.0,0.5), - new Coordinate(1.0,0.5), - new Coordinate(1.0,-0.5), - new Coordinate(-1.0,-0.5) - }; - Coordinate[] fp2 = new Coordinate[] { - new Coordinate(0.36, 0.0), - new Coordinate(0.18, 0.36), - new Coordinate(-0.18, 0.36), - new Coordinate(-0.36, 0.0), - new Coordinate(-0.18, -0.36), - new Coordinate(0.18, -0.36) - }; - Coordinate[] fp3 = new Coordinate[] { - new Coordinate(-2.0,0.9), - new Coordinate(2.0,0.9), - new Coordinate(2.0,-0.9), - new Coordinate(-2.0,-0.9) - }; - tec.setFootprint(1,fp1); - tec.setFootprint(2,fp2); - tec.setFootprint(3,fp3); - - tec.setupSolver(0, 100000000); - tec.startInference(); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20,1.53,10.51); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - - - String yamlFile = "maps/map-empty.yaml"; - viz.setMap(yamlFile); - - - - Pose startPoseRobot1 = new Pose(16.0,6.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,6.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,6.0,0.0); - - - ForwardModel fm1 = new ConstantAccelerationForwardModel(0.5, 2, tec.getTemporalResolution(), tec.getControlPeriod(), tec.getTrackingPeriod()); - ForwardModel fm2 = new ConstantAccelerationForwardModel(2, 8, tec.getTemporalResolution(), tec.getControlPeriod(), tec.getTrackingPeriod()); - ForwardModel fm3 = new ConstantAccelerationForwardModel(MAX_ACCEL, MAX_VEL, tec.getTemporalResolution(), tec.getControlPeriod(), tec.getTrackingPeriod()); - Robot robot1 = new Robot(1,1,fp3,fm1); - Robot robot2 = new Robot(2,1,fp2,fm2); - Robot robot3 = new Robot(3,1,fp1,fm3); - - - tec.addRobot(robot1, startPoseRobot1); - tec.addRobot(robot2, startPoseRobot2); - tec.addRobot(robot3, startPoseRobot3); - - - /* - Pose startPoseGoal1 = new Pose(20.0,6.0,0.0); - Pose startPoseGoal2 = new Pose(20.0,6.0,0.0); - Pose startPoseGoal3 = new Pose(20.0,6.0,0.0); - Pose goalPoseRobot1 = new Pose(30.0,6.0,0.0); - Pose goalPoseRobot2 = new Pose(26.0,6.0,0.0); - Pose goalPoseRobot3 = new Pose(22.0,6.0,0.0); - */ - - Pose startPoseGoal1 = new Pose(30.0,6.0,0.0); - Pose startPoseGoal2 = new Pose(30.0,6.0,0.0); - Pose startPoseGoal3 = new Pose(30.0,6.0,0.0); - Pose goalPoseRobot1 = new Pose(50.0,6.0,0.0); - Pose goalPoseRobot2 = new Pose(44.0,6.0,0.0); - Pose goalPoseRobot3 = new Pose(38.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.7; - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(1.0, 0.0, 0.0); - assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - } -} - diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap.java deleted file mode 100644 index 83dede5..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap.java +++ /dev/null @@ -1,176 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMap { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - viz.setMap("maps/map-corridors-vi.yaml"); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-corridors-vi.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-corridors-vi.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(5); - - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,Math.PI/2); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ); - Robot robot5 = new Robot(5, 1 ); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(24.0,6.0,0.0); - Pose goalPoseRobot1 = new Pose(38.0,8.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(48.0,6.0,0.0); - - - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - - Pose startPoseGoal4 = new Pose(9.0,27.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - Pose goalPoseRobot4 = new Pose(45.0,31.0,0.0); - Pose goalPoseRobot5 = new Pose(42.0,6.0,0.0); - - - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 1.0; - TaskAssignmentSimple assignmentProblem = new TaskAssignmentSimple(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - assignmentProblem.setFleetVisualization(viz); - - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setDefaultMotionPlanner(rsp); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - - tec.setDefaultMotionPlanner(assignmentProblem.getDefaultMotionPlanner()); - - assignmentProblem.startTaskAssignment(tec); - //assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap2.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap2.java deleted file mode 100644 index ebd9a4c..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap2.java +++ /dev/null @@ -1,183 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMap2 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - viz.setMap("maps/map-corridors-vi.yaml"); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-corridors-vi.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-corridors-vi.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(5); - - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,Math.PI/2); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Coordinate[] DEFAULT_FOOTPRINT = new Coordinate[] { - new Coordinate(-1.7, 0.7), //back left - new Coordinate(-1.7, -1.7), //back right - new Coordinate(2.7, -1.7), //front right - new Coordinate(2.7, 0.7)}; - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ,DEFAULT_FOOTPRINT, new ConstantAccelerationForwardModel(MAX_ACCEL, MAX_VEL, tec.getTemporalResolution(), tec.getControlPeriod(), tec.getTrackingPeriod())); - Robot robot5 = new Robot(5, 1 ); - tec.setFootprint(4, DEFAULT_FOOTPRINT); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(24.0,6.0,0.0); - Pose goalPoseRobot1 = new Pose(38.0,8.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(26.0,6.0,0.0); - - - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - - Pose startPoseGoal4 = new Pose(9.0,27.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - Pose endPoseGoal4 = new Pose(45.0,31.0,0.0); - Pose endPoseGoal5 = new Pose(42.0,6.0,0.0); - - - Task task4 = new Task(4,startPoseGoal4,endPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,endPoseGoal5,1); - - - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.8; - TaskAssignmentSimple assignmentProblem = new TaskAssignmentSimple(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setDefaultMotionPlanner(rsp); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - tec.setDefaultMotionPlanner(assignmentProblem.getDefaultMotionPlanner()); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.startTaskAssignment(tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap3.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap3.java deleted file mode 100644 index 50a7aa9..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap3.java +++ /dev/null @@ -1,176 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMap3 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - viz.setMap("maps/map-corridors-vi.yaml"); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint1,footprint2,footprint3,footprint4); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-corridors-vi.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-corridors-vi.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(3); - - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,Math.PI/2); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ); - Robot robot5 = new Robot(5, 1 ); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(24.0,6.0,0.0); - Pose goalPoseRobot1 = new Pose(38.0,10.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(48.0,6.0,0.0); - - - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - - Pose startPoseGoal4 = new Pose(9.0,27.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - Pose goalPoseRobot4 = new Pose(45.0,31.0,0.0); - Pose goalPoseRobot5 = new Pose(42.0,6.0,0.0); - - - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.6; - TaskAssignmentSimple assignmentProblem = new TaskAssignmentSimple(); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setLinearWeight(alpha); - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setDefaultMotionPlanner(rsp); - - tec.setDefaultMotionPlanner(assignmentProblem.getDefaultMotionPlanner()); - - assignmentProblem.startTaskAssignment(tec); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap4.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap4.java deleted file mode 100644 index bccbfa3..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap4.java +++ /dev/null @@ -1,216 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - - - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMap4 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - viz.setMap("maps/map-corridors-vi.yaml"); - - - - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,Math.PI/2); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1); - Robot robot2 = new Robot(2, 1); - Robot robot3 = new Robot(3, 1); - Robot robot4 = new Robot(4, 1); - Robot robot5 = new Robot(5, 1); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(24.0,6.0,0.0); - Pose startPoseGoal4 = new Pose(9.0,27.0,0.0); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - Pose goalPoseRobot1 = new Pose(38.0,8.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(48.0,6.0,0.0); - Pose goalPoseRobot4 = new Pose(45.0,31.0,0.0); - Pose goalPoseRobot5 = new Pose(42.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - - TaskAssignment assignmentProblem = new TaskAssignment(); - /////////////////////////////////////////////////////// - - - double [][][]optimalAllocation = {{{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0}}} - -; - //Solve the problem to find some feasible solution - double alpha = 1.0; - - - - - //tec.setFakeCoordinator(true); - assignmentProblem.LoadScenarioAllocation(optimalAllocation); - - tec.setAvoidDeadlocksGlobally(true); - //assignmentProblem.LoadScenario("ProvaScenario"); - - - int numPaths = 1; - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-corridors-vi.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-corridors-vi.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - - } - - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.startTaskAssignment(tec); - - /* - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemLocalSearch(tec,-1); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - */ - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap5.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap5.java deleted file mode 100644 index e1407fb..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap5.java +++ /dev/null @@ -1,189 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - - - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMap5 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - viz.setMap("maps/map-corridors-vi.yaml"); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,Math.PI/2); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Coordinate[] DEFAULT_FOOTPRINT = new Coordinate[] { - new Coordinate(-1.7, 0.7), //back left - new Coordinate(-1.7, -1.7), //back right - new Coordinate(2.7, -1.7), //front right - new Coordinate(2.7, 0.7)}; - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ,DEFAULT_FOOTPRINT, new ConstantAccelerationForwardModel(MAX_ACCEL, MAX_VEL, tec.getTemporalResolution(), tec.getControlPeriod(), tec.getTrackingPeriod())); - Robot robot5 = new Robot(5, 1 ); - tec.setFootprint(4, DEFAULT_FOOTPRINT); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(24.0,6.0,0.0); - Pose startPoseGoal4 = new Pose(9.0,27.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - - Pose goalPoseRobot1 = new Pose(38.0,5.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(26.0,6.0,0.0); - Pose goalPoseRobot4 = new Pose(45.0,31.0,0.0); - Pose goalPoseRobot5 = new Pose(42.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.8; - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-corridors-vi.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-corridors-vi.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - - } - - - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.startTaskAssignment(tec); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap6.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap6.java deleted file mode 100644 index 7c4efef..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap6.java +++ /dev/null @@ -1,193 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - - - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMap6 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - viz.setMap("maps/map-corridors-vi.yaml"); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - - - - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,Math.PI/2); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Coordinate[] DEFAULT_FOOTPRINT = new Coordinate[] { - new Coordinate(-1.7, 0.7), //back left - new Coordinate(-1.7, -1.7), //back right - new Coordinate(2.7, -1.7), //front right - new Coordinate(2.7, 0.7)}; - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ); - Robot robot5 = new Robot(5, 1 ); - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(24.0,6.0,0.0); - Pose startPoseGoal4 = new Pose(9.0,27.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - - Pose goalPoseRobot1 = new Pose(38.0,10.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(26.0,6.0,0.0); - Pose goalPoseRobot4 = new Pose(45.0,31.0,0.0); - Pose goalPoseRobot5 = new Pose(42.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 0.8; - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-corridors-vi.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-corridors-vi.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - - } - - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.startTaskAssignment(tec); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap7.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap7.java deleted file mode 100644 index 33bebb5..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap7.java +++ /dev/null @@ -1,216 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - - - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMap7 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - viz.setMap("maps/map-partial-2.yaml"); - - - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,0.0); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ); - Robot robot5 = new Robot(5, 1 ); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(30.0,8.0,0.0); - - Pose startPoseGoal4 = new Pose(27.0,20.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - - Pose goalPoseRobot1 = new Pose(41.0,4.0,0.0); - Pose goalPoseRobot2 = new Pose(46.0,13.0,0.0); - Pose goalPoseRobot3 = new Pose(48.0,4.0,0.0); - Pose goalPoseRobot4 = new Pose(48.0,27.0,0.0); - Pose goalPoseRobot5 = new Pose(40.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - - - - - - double [][][]optimalAllocation = {{{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0}}} - -; - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - TaskAssignment assignmentProblem = new TaskAssignment(); - - double alpha = 0.8; - - //tec.setFakeCoordinator(true); - assignmentProblem.LoadScenarioAllocation(optimalAllocation); - - //tec.setAvoidDeadlocksGlobally(true); - assignmentProblem.LoadScenario("ProvaScenario"); - int numPaths = 1; - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-partial-2.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-partial-2.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.startTaskAssignment(tec); - - - //double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemLocalSearch(tec,-1); - - /* - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - */ - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap8.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap8.java deleted file mode 100644 index 3b9f351..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMap8.java +++ /dev/null @@ -1,261 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - - - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMap8 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - viz.setMap("maps/CentroPiaggio.yaml"); - - - Pose startPoseRobot1 = new Pose(37.0,36.0,0.0); - Pose startPoseRobot2 = new Pose(37.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(73.0,20.0,Math.PI); - Pose startPoseRobot4 = new Pose(73.0,25.0,Math.PI); - Pose startPoseRobot5 = new Pose(33.0,53.0,0.0); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 2 ); - - - Robot robot4 = new Robot(4, 1 ); - Robot robot5 = new Robot(5, 3 ); - Robot robot6 = new Robot(6, 1 ); - Robot robot7 = new Robot(7, 1 ); - - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - - - - - - - Pose startPoseGoal1 = new Pose(35.0,84.0,Math.PI/2); - Pose startPoseGoal2 = new Pose(35.0,97.0,Math.PI/2); - Pose startPoseGoal3 = new Pose(68.0,76.0,0.0); - - Pose startPoseGoal4 = new Pose(42.0,119.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(83.0,93.0,0.0); - Pose startPoseGoal6 = new Pose(71.0,73.0,Math.PI/2); - Pose startPoseGoal7 = new Pose(58.0,97.0,Math.PI/2); - Pose startPoseGoal8 = new Pose(47.0,97.0,Math.PI/2); - - Pose goalPoseGoal1 = new Pose(37.0,87.0,Math.PI/2); - Pose goalPoseGoal2 = new Pose(38.0,95.0,Math.PI/2); - Pose goalPoseGoal3 = new Pose(73.0,80.0,0.0); - Pose goalPoseGoal4 = new Pose(43.0,120.0,Math.PI/2); - Pose goalPoseGoal5 = new Pose(84.0,94.0,0.0); - Pose goalPoseGoal6 = new Pose(72.0,74.0,Math.PI/2); - Pose goalPoseGoal7 = new Pose(60.0,98.0,Math.PI/2); - Pose goalPoseGoal8 = new Pose(48.0,98.0,Math.PI/2); - - - - //tec.addRobot(robot5,startPoseGoal4); - //tec.addRobot(robot6,goalPoseGoal8); - //tec.addRobot(robot7,goalPoseGoal7); - - - Task task1 = new Task(1,startPoseGoal1,goalPoseGoal1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseGoal2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseGoal3,2); - Task task4 = new Task(4,startPoseGoal4,goalPoseGoal4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseGoal5,1); - Task task6 = new Task(6,startPoseGoal6,goalPoseGoal6,2); - Task task7 = new Task(7,startPoseGoal7,goalPoseGoal7,3); - Task task8 = new Task(8,startPoseGoal8,goalPoseGoal8,3); - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double [][][]optimalAllocation = {{{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0}}, - {{0.0},{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0}}, - {{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{1.0},{0.0}}, - {{0.0},{0.0},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0}}} - - - - - - -; - - - - - - - - TaskAssignment assignmentProblem = new TaskAssignment(); - //Solve the problem to find some feasible solution - double alpha = 1.0; - - - - - //tec.setFakeCoordinator(true); - assignmentProblem.LoadScenarioAllocation(optimalAllocation); - - tec.setAvoidDeadlocksGlobally(true); - //assignmentProblem.LoadScenario("ProvaScenario"); - int numPaths = 1; - - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - assignmentProblem.addTask(task6); - assignmentProblem.addTask(task7); - assignmentProblem.addTask(task8); - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - //rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - //double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/CentroPiaggio.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/CentroPiaggio.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - //assignmentProblem.startTaskAssignment(tec); - MPSolver solver = assignmentProblem.buildOptimizationProblemWithBNormalized(tec); - double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblem(solver,tec); - - - //double [][][] assignmentMatrix = assignmentProblem.solveOptimizationProblemLocalSearch(tec,-1); - - for (int i = 0; i < assignmentMatrix.length; i++) { - for (int j = 0; j < assignmentMatrix[0].length; j++) { - for(int s = 0; s < numPaths; s++) { - System.out.println("x"+"["+(i+1)+","+(j+1)+","+(s+1)+"]"+" is "+ assignmentMatrix[i][j][s]); - if(assignmentMatrix[i][j][s] == 1) { - System.out.println("Robot " +(i+1) +" is assigned to Task "+ (j+1)+" throw Path " + (s+1)); - } - } - } - } - assignmentProblem.TaskAllocation(assignmentMatrix,tec); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy.java deleted file mode 100644 index e673912..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy.java +++ /dev/null @@ -1,180 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMapGreedy { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - - viz.setMap("maps/map-corridors-vi.yaml"); - - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,Math.PI/2); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ); - Robot robot5 = new Robot(5, 1 ); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(24.0,6.0,0.0); - Pose goalPoseRobot1 = new Pose(38.0,8.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(48.0,6.0,0.0); - - - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - - Pose startPoseGoal4 = new Pose(9.0,27.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - Pose goalPoseRobot4 = new Pose(45.0,31.0,0.0); - Pose goalPoseRobot5 = new Pose(42.0,6.0,0.0); - - - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-corridors-vi.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-corridors-vi.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy2.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy2.java deleted file mode 100644 index 76a3960..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy2.java +++ /dev/null @@ -1,178 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - - - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMapGreedy2 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - viz.setMap("maps/map-partial-2.yaml"); - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,0.0); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ); - Robot robot5 = new Robot(5, 1 ); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(30.0,8.0,0.0); - - Pose startPoseGoal4 = new Pose(27.0,20.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - - Pose goalPoseRobot1 = new Pose(40.0,8.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(48.0,4.0,0.0); - Pose goalPoseRobot4 = new Pose(48.0,27.0,0.0); - Pose goalPoseRobot5 = new Pose(42.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 1.0; - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-partial-2.yaml")); - double res = Double.parseDouble(Missions.getProperty("resolution", "maps/map-partial-2.yaml")); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy3.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy3.java deleted file mode 100644 index 9f63af6..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TaskAssignmentWithMapGreedy3.java +++ /dev/null @@ -1,178 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; - - - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TaskAssignmentWithMapGreedy3 { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - BrowserVisualization viz = new BrowserVisualization(); - viz.setInitialTransform(20, 0, 0); - tec.setVisualization(viz); - tec.setUseInternalCriticalPoints(false); - viz.setMap("maps/map-empty.yaml"); - Pose startPoseRobot1 = new Pose(20.0,15.0,0.0); - Pose startPoseRobot2 = new Pose(12.0,28.0,0.0); - Pose startPoseRobot3 = new Pose(8.0,20.0,0.0); - Pose startPoseRobot4 = new Pose(8.0,30.0,0.0); - Pose startPoseRobot5 = new Pose(4.0,12.0,Math.PI/2); - - Robot robot1 = new Robot(1, 1 ); - Robot robot2 = new Robot(2, 1 ); - Robot robot3 = new Robot(3, 1 ); - Robot robot4 = new Robot(4, 1 ); - Robot robot5 = new Robot(5, 1 ); - - tec.addRobot(robot1,startPoseRobot1); - tec.addRobot(robot2,startPoseRobot2); - tec.addRobot(robot3,startPoseRobot3); - tec.addRobot(robot4,startPoseRobot4); - tec.addRobot(robot5,startPoseRobot5); - - Pose startPoseGoal1 = new Pose(28.0,5.0,0.0); - Pose startPoseGoal2 = new Pose(22.0,15.0,0.0); - Pose startPoseGoal3 = new Pose(30.0,8.0,0.0); - - Pose startPoseGoal4 = new Pose(27.0,20.0,Math.PI/2); - Pose startPoseGoal5 = new Pose(26.0,5.0,0.0); - - Pose goalPoseRobot1 = new Pose(40.0,8.0,0.0); - Pose goalPoseRobot2 = new Pose(45.0,15.0,0.0); - Pose goalPoseRobot3 = new Pose(48.0,4.0,0.0); - Pose goalPoseRobot4 = new Pose(48.0,27.0,0.0); - Pose goalPoseRobot5 = new Pose(52.0,6.0,0.0); - - Task task1 = new Task(1,startPoseGoal1,goalPoseRobot1,1); - Task task2 = new Task(2,startPoseGoal2,goalPoseRobot2,1); - Task task3 = new Task(3,startPoseGoal3,goalPoseRobot3,1); - Task task4 = new Task(4,startPoseGoal4,goalPoseRobot4,1); - Task task5 = new Task(5,startPoseGoal5,goalPoseRobot5,1); - - /////////////////////////////////////////////////////// - //Solve the problem to find some feasible solution - double alpha = 1.0; - int numPaths = 1; - TaskAssignment assignmentProblem = new TaskAssignment(); - assignmentProblem.setmaxNumPaths(numPaths); - assignmentProblem.addTask(task1); - assignmentProblem.addTask(task2); - assignmentProblem.addTask(task3); - assignmentProblem.addTask(task4); - assignmentProblem.addTask(task5); - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - - - assignmentProblem.setFleetVisualization(viz); - assignmentProblem.setLinearWeight(alpha); - assignmentProblem.setCostFunctionsWeight(0.8, 0.1, 0.1); - assignmentProblem.setminMaxVelandAccel(MAX_VEL, MAX_ACCEL); - assignmentProblem.instantiateFleetMaster(0.1, false); - assignmentProblem.startTaskAssignmentGreedyAlgorithm(tec); - - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TestCreateEnvelope.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TestCreateEnvelope.java deleted file mode 100644 index 57e53b9..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TestCreateEnvelope.java +++ /dev/null @@ -1,471 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintStream; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Calendar; -import java.util.Comparator; -import java.util.Random; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.vividsolutions.jts.geom.Coordinate; -import com.vividsolutions.jts.geom.Geometry; -import com.vividsolutions.jts.geom.GeometryFactory; -import com.vividsolutions.jts.geom.Polygon; - -import aima.core.agent.Model; -import se.oru.coordination.coordination_oru.AbstractTrajectoryEnvelopeCoordinator; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.demo.DemoDescription; -import se.oru.coordination.coordination_oru.motionplanning.AbstractMotionPlanner; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TimedTrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - - - - -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignment; -import se.oru.coordination.coordination_oru.taskassignment.TaskAssignmentSimple; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; - -import se.oru.coordination.coordination_oru.taskassignment.Robot; -import se.oru.coordination.coordination_oru.taskassignment.Task; - - - - -import com.google.ortools.linearsolver.*; -import com.google.ortools.linearsolver.MPSolver.ResultStatus; -import com.google.ortools.linearsolver.PartialVariableAssignment; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.constraintsolver.Solver; -import com.google.ortools.sat.*; - -@DemoDescription(desc = "One-shot navigation of 3 robots coordinating on paths obtained with the ReedsSheppCarPlanner.") -public class TestCreateEnvelope { - //load library used for optimization - static { - System.loadLibrary("jniortools"); - } - - protected int maxNumPaths; - protected int numRobot; - protected int numRobotAug; - protected int numTaskAug; - protected ArrayList IDsIdleRobots = new ArrayList (); - protected int numTask; - protected int dummyTask; - protected int dummyRobot; - protected double [][][] pathArray; - protected double timeOut = Double.POSITIVE_INFINITY; - protected double MaxPathLength = Integer.MAX_VALUE; - protected ArrayList taskQueue = new ArrayList (); - - - - private void dummyRobotorTask(int numRobot, int numTasks,AbstractTrajectoryEnvelopeCoordinator tec) { - numRobotAug = numRobot; - numTaskAug = numTasks; - //Restore initial value for dummy robot and task - dummyTask = 0; - dummyRobot = 0; - //Considering the possibility to have n != m - //If n > m -> we have dummy robot, so at some robot is assign the task to stay in starting position - if (numRobot > numTasks) { - dummyTask = numRobot - numTasks; - numTaskAug = numTasks + dummyTask; - } - else if (numRobot < numTasks) { - dummyRobot = numTasks - numRobot; - numRobotAug = numRobot + dummyRobot; - } - //This second check is used when we have particular cases due to Robot Type and Task Type - //Only if we have already a dummy task this check can be avoided - //If A robot cannot be assigned to any task - if (dummyTask == 0 || dummyRobot != 0) { - for (int i = 0; i < numRobot; i++) { - boolean flagAllocateRobot = false; - for (int j = 0; j < numTasks; j++) { - //check if robot can be assigned to one task - //if (taskQueue.get(j).getTaskType() == tec.getRobotType(IDsIdleRobots[i])) { - if (taskQueue.get(j).isCompatible(tec.getRobot(IDsIdleRobots.get(i)))) { - flagAllocateRobot = true; - - } - } - //the robot cannot be assigned to any task -> add a dummy robot and task - if (!flagAllocateRobot) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - } - } - } - //Only if we have already a dummy robot this check can be avoided - //If A task cannot be assigned to any robot - if (dummyRobot == 0 || dummyTask != 0) { - for (int i = 0; i < numTasks; i++) { - boolean flagAllocateTask = false; - for (int j = 0; j < numRobot; j++) { - //check if task can be assigned to one robot - //if (taskQueue.get(i).getTaskType() == tec.getRobotType(IDsIdleRobots[j])) { - if (taskQueue.get(i).isCompatible(tec.getRobot(IDsIdleRobots.get(j)))) { - flagAllocateTask = true; - } - } - //the task cannot be assigned to any robot -> add a dummy robot and task - if (!flagAllocateTask) { - dummyRobot += 1 ; - dummyTask += 1 ; - numRobotAug += 1; - numTaskAug += 1; - } - } - } - } - - - public boolean addTask(Task task) { - if (task == null) { - throw new Error("Cannot add the task"); - } - boolean TaskisAdded = taskQueue.add(task); - return TaskisAdded; - } - - public int getNumRobotAug() { - return this.numRobotAug; - } - - - - public void checkOnSharedTask() { - int sum=0; - - numRobotAug = numRobot; - System.out.println("numRobotAug" + numRobotAug); - for(Task taskID : taskQueue) { - sum += taskID.getRobotRequired(); - - } - System.out.println("suuum" +sum +" diff " + (sum-numRobot)); - if((sum-numRobot)>0) { - numRobotAug = numRobot + (sum-numRobotAug); - } - System.out.println("numRobotAug" + numRobotAug); - } - public void setPathArray(double [][][] matrix) { - this.pathArray = matrix; - } - private MPVariable [][][] tranformArray(MPSolver optimizationProblem) { - //Take the vector of Decision Variable from the Optimization Problem - MPVariable [] array1D = optimizationProblem.variables(); - MPVariable [][][] decisionVariable = new MPVariable [numRobot][numTask][maxNumPaths]; - //Store them in a 2D Matrix - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTask; j++) { - for (int s = 0; s < maxNumPaths; s++) { - decisionVariable[i][j][s] = array1D[i*numTask*maxNumPaths+j*maxNumPaths+s]; - } - - } - } - return decisionVariable; - } - - private MPSolver constraintOnPreviousSolution(MPSolver optimizationProblem, double [][][] assignmentMatrix) { - //Take decision Variable from Optimization Problem - MPVariable [][][] DecisionVariable = tranformArray(optimizationProblem); - //Initialize a Constraint - //MPConstraint c2 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY,1); - MPConstraint c2 = optimizationProblem.makeConstraint(0,numRobot-1); - //Define the actual optimal solution as a Constraint in order to not consider more it - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTask; j++) { - for(int s = 0;s < maxNumPaths; s++) { - if (assignmentMatrix[i][j][s] >0) { - c2.setCoefficient(DecisionVariable[i][j][s],1); - } - } - } - } - //Return the updated Optimization Problem - return optimizationProblem; - } - - private MPSolver buildOptimizationProblem(int numRobotAug,int numTasksAug) { - //Initialize a linear solver - - MPSolver optimizationProblem = new MPSolver( - "TaskAssignment", MPSolver.OptimizationProblemType.CBC_MIXED_INTEGER_PROGRAMMING); - //START DECISION VARIABLE VARIABLE - MPVariable [][][] decisionVariable = new MPVariable[numRobotAug][numTasksAug][maxNumPaths]; - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTasksAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - decisionVariable[i][j][s] = optimizationProblem.makeBoolVar("x"+"["+i+","+j+","+s+"]"); - } - - } - } - //END DECISION VARIABLE - ////////////////////////// - // START CONSTRAINTS - //Each Robot can be assign only to a Task - for (int i = 0; i < numRobotAug; i++) { - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY, 1); - for (int j = 0; j < numTasksAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j][s], 1); - } - - } - } - - //Each task can be performed only by a robot - for (int j = 0; j < numTasksAug; j++) { - int RobotReq= 1; - if(j < taskQueue.size()) { - Task tt= taskQueue.get(j); - RobotReq = tt.getRobotRequired(); - } - - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(1, 1); - for (int i = 0; i < numRobotAug; i++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j][s], 1); - } - } - } - - for (int i=0; i< numRobot; i++ ) { - for (int j=0; j< numTask; j++ ) { - for(int s = 0; s < maxNumPaths; s++) { - if (i < numRobot) { //Considering only real Robot - double pss = pathArray[i][j][s]; - if(pss==0) { - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j][s],1); - } - } - } - } - } - ///////////////////////////////////////////////// - return optimizationProblem; - } - - - - - public MPSolver buildOptimizationProblemWithBNormalized(AbstractTrajectoryEnvelopeCoordinator tec) { - - numTask = taskQueue.size(); - //Get free robots and their IDs - numRobot = tec.getIdleRobots().size(); - IDsIdleRobots = tec.getIdleRobots(); - - - dummyRobotorTask(numRobot,numTask,tec); - checkOnSharedTask(); - - //Evaluate dummy robot and dummy task - - System.out.println("llll" +numRobotAug + "ppppp " + numTaskAug); - //Take the number of tasks - - //Build the solver and an objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobotAug,numTask); - - MPVariable [][][] decisionVariable = tranformArray(optimizationProblem); - ///////////////////////////////// - //START OBJECTIVE FUNCTION - MPObjective objective = optimizationProblem.objective(); - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTask; j++) { - for(int s = 0; s < maxNumPaths; s++) { - double pathLength = pathArray[i][j][s]; - if ( pathLength != MaxPathLength) { - //Set the coefficient of the objective function with the normalized path length - objective.setCoefficient(decisionVariable[i][j][s], pathLength); - }else { // if the path does not exists or the robot type is different from the task type - //the path to reach the task not exists - //the decision variable is set to 0 -> this allocation is not valid - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j][s],1); - } - } - } - } - //Define the problem as a minimization problem - objective.setMinimization(); - //END OBJECTIVE FUNCTION - return optimizationProblem; - } - - - - public static void main(String[] args) throws InterruptedException { - //Max Vel and Acc for the robots - double MAX_ACCEL = 1.0; - double MAX_VEL = 4.0; - //Instantiate a timed trajectory envelope coordinator. - //You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added) - //final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - final TimedTrajectoryEnvelopeCoordinatorSimulation tec = new TimedTrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL); - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - CriticalSection cs = o1.getCriticalSection(); - RobotReport robotReport1 = o1.getRobotReport(); - RobotReport robotReport2 = o2.getRobotReport(); - return ((cs.getTe1Start()-robotReport1.getPathIndex())-(cs.getTe2Start()-robotReport2.getPathIndex())); - } - }); - - tec.addComparator(new Comparator () { - @Override - public int compare(RobotAtCriticalSection o1, RobotAtCriticalSection o2) { - return (o2.getRobotReport().getRobotID()-o1.getRobotReport().getRobotID()); - } - }); - - //Need to instantiate the fleetmaster interface - tec.instantiateFleetMaster(0.1, false); - Coordinate footprint1 = new Coordinate(-1.0,0.5); - Coordinate footprint2 = new Coordinate(1.0,0.5); - Coordinate footprint3 = new Coordinate(1.0,-0.5); - Coordinate footprint4 = new Coordinate(-1.0,-0.5); - tec.setDefaultFootprint(footprint1, footprint2, footprint3, footprint4); - //Need to setup infrastructure that maintains the representation - tec.setupSolver(0, 100000000); - //JTSDrawingPanelVisualization viz = new JTSDrawingPanelVisualization(); - //viz.setSize(1024, 768); - - - //BrowserVisualization viz = new BrowserVisualization(); - //viz.setInitialTransform(20, 0, 0); - //tec.setVisualization(viz); - //tec.setUseInternalCriticalPoints(false); - - String yamlFile = "maps/map-empty.yaml"; - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - - - Random rand = new Random(); - double alpha = 1.0; - double timeOut = 15*1000; - - int numPath = 1; - - TestCreateEnvelope test = new TestCreateEnvelope(); - int numPaths = 1; - double delta = 0; - for(int i = 1; i<= 3; i++) { - - Pose startPoseRobot = new Pose(4.0,(6.0 + delta),0.0); - int robotType = rand.nextInt(2)+1; - Robot robot = new Robot(i,1); - tec.addRobot(robot,startPoseRobot); - Pose startPoseGoal = new Pose(15.0,(6.0 + delta),0.0); - Pose goalPoseRobot = new Pose(30.0 ,(6.0 + delta) ,0.0); - - int taskType = rand.nextInt(2)+1; - Task task = new Task(i,startPoseGoal,goalPoseRobot,1); - if(i==2) { - task = new Task(i,10,startPoseGoal,goalPoseRobot,1); - task.setRobotRequired(2); - }else if(i==1) { - task = new Task(i,5,startPoseGoal,goalPoseRobot,1); - }else if(i==4) { - task = new Task(i,10,startPoseGoal,goalPoseRobot,1); - } - test.addTask(task); - - delta += 3.0; - - } - - - PrintStream fileStream = null; - - try { - fileStream = new PrintStream(new File("ProvaTask.txt")); - } catch (FileNotFoundException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - fileStream.println(tec.getRobotType(1)+""); - - - for (int robotID : tec.getIdleRobots()) { - Coordinate[] footprint = tec.getFootprint(robotID); - - - //Instantiate a simple motion planner (no map given here, otherwise provide yaml file) - ReedsSheppCarPlanner rsp = new ReedsSheppCarPlanner(); - rsp.setRadius(0.2); - rsp.setFootprint(footprint); - rsp.setTurningRadius(4.0); - rsp.setDistanceBetweenPathPoints(0.5); - rsp.setMapFilename("maps"+File.separator+Missions.getProperty("image", "maps/map-empty.yaml")); - double res = 0.2;// Double.parseDouble(getProperty("resolution", yamlFile)); - rsp.setMapResolution(res); - rsp.setPlanningTimeInSecs(2); - tec.setMotionPlanner(robotID, rsp); - } - tec.setFakeCoordinator(true); - //Solve the problem to find some feasible solution - - - double [][][]pathMatrix = new double[test.numRobotAug][test.numTaskAug][numPath]; - for (int i = 0; i < test.numRobotAug; i++) { - for (int j = 0; j < test.numTaskAug; j++) { - for(int s = 0; s < numPath; s++) { - pathMatrix[i][j][s] = 17.50; - } - } - } - test.setPathArray(pathMatrix); - - MPSolver optProblem = test.buildOptimizationProblemWithBNormalized(tec); - MPSolver.ResultStatus resultStatus = optProblem.solve(); - long timeOffsetInitial = Calendar.getInstance().getTimeInMillis(); - System.out.println("Test: " + resultStatus); - System.out.println("NumRobot: " + test.numRobot); - System.out.println("NumTask: " + test.numTask); - System.out.println("NumRobot A: " + test.numRobotAug); - System.out.println("NumTask A: " + test.numTaskAug); - MPVariable [][][] ll= test.tranformArray(optProblem); - for (int i = 0; i < test.numRobotAug; i++) { - for (int j = 0; j < test.numTaskAug; j++) { - for(int s = 0; s < numPath; s++) { - System.out.println("Value >> " + ll[i][j][s].solutionValue() +" i>> " +(i+1) + " j>> " + (j+1) + " s>> " + (s+1) ); - } - } - } - } -} diff --git a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TestScalability.java b/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TestScalability.java deleted file mode 100644 index 4d13668..0000000 --- a/src/main/java/se/oru/assignment/assignment_oru/taskassignment/test/TestScalability.java +++ /dev/null @@ -1,229 +0,0 @@ -package se.oru.coordination.coordination_oru.taskassignment.test; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Calendar; -import java.util.Comparator; -import java.util.HashMap; - -import org.metacsp.multi.spatioTemporal.paths.Pose; -import org.metacsp.multi.spatioTemporal.paths.PoseSteering; -import org.metacsp.multi.spatioTemporal.paths.Trajectory; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope; -import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope.SpatialEnvelope; - -import com.google.ortools.linearsolver.MPConstraint; -import com.google.ortools.linearsolver.MPObjective; -import com.google.ortools.linearsolver.MPSolver; -import com.google.ortools.linearsolver.MPVariable; -import com.vividsolutions.jts.geom.Coordinate; - -import se.oru.coordination.coordination_oru.AbstractTrajectoryEnvelopeCoordinator; -import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel; -import se.oru.coordination.coordination_oru.CriticalSection; -import se.oru.coordination.coordination_oru.Mission; -import se.oru.coordination.coordination_oru.RobotAtCriticalSection; -import se.oru.coordination.coordination_oru.RobotReport; -import se.oru.coordination.coordination_oru.TrackingCallback; -import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner; -import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation; -import se.oru.coordination.coordination_oru.util.BrowserVisualization; -import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization; -import se.oru.coordination.coordination_oru.util.Missions; - -public class TestScalability { - - static { - System.loadLibrary("jniortools"); - } - - protected int maxNumPaths; - protected int numRobot; - protected int numTask; - protected double [][][] pathArray; - protected double timeOut = Double.POSITIVE_INFINITY; - protected double MaxPathLength = Integer.MAX_VALUE; - - public TestScalability(int numRobot,int numTask,int maxNumPaths) { - this.numRobot = numRobot; - this.numTask = numTask; - this.maxNumPaths = maxNumPaths; - } - - public void setPathArray(double [][][] matrix) { - this.pathArray = matrix; - } - private MPVariable [][][] tranformArray(MPSolver optimizationProblem) { - //Take the vector of Decision Variable from the Optimization Problem - MPVariable [] array1D = optimizationProblem.variables(); - MPVariable [][][] decisionVariable = new MPVariable [numRobot][numTask][maxNumPaths]; - //Store them in a 2D Matrix - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTask; j++) { - for (int s = 0; s < maxNumPaths; s++) { - decisionVariable[i][j][s] = array1D[i*numTask*maxNumPaths+j*maxNumPaths+s]; - } - - } - } - return decisionVariable; - } - - private MPSolver constraintOnPreviousSolution(MPSolver optimizationProblem, double [][][] assignmentMatrix) { - //Take decision Variable from Optimization Problem - MPVariable [][][] DecisionVariable = tranformArray(optimizationProblem); - //Initialize a Constraint - //MPConstraint c2 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY,1); - MPConstraint c2 = optimizationProblem.makeConstraint(0,numRobot-1); - //Define the actual optimal solution as a Constraint in order to not consider more it - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTask; j++) { - for(int s = 0;s < maxNumPaths; s++) { - if (assignmentMatrix[i][j][s] >0) { - c2.setCoefficient(DecisionVariable[i][j][s],1); - } - } - } - } - //Return the updated Optimization Problem - return optimizationProblem; - } - - private MPSolver buildOptimizationProblem(int numRobotAug,int numTasksAug) { - //Initialize a linear solver - - MPSolver optimizationProblem = new MPSolver( - "TaskAssignment", MPSolver.OptimizationProblemType.CBC_MIXED_INTEGER_PROGRAMMING); - //START DECISION VARIABLE VARIABLE - MPVariable [][][] decisionVariable = new MPVariable[numRobotAug][numTasksAug][maxNumPaths]; - for (int i = 0; i < numRobotAug; i++) { - for (int j = 0; j < numTasksAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - decisionVariable[i][j][s] = optimizationProblem.makeBoolVar("x"+"["+i+","+j+","+s+"]"); - } - - } - } - //END DECISION VARIABLE - ////////////////////////// - // START CONSTRAINTS - //Each Robot can be assign only to a Task - for (int i = 0; i < numRobotAug; i++) { - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(-Double.POSITIVE_INFINITY, 1); - for (int j = 0; j < numTasksAug; j++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j][s], 1); - } - - } - } - - //Each task can be performed only by a robot - for (int j = 0; j < numTasksAug; j++) { - //Initialize the constraint - MPConstraint c0 = optimizationProblem.makeConstraint(1, 1); - for (int i = 0; i < numRobotAug; i++) { - for(int s = 0; s < maxNumPaths; s++) { - //Build the constraint - c0.setCoefficient(decisionVariable[i][j][s], 1); - } - } - } - - for (int i=0; i< numRobot; i++ ) { - for (int j=0; j< numTask; j++ ) { - for(int s = 0; s < maxNumPaths; s++) { - if (i < numRobot) { //Considering only real Robot - double pss = pathArray[i][j][s]; - if(pss==0) { - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j][s],1); - } - } - } - } - } - ///////////////////////////////////////////////// - return optimizationProblem; - } - - - - - public MPSolver buildOptimizationProblemWithBNormalized() { - - //Take the number of tasks - - //Build the solver and an objective function - MPSolver optimizationProblem = buildOptimizationProblem(numRobot,numTask); - - MPVariable [][][] decisionVariable = tranformArray(optimizationProblem); - ///////////////////////////////// - //START OBJECTIVE FUNCTION - MPObjective objective = optimizationProblem.objective(); - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTask; j++) { - for(int s = 0; s < maxNumPaths; s++) { - double pathLength = pathArray[i][j][s]; - if ( pathLength != MaxPathLength) { - //Set the coefficient of the objective function with the normalized path length - objective.setCoefficient(decisionVariable[i][j][s], pathLength); - }else { // if the path does not exists or the robot type is different from the task type - //the path to reach the task not exists - //the decision variable is set to 0 -> this allocation is not valid - MPConstraint c3 = optimizationProblem.makeConstraint(0,0); - c3.setCoefficient(decisionVariable[i][j][s],1); - } - } - } - } - //Define the problem as a minimization problem - objective.setMinimization(); - //END OBJECTIVE FUNCTION - return optimizationProblem; - } - - - - - public static void main(String[] args) throws InterruptedException { - long initialTime = Calendar.getInstance().getTimeInMillis(); - int numRobot = 3; - int numTask = 3; - int numPath = 1; - TestScalability test = new TestScalability(numRobot,numTask,numPath); - double [][][]pathMatrix = new double[numRobot][numTask][numPath]; - for (int i = 0; i < numRobot; i++) { - for (int j = 0; j < numTask; j++) { - for(int s = 0; s < numPath; s++) { - pathMatrix[i][j][s] = 17.50; - } - } - } - test.setPathArray(pathMatrix); - MPSolver optProblem = test.buildOptimizationProblemWithBNormalized(); - MPSolver.ResultStatus resultStatus = optProblem.solve(); - long timeOffsetInitial = Calendar.getInstance().getTimeInMillis(); - long timeOffset = 0; - double timeOut = 15*1000; - while(resultStatus != MPSolver.ResultStatus.INFEASIBLE && timeOffset < timeOut) { - - resultStatus = optProblem.solve(); - long timeOffsetFinal = Calendar.getInstance().getTimeInMillis(); - timeOffset = timeOffsetFinal - timeOffsetInitial; - System.out.println("Time for solution " + timeOffset/1000 +" [s]"); - } - - long finalTime = Calendar.getInstance().getTimeInMillis(); - long timeRequire = finalTime - initialTime; - System.out.println("Required Time for Optimization " + timeRequire +" [ms]"); - System.out.println("Number of robot: " + numRobot); - System.out.println("Number of Task: " + numTask); - System.out.println("Number of Path: " + numPath); - System.out.println("Number of Path: " + (Calendar.getInstance().getTimeInMillis()