Add update_travel_time method

ropod-project · Oct 6, 2020 · 35c1b54 · 35c1b54
1 parent 4116f2d
commit 35c1b54
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 0 deletions.
diff --git a/stn/pstn/pstn.py b/stn/pstn/pstn.py
@@ -150,6 +150,20 @@ def add_intertimepoints_constraints(self, constraints, task):
                 # wait time between finish of one task and departure of the next one. Fixed to [0, inf]
                 self.add_constraint(i, j)
 
+    def update_travel_time(self, task):
+        position = self.get_task_position(task.task_id)
+        departure_node_id = 2 * position + (position-2)
+        start_node_id = departure_node_id + 1
+        distribution = self.get_travel_time_distribution(task)
+
+        if self.has_edge(departure_node_id, start_node_id):
+            if distribution.endswith("_0.0"):  # the distribution has no variation (stdev is 0)
+                # Make the constraint a requirement constraint
+                mean = float(distribution.split("_")[1])
+                self.add_constraint(departure_node_id, start_node_id, mean, mean)
+            else:
+                self.add_constraint(departure_node_id, start_node_id, distribution=distribution)
+
     @staticmethod
     def get_travel_time_distribution(task):
         travel_time = task.get_edge("travel_time")

diff --git a/stn/stn.py b/stn/stn.py
@@ -245,6 +245,15 @@ def add_intertimepoints_constraints(self, constraints, task):
                 # wait time between finish of one task and departure of the next one. Fixed to [0, inf]
                 self.add_constraint(i, j)
 
+    def update_travel_time(self, task):
+        position = self.get_task_position(task.task_id)
+        departure_node_id = 2 * position + (position-2)
+        start_node_id = departure_node_id + 1
+        travel_time = self.get_travel_time(task)
+
+        if self.has_edge(departure_node_id, start_node_id):
+            self.add_constraint(departure_node_id, start_node_id, travel_time, travel_time)
+
     @staticmethod
     def get_travel_time(task):
         """ Returns the mean of the travel time (time for going from current pose to start pose)

diff --git a/stn/stnu/stnu.py b/stn/stnu/stnu.py
@@ -136,6 +136,18 @@ def add_intertimepoints_constraints(self, constraints, task):
                 # wait time between finish of one task and departure of the next one. Fixed to [0, inf]
                 self.add_constraint(i, j, 0)
 
+    def update_travel_time(self, task):
+        position = self.get_task_position(task.task_id)
+        departure_node_id = 2 * position + (position-2)
+        start_node_id = departure_node_id + 1
+        lower_bound, upper_bound = self.get_travel_time_bounded_duration(task)
+
+        if self.has_edge(departure_node_id, start_node_id):
+            if lower_bound == upper_bound:
+                self.add_constraint(departure_node_id, start_node_id, 0, 0)
+            else:
+                self.add_constraint(departure_node_id, start_node_id, lower_bound, upper_bound, is_contingent=True)
+
     @staticmethod
     def get_travel_time_bounded_duration(task):
         """ Returns the estimated travel time as a bounded interval