diff --git a/rmf_tasks/include/rmf_tasks/Estimate.hpp b/rmf_tasks/include/rmf_tasks/Estimate.hpp
index b57c8e79e..d0d41f0c9 100644
--- a/rmf_tasks/include/rmf_tasks/Estimate.hpp
+++ b/rmf_tasks/include/rmf_tasks/Estimate.hpp
@@ -29,14 +29,25 @@ class Estimate
 {
 public:
 
+  /// Constructor of an estimate of a task request.
+  ///
+  /// \param[in] finish_state
+  ///   Finish state of the robot once it completes the task request.
+  ///
+  /// \param[in] wait_until
+  ///   The ideal time the robot starts executing this task.
   Estimate(agv::State finish_state, rmf_traffic::Time wait_until);
 
+  /// Finish state of the robot once it completes the task request.
   agv::State finish_state() const;
 
+  /// Sets a new finish state for the robot.
   Estimate& finish_state(agv::State new_finish_state);
 
+  /// The ideal time the robot starts executing this task.
   rmf_traffic::Time wait_until() const;
 
+  /// Sets a new starting time for the robot to execute the task request.
   Estimate& wait_until(rmf_traffic::Time new_wait_until);
 
   class Implementation;
diff --git a/rmf_tasks/include/rmf_tasks/Request.hpp b/rmf_tasks/include/rmf_tasks/Request.hpp
index 846fe89fa..c8fbbcf0a 100644
--- a/rmf_tasks/include/rmf_tasks/Request.hpp
+++ b/rmf_tasks/include/rmf_tasks/Request.hpp
@@ -29,26 +29,26 @@
 
 namespace rmf_tasks {
 
-/// Implement this for new tasks
+/// Implement this for new type of requests.
 class Request
 {
 public:
 
   using SharedPtr = std::shared_ptr<Request>;
 
-  // Get the id of the task
+  /// Get the id of the task
   virtual std::size_t id() const = 0;
 
-  // Estimate the state of the robot when the task is finished along with the
-  // time the robot has to wait before commencing the task
+  /// Estimate the state of the robot when the task is finished along with the
+  /// time the robot has to wait before commencing the task
   virtual rmf_utils::optional<Estimate> estimate_finish(
     const agv::State& initial_state,
     const agv::StateConfig& state_config) const = 0;
 
-  // Estimate the invariant component of the task's duration
+  /// Estimate the invariant component of the task's duration
   virtual rmf_traffic::Duration invariant_duration() const = 0;
 
-  // Get the earliest start time that this task may begin
+  /// Get the earliest start time that this task may begin
   virtual rmf_traffic::Time earliest_start_time() const = 0;
 
   virtual ~Request() = default;
diff --git a/rmf_tasks/include/rmf_tasks/agv/State.hpp b/rmf_tasks/include/rmf_tasks/agv/State.hpp
index bd569874b..454076f99 100644
--- a/rmf_tasks/include/rmf_tasks/agv/State.hpp
+++ b/rmf_tasks/include/rmf_tasks/agv/State.hpp
@@ -24,45 +24,67 @@
 #include <rmf_utils/optional.hpp>
 
 #include <rmf_traffic/Time.hpp>
+#include <rmf_traffic/agv/Planner.hpp>
 
 namespace rmf_tasks {
 namespace agv {
 
+/// This state representation is used for task planning.
 class State
 {
 public:
 
   /// Constructor
   ///
-  /// \param[in] waypoint
+  /// \param[in] plan_start
+  ///   Current state's next start for new plans, includes the time which
+  ///   the plan can feasibly start, according to the finishing time of any
+  ///   tasks that the robot is currently performing.
+  ///
   /// \param[in] charging_waypoint
-  /// \param[in] finish_time
+  ///   The charging waypoint index of this robot.
+  ///
   /// \param[in] battery_soc
-  /// \param[in] threshold_soc
+  ///   Current battery state of charge of the robot. This value needs to be
+  ///   between 0.0 to 1.0.
   State(
-    std::size_t waypoint, 
+    rmf_traffic::agv::Plan::Start plan_start,
     std::size_t charging_waypoint,
-    rmf_traffic::Time finish_time = std::chrono::steady_clock::now(),
-    double battery_soc = 1.0);
-
-  State();
+    double battery_soc);
   
-  std::size_t waypoint() const;
+  /// The next plan start based on the current state.
+  rmf_traffic::agv::Plan::Start plan_start() const;
 
-  State& waypoint(std::size_t new_waypoint);
+  /// Sets the next plan start.
+  State& plan_start(rmf_traffic::agv::Plan::Start new_plan_start);
 
+  /// Robot's charging waypoint index.
   std::size_t charging_waypoint() const;
 
+  /// Sets the charging waypoint index.
   State& charging_waypoint(std::size_t new_charging_waypoint);
 
-  rmf_traffic::Time finish_time() const;
-
-  State& finish_time(rmf_traffic::Time new_finish_time);
-
+  /// The current battery state of charge of the robot. This value is between
+  /// 0.0 and 1.0.
   double battery_soc() const;
 
+  /// Sets a new battery state of charge value. This value needs to be between
+  /// 0.0 and 1.0.
   State& battery_soc(double new_battery_soc);
 
+  /// The current location waypoint index.
+  std::size_t waypoint() const;
+
+  /// Sets the current location waypoint index.
+  State& waypoint(std::size_t new_waypoint);
+
+  /// The time which the robot finishes its current task or when it is ready for
+  /// a new task.
+  rmf_traffic::Time finish_time() const;
+
+  /// Sets the finish time for the robot.
+  State& finish_time(rmf_traffic::Time new_finish_time);
+
   class Implementation;
 private:
   rmf_utils::impl_ptr<Implementation> _pimpl;
diff --git a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp
index 87274040f..0c21cda49 100644
--- a/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp
+++ b/rmf_tasks/include/rmf_tasks/agv/StateConfig.hpp
@@ -30,10 +30,15 @@ class StateConfig
   /// Constructor
   ///
   /// \param[in] threshold_soc
+  ///   Minimum charge level the battery is allowed to deplete to. This
+  ///   value needs to be between 0.0 and 1.0.
   StateConfig(double threshold_soc);
 
+  /// Gets the battery state of charge threshold value.
   double threshold_soc() const;
 
+  /// Sets a new battery state of charge threshold value. This value needs to be
+  /// between 0.0 and 1.0.
   StateConfig& threshold_soc(double threshold_soc);
 
   class Implementation;
diff --git a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp
index 731258b8c..aba301b88 100644
--- a/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp
+++ b/rmf_tasks/include/rmf_tasks/agv/TaskPlanner.hpp
@@ -127,6 +127,7 @@ class TaskPlanner
   /// Get the greedy planner based assignments for a set of initial states and 
   /// requests
   Assignments greedy_plan(
+    rmf_traffic::Time relative_start_time,
     std::vector<State> initial_states,
     std::vector<StateConfig> state_configs,
     std::vector<Request::SharedPtr> requests);
@@ -138,6 +139,7 @@ class TaskPlanner
   /// recommended to call plan() method and use the greedy solution for bidding.
   /// If a bid is awarded, the optimal solution may be used for assignments.
   Assignments optimal_plan(
+    rmf_traffic::Time relative_start_time,
     std::vector<State> initial_states,
     std::vector<StateConfig> state_configs,
     std::vector<Request::SharedPtr> requests,
diff --git a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp
index 759fbe335..53102f1df 100644
--- a/rmf_tasks/include/rmf_tasks/requests/Clean.hpp
+++ b/rmf_tasks/include/rmf_tasks/requests/Clean.hpp
@@ -49,8 +49,8 @@ class Clean : public rmf_tasks::Request
     std::shared_ptr<rmf_battery::DevicePowerSink> ambient_sink,
     std::shared_ptr<rmf_battery::DevicePowerSink> cleaning_sink,
     std::shared_ptr<rmf_traffic::agv::Planner> planner,
-    bool drain_battery = true,
-    rmf_traffic::Time start_time = std::chrono::steady_clock::now());
+    rmf_traffic::Time start_time,
+    bool drain_battery = true);
 
   std::size_t id() const final;
 
diff --git a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp
index 16a2e8937..0bd9a3dc1 100644
--- a/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp
+++ b/rmf_tasks/include/rmf_tasks/requests/Delivery.hpp
@@ -46,8 +46,8 @@ class Delivery : public rmf_tasks::Request
     std::shared_ptr<rmf_battery::MotionPowerSink> motion_sink,
     std::shared_ptr<rmf_battery::DevicePowerSink> device_sink,
     std::shared_ptr<rmf_traffic::agv::Planner> planner,
-    bool drain_battery = true,
-    rmf_traffic::Time start_time = std::chrono::steady_clock::now());
+    rmf_traffic::Time start_time,
+    bool drain_battery = true);
 
   std::size_t id() const final;
 
diff --git a/rmf_tasks/src/agv/State.cpp b/rmf_tasks/src/agv/State.cpp
index accbd3112..13b1cef09 100644
--- a/rmf_tasks/src/agv/State.cpp
+++ b/rmf_tasks/src/agv/State.cpp
@@ -15,6 +15,8 @@
  *
 */
 
+#include <stdexcept>
+
 #include <rmf_tasks/agv/State.hpp>
 
 namespace rmf_tasks {
@@ -26,49 +28,42 @@ class State::Implementation
 public:
 
   Implementation(
-    std::size_t waypoint, 
+    rmf_traffic::agv::Plan::Start plan_start,
     std::size_t charging_waypoint,
-    rmf_traffic::Time finish_time,
     double battery_soc)
-  : _waypoint(waypoint),
+  : _plan_start(std::move(plan_start)),
     _charging_waypoint(charging_waypoint),
-    _finish_time(finish_time),
     _battery_soc(battery_soc)
-  {}
+  {
+    if (_battery_soc < 0.0 || _battery_soc > 1.0)
+      throw std::invalid_argument(
+        "Battery State of Charge needs be between 0.0 and 1.0.");
+  }
 
-  std::size_t _waypoint;
+  rmf_traffic::agv::Plan::Start _plan_start;
   std::size_t _charging_waypoint;
-  rmf_traffic::Time _finish_time;
   double _battery_soc;
 };
 
 //==============================================================================
 State::State(
-  std::size_t waypoint, 
+  rmf_traffic::agv::Plan::Start plan_start,
   std::size_t charging_waypoint,
-  rmf_traffic::Time finish_time,
   double battery_soc)
 : _pimpl(rmf_utils::make_impl<Implementation>(
-    Implementation(
-      waypoint, charging_waypoint, finish_time, battery_soc)))
-{}
-
-//==============================================================================
-State::State()
-: _pimpl(rmf_utils::make_impl<Implementation>(
-    Implementation(0, 0, std::chrono::steady_clock::now(), 0.0)))
+    Implementation(std::move(plan_start), charging_waypoint, battery_soc)))
 {}
 
 //==============================================================================
-std::size_t State::waypoint() const
+rmf_traffic::agv::Plan::Start State::plan_start() const
 {
-  return _pimpl->_waypoint;
+  return _pimpl->_plan_start;
 }
 
 //==============================================================================
-State& State::waypoint(std::size_t new_waypoint)
+State& State::plan_start(rmf_traffic::agv::Plan::Start new_plan_start)
 {
-  _pimpl->_waypoint = new_waypoint;
+  _pimpl->_plan_start = std::move(new_plan_start);
   return *this;
 }
 
@@ -86,30 +81,48 @@ State& State::charging_waypoint(std::size_t new_charging_waypoint)
 }
 
 //==============================================================================
-rmf_traffic::Time State::finish_time() const
+double State::battery_soc() const
 {
-  return _pimpl->_finish_time;
+  return _pimpl->_battery_soc;
 }
 
 //==============================================================================
-State& State::finish_time(rmf_traffic::Time new_finish_time)
+State& State::battery_soc(double new_battery_soc)
 {
-  _pimpl->_finish_time = new_finish_time;
+  if (new_battery_soc < 0.0 || new_battery_soc > 1.0)
+    throw std::invalid_argument(
+      "Battery State of Charge needs be between 0.0 and 1.0.");
+
+  _pimpl->_battery_soc = new_battery_soc;
   return *this;
 }
 
 //==============================================================================
-double State::battery_soc() const
+std::size_t State::waypoint() const
 {
-  return _pimpl->_battery_soc;
+  return _pimpl->_plan_start.waypoint();
 }
 
 //==============================================================================
-State& State::battery_soc(double new_battery_soc)
+State& State::waypoint(std::size_t new_waypoint)
 {
-  _pimpl->_battery_soc = new_battery_soc;
+  _pimpl->_plan_start.waypoint(new_waypoint);
+  return *this;
+}
+
+//==============================================================================
+rmf_traffic::Time State::finish_time() const
+{
+  return _pimpl->_plan_start.time();
+}
+
+//==============================================================================
+State& State::finish_time(rmf_traffic::Time new_finish_time)
+{
+  _pimpl->_plan_start.time(new_finish_time);
   return *this;
 }
 
+//==============================================================================
 } // namespace agv
 } // namespace rmf_tasks
diff --git a/rmf_tasks/src/agv/StateConfig.cpp b/rmf_tasks/src/agv/StateConfig.cpp
index d5593cb72..b187fe0d8 100644
--- a/rmf_tasks/src/agv/StateConfig.cpp
+++ b/rmf_tasks/src/agv/StateConfig.cpp
@@ -15,6 +15,8 @@
  *
 */
 
+#include <stdexcept>
+
 #include <rmf_tasks/agv/StateConfig.hpp>
 
 namespace rmf_tasks {
@@ -34,7 +36,9 @@ StateConfig::StateConfig(double threshold_soc)
       threshold_soc
     }))
 {
-  // Do nothing
+  if (threshold_soc < 0.0 || threshold_soc > 1.0)
+    throw std::invalid_argument(
+      "Battery State of Charge threshold needs be between 0.0 and 1.0.");
 }
 
 double StateConfig::threshold_soc() const
@@ -44,6 +48,10 @@ double StateConfig::threshold_soc() const
 
 auto StateConfig::threshold_soc(double threshold_soc) -> StateConfig&
 {
+  if (threshold_soc < 0.0 || threshold_soc > 1.0)
+    throw std::invalid_argument(
+      "Battery State of Charge threshold needs be between 0.0 and 1.0.");
+
   _pimpl->threshold_soc = threshold_soc;
   return *this;
 }
diff --git a/rmf_tasks/src/agv/TaskPlanner.cpp b/rmf_tasks/src/agv/TaskPlanner.cpp
index e1a25a692..bb03a2996 100644
--- a/rmf_tasks/src/agv/TaskPlanner.cpp
+++ b/rmf_tasks/src/agv/TaskPlanner.cpp
@@ -260,7 +260,8 @@ Candidates Candidates::make(
   {
     const auto& state = initial_states[i];
     const auto& state_config = state_configs[i];
-    const auto finish = request.estimate_finish(state, state_config);
+    const auto finish =
+      request.estimate_finish(state, state_config);
     if (finish.has_value())
     {
       initial_map.insert({
@@ -273,8 +274,10 @@ Candidates Candidates::make(
         charge_battery_request.estimate_finish(state, state_config);
       if (battery_estimate.has_value())
       {
-        auto new_finish = request.estimate_finish(
-          battery_estimate.value().finish_state(), state_config);
+        auto new_finish =
+          request.estimate_finish(
+            battery_estimate.value().finish_state(),
+            state_config);
         assert(new_finish.has_value());
         initial_map.insert(
           {new_finish.value().finish_state().finish_time(),
@@ -614,6 +617,7 @@ class TaskPlanner::Implementation
   }
   
   Assignments complete_solve(
+    rmf_traffic::Time relative_start_time,
     std::vector<State>& initial_states,
     const std::vector<StateConfig>& state_configs,
     const std::vector<Request::SharedPtr>& requests,
@@ -621,9 +625,8 @@ class TaskPlanner::Implementation
     bool greedy)
   {
     assert(initial_states.size() == state_configs.size());
-
-    const rmf_traffic::Time start_time = std::chrono::steady_clock::now();
-    auto node = make_initial_node(initial_states, state_configs, requests, start_time);
+  
+    auto node = make_initial_node(initial_states, state_configs, requests, relative_start_time);
 
     Node::AssignedTasks complete_assignments;
     complete_assignments.resize(node->assigned_tasks.size());
@@ -631,9 +634,9 @@ class TaskPlanner::Implementation
     while (node)
     {
       if (greedy)
-        node = greedy_solve(node, initial_states, state_configs, start_time);
+        node = greedy_solve(node, initial_states, state_configs, relative_start_time);
       else
-        node = solve(node, initial_states, state_configs, requests.size(), start_time, interrupter);
+        node = solve(node, initial_states, state_configs, requests.size(), relative_start_time, interrupter);
 
       if (!node)
         return {};
@@ -659,7 +662,11 @@ class TaskPlanner::Implementation
 
       // copy final state estimates 
       std::vector<State> estimates;
-      estimates.resize(node->assigned_tasks.size());
+      rmf_traffic::agv::Plan::Start empty_new_start{
+        relative_start_time, 0, 0.0};
+      estimates.resize(
+        node->assigned_tasks.size(),
+        State{empty_new_start, 0, 0.0});
       for (std::size_t i = 0; i < node->assigned_tasks.size(); ++i)
       {
         const auto& assignments = node->assigned_tasks[i];
@@ -669,7 +676,8 @@ class TaskPlanner::Implementation
           estimates[i] = assignments.back().state();        
       }
 
-      node = make_initial_node(estimates, state_configs, new_tasks, start_time);
+      node = make_initial_node(
+        estimates, state_configs, new_tasks, relative_start_time);
       initial_states = estimates;
     }
 
@@ -720,7 +728,7 @@ class TaskPlanner::Implementation
         else
           value =
             rmf_traffic::time::to_seconds(
-              assignments.back().state().finish_time() - relative_start_time);
+              relative_start_time - assignments.back().state().finish_time());
       }
     }
 
@@ -841,8 +849,7 @@ class TaskPlanner::Implementation
     for (auto& new_u : new_node->unassigned_tasks)
     {
       const auto finish =
-        new_u.second.request->estimate_finish(
-          entry.state, state_config);
+        new_u.second.request->estimate_finish(entry.state, state_config);
 
       if (finish.has_value())
       {
@@ -881,7 +888,8 @@ class TaskPlanner::Implementation
 
     if (add_charger)
     {
-      auto battery_estimate = config->charge_battery_request()->estimate_finish(entry.state, state_config);
+      auto battery_estimate =
+        config->charge_battery_request()->estimate_finish(entry.state, state_config);
       if (battery_estimate.has_value())
       {
         new_node->assigned_tasks[entry.candidate].push_back(
@@ -894,11 +902,15 @@ class TaskPlanner::Implementation
         for (auto& new_u : new_node->unassigned_tasks)
         {
           const auto finish =
-            new_u.second.request->estimate_finish(battery_estimate.value().finish_state(), state_config);
+            new_u.second.request->estimate_finish(
+              battery_estimate.value().finish_state(),
+              state_config);
           if (finish.has_value())
           {
             new_u.second.candidates.update_candidate(
-              entry.candidate, finish.value().finish_state(), finish.value().wait_until());
+              entry.candidate,
+              finish.value().finish_state(),
+              finish.value().wait_until());
           }
           else
           {
@@ -962,11 +974,14 @@ class TaskPlanner::Implementation
       {
         const auto finish =
           new_u.second.request->estimate_finish(
-            estimate.value().finish_state(), state_configs[agent]);
+            estimate.value().finish_state(),
+            state_configs[agent]);
         if (finish.has_value())
         {
           new_u.second.candidates.update_candidate(
-            agent, finish.value().finish_state(), finish.value().wait_until());
+            agent,
+            finish.value().finish_state(),
+            finish.value().wait_until());
         }
         else
         {
@@ -1145,11 +1160,13 @@ TaskPlanner::TaskPlanner(std::shared_ptr<Configuration> config)
 }
 
 auto TaskPlanner::greedy_plan(
+  rmf_traffic::Time relative_start_time,
   std::vector<State> initial_states,
   std::vector<StateConfig> state_configs,
   std::vector<Request::SharedPtr> requests) -> Assignments
 {
   return _pimpl->complete_solve(
+    relative_start_time,
     initial_states,
     state_configs,
     requests,
@@ -1158,12 +1175,14 @@ auto TaskPlanner::greedy_plan(
 }
 
 auto TaskPlanner::optimal_plan(
+  rmf_traffic::Time relative_start_time,
   std::vector<State> initial_states,
   std::vector<StateConfig> state_configs,
   std::vector<Request::SharedPtr> requests,
   std::function<bool()> interrupter) -> Assignments
 {
   return _pimpl->complete_solve(
+    relative_start_time,
     initial_states,
     state_configs,
     requests,
@@ -1171,7 +1190,7 @@ auto TaskPlanner::optimal_plan(
     false);
 }
 
-auto TaskPlanner::compute_cost(const Assignments& assignments) -> double
+double TaskPlanner::compute_cost(const Assignments& assignments)
 {
   return _pimpl->compute_g(assignments);
 }
diff --git a/rmf_tasks/src/requests/ChargeBattery.cpp b/rmf_tasks/src/requests/ChargeBattery.cpp
index 1c466b9f7..41e360b02 100644
--- a/rmf_tasks/src/requests/ChargeBattery.cpp
+++ b/rmf_tasks/src/requests/ChargeBattery.cpp
@@ -86,11 +86,14 @@ rmf_utils::optional<rmf_tasks::Estimate> ChargeBattery::estimate_finish(
     return rmf_utils::nullopt;
 
   // Compute time taken to reach charging waypoint from current location
-  agv::State state(
+  rmf_traffic::agv::Plan::Start final_plan_start{
+    initial_state.finish_time(),
     initial_state.charging_waypoint(),
+    initial_state.plan_start().orientation()};
+  agv::State state{
+    std::move(final_plan_start),
     initial_state.charging_waypoint(),
-    initial_state.finish_time(),
-    initial_state.battery_soc());
+    initial_state.battery_soc()};
 
   const auto start_time = initial_state.finish_time();
 
@@ -134,9 +137,11 @@ rmf_utils::optional<rmf_tasks::Estimate> ChargeBattery::estimate_finish(
     _pimpl->_battery_system->charging_current();
 
   const rmf_traffic::Time wait_until = initial_state.finish_time();
-  state.finish_time(
+
+  const rmf_traffic::Time new_finish_time =
     wait_until + variant_duration +
-    rmf_traffic::time::from_seconds(time_to_charge));
+    rmf_traffic::time::from_seconds(time_to_charge);
+  state.finish_time(new_finish_time);
   state.battery_soc(_pimpl->_charge_soc);
 
   return Estimate(state, wait_until);
diff --git a/rmf_tasks/src/requests/Clean.cpp b/rmf_tasks/src/requests/Clean.cpp
index 2d3e6bb59..a7f9aba0c 100644
--- a/rmf_tasks/src/requests/Clean.cpp
+++ b/rmf_tasks/src/requests/Clean.cpp
@@ -55,8 +55,8 @@ rmf_tasks::Request::SharedPtr Clean::make(
   std::shared_ptr<rmf_battery::DevicePowerSink> ambient_sink,
   std::shared_ptr<rmf_battery::DevicePowerSink> cleaning_sink,
   std::shared_ptr<rmf_traffic::agv::Planner> planner,
-  bool drain_battery,
-  rmf_traffic::Time start_time)
+  rmf_traffic::Time start_time,
+  bool drain_battery)
 {
   std::shared_ptr<Clean> clean(new Clean());
   clean->_pimpl->id = id;
@@ -112,11 +112,14 @@ rmf_utils::optional<rmf_tasks::Estimate> Clean::estimate_finish(
   const agv::State& initial_state,
   const agv::StateConfig& state_config) const
 {
-  agv::State state(
-    _pimpl->end_waypoint, 
-    initial_state.charging_waypoint(),
+  rmf_traffic::agv::Plan::Start final_plan_start{
     initial_state.finish_time(),
-    initial_state.battery_soc());
+    _pimpl->end_waypoint,
+    initial_state.plan_start().orientation()};
+  agv::State state{
+    std::move(final_plan_start),
+    initial_state.charging_waypoint(),
+    initial_state.battery_soc()};
 
   rmf_traffic::Duration variant_duration(0);
 
@@ -157,13 +160,14 @@ rmf_utils::optional<rmf_tasks::Estimate> Clean::estimate_finish(
   }
 
   const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration;
-  const rmf_traffic::Time wait_until =
-    initial_state.finish_time() > ideal_start ?
-    initial_state.finish_time() : ideal_start;
+  const rmf_traffic::Time state_finish_time = initial_state.finish_time();
+  const rmf_traffic::Time wait_until = state_finish_time > ideal_start ?
+    state_finish_time : ideal_start;
 
   // Factor in invariants
-  state.finish_time(
-    wait_until + variant_duration + _pimpl->invariant_duration);
+  const rmf_traffic::Time new_finish_time =
+    wait_until + variant_duration + _pimpl->invariant_duration;
+  state.finish_time(new_finish_time);
 
   if (_pimpl->drain_battery)
   {
diff --git a/rmf_tasks/src/requests/Delivery.cpp b/rmf_tasks/src/requests/Delivery.cpp
index 1fbe10d86..3adc6342c 100644
--- a/rmf_tasks/src/requests/Delivery.cpp
+++ b/rmf_tasks/src/requests/Delivery.cpp
@@ -36,8 +36,8 @@ class Delivery::Implementation
   std::shared_ptr<rmf_battery::MotionPowerSink> _motion_sink;
   std::shared_ptr<rmf_battery::DevicePowerSink> _device_sink;
   std::shared_ptr<rmf_traffic::agv::Planner> _planner;
-  bool _drain_battery;
   rmf_traffic::Time _start_time;
+  bool _drain_battery;
 
   rmf_traffic::Duration _invariant_duration;
   double _invariant_battery_drain;
@@ -51,8 +51,8 @@ rmf_tasks::Request::SharedPtr Delivery::make(
   std::shared_ptr<rmf_battery::MotionPowerSink> motion_sink,
   std::shared_ptr<rmf_battery::DevicePowerSink> device_sink,
   std::shared_ptr<rmf_traffic::agv::Planner> planner,
-  bool drain_battery,
-  rmf_traffic::Time start_time)
+  rmf_traffic::Time start_time,
+  bool drain_battery)
 {
   std::shared_ptr<Delivery> delivery(new Delivery());
   delivery->_pimpl->_id = id;
@@ -61,8 +61,8 @@ rmf_tasks::Request::SharedPtr Delivery::make(
   delivery->_pimpl->_motion_sink = std::move(motion_sink);
   delivery->_pimpl->_device_sink = std::move(device_sink);
   delivery->_pimpl->_planner = std::move(planner);
-  delivery->_pimpl->_drain_battery = drain_battery;
   delivery->_pimpl->_start_time = start_time;
+  delivery->_pimpl->_drain_battery = drain_battery;
 
   // Calculate duration of invariant component of task
   const auto plan_start_time = std::chrono::steady_clock::now();
@@ -110,15 +110,19 @@ rmf_utils::optional<rmf_tasks::Estimate> Delivery::estimate_finish(
   const agv::State& initial_state,
   const agv::StateConfig& state_config) const
 {
-  agv::State state(
-    _pimpl->_dropoff_waypoint, 
-    initial_state.charging_waypoint(),
+  rmf_traffic::agv::Plan::Start final_plan_start{
     initial_state.finish_time(),
-    initial_state.battery_soc());
+    _pimpl->_dropoff_waypoint,
+    initial_state.plan_start().orientation()};
+  agv::State state{
+    std::move(final_plan_start),
+    initial_state.charging_waypoint(),
+    initial_state.battery_soc()};
 
   rmf_traffic::Duration variant_duration(0);
 
   const rmf_traffic::Time start_time = initial_state.finish_time();
+
   double battery_soc = initial_state.battery_soc();
   double dSOC_motion = 0.0;
   double dSOC_device = 0.0;
@@ -155,13 +159,14 @@ rmf_utils::optional<rmf_tasks::Estimate> Delivery::estimate_finish(
   }
 
   const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration;
-  const rmf_traffic::Time wait_until =
-    initial_state.finish_time() > ideal_start ?
-    initial_state.finish_time() : ideal_start;
+  const rmf_traffic::Time state_finish_time = initial_state.finish_time();
+  const rmf_traffic::Time wait_until = state_finish_time > ideal_start ?
+    state_finish_time : ideal_start;
 
   // Factor in invariants
-  state.finish_time(
-    wait_until + variant_duration + _pimpl->_invariant_duration);
+  const rmf_traffic::Time new_finish_time =
+    wait_until + variant_duration + _pimpl->_invariant_duration;
+  state.finish_time(new_finish_time);
 
   if (_pimpl->_drain_battery)
   {
diff --git a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp
index 01f377307..b5d9c8bf7 100644
--- a/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp
+++ b/rmf_tasks/test/unit/agv/test_TaskPlanner.cpp
@@ -55,7 +55,8 @@ inline void display_solution(
     {
       const auto& s = a.state();
       const double start_seconds = a.earliest_start_time().time_since_epoch().count()/1e9;
-      const double finish_seconds = s.finish_time().time_since_epoch().count()/1e9;
+      const rmf_traffic::Time finish_time = s.finish_time();
+      const double finish_seconds = finish_time.time_since_epoch().count()/1e9;
       std::cout << "    <" << a.task_id() << ": " << start_seconds 
                 << ", "<< finish_seconds << ", " << 100* s.battery_soc() 
                 << "%>" << std::endl;
@@ -136,11 +137,15 @@ SCENARIO("Grid World")
   WHEN("Planning for 3 requests and 2 agents")
   {
     const auto now = std::chrono::steady_clock::now();
+    const double default_orientation = 0.0;
+
+    rmf_traffic::agv::Plan::Start first_start{now, 13, default_orientation};
+    rmf_traffic::agv::Plan::Start second_start{now, 2, default_orientation};
 
     std::vector<rmf_tasks::agv::State> initial_states =
     {
-      rmf_tasks::agv::State{13, 13},
-      rmf_tasks::agv::State{2, 2}
+      rmf_tasks::agv::State{first_start, 13, 1.0},
+      rmf_tasks::agv::State{second_start, 2, 1.0}
     };
 
     std::vector<rmf_tasks::agv::StateConfig> state_configs =
@@ -158,8 +163,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         2,
@@ -168,8 +173,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         3,
@@ -178,8 +183,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0))
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery)
     };
 
     std::shared_ptr<rmf_tasks::agv::TaskPlanner::Configuration>  task_config =
@@ -187,11 +192,11 @@ SCENARIO("Grid World")
     rmf_tasks::agv::TaskPlanner task_planner(task_config);
 
     const auto greedy_assignments = task_planner.greedy_plan(
-      initial_states, state_configs, requests);
+      now, initial_states, state_configs, requests);
     const double greedy_cost = task_planner.compute_cost(greedy_assignments);
 
     const auto optimal_assignments = task_planner.optimal_plan(
-      initial_states, state_configs, requests, nullptr);
+      now, initial_states, state_configs, requests, nullptr);
     const double optimal_cost = task_planner.compute_cost(optimal_assignments);
     
     display_solution("Greedy", greedy_assignments, greedy_cost);
@@ -203,11 +208,15 @@ SCENARIO("Grid World")
   WHEN("Planning for 11 requests and 2 agents")
   {
     const auto now = std::chrono::steady_clock::now();
+    const double default_orientation = 0.0;
+
+    rmf_traffic::agv::Plan::Start first_start{now, 13, default_orientation};
+    rmf_traffic::agv::Plan::Start second_start{now, 2, default_orientation};
 
     std::vector<rmf_tasks::agv::State> initial_states =
     {
-      rmf_tasks::agv::State{13, 13},
-      rmf_tasks::agv::State{2, 2}
+      rmf_tasks::agv::State{first_start, 13, 1.0},
+      rmf_tasks::agv::State{second_start, 2, 1.0}
     };
 
     std::vector<rmf_tasks::agv::StateConfig> state_configs =
@@ -225,8 +234,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         2,
@@ -235,8 +244,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         3,
@@ -245,8 +254,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         3,
@@ -255,8 +264,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(0)),
+        now + rmf_traffic::time::from_seconds(0),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         4,
@@ -265,8 +274,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(50000)),
+        now + rmf_traffic::time::from_seconds(50000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         5,
@@ -275,8 +284,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(50000)),
+        now + rmf_traffic::time::from_seconds(50000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         6,
@@ -285,8 +294,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         7,
@@ -295,8 +304,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         8,
@@ -305,8 +314,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         9,
@@ -315,8 +324,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         10,
@@ -325,8 +334,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000)),
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery),
 
       rmf_tasks::requests::Delivery::make(
         11,
@@ -335,8 +344,8 @@ SCENARIO("Grid World")
         motion_sink,
         device_sink,
         planner,
-        drain_battery,
-        now + rmf_traffic::time::from_seconds(60000))
+        now + rmf_traffic::time::from_seconds(60000),
+        drain_battery)
     };
 
     std::shared_ptr<rmf_tasks::agv::TaskPlanner::Configuration>  task_config =
@@ -344,11 +353,11 @@ SCENARIO("Grid World")
     rmf_tasks::agv::TaskPlanner task_planner(task_config);
 
     const auto greedy_assignments = task_planner.greedy_plan(
-      initial_states, state_configs, requests);
+      now, initial_states, state_configs, requests);
     const double greedy_cost = task_planner.compute_cost(greedy_assignments);
 
     const auto optimal_assignments = task_planner.optimal_plan(
-      initial_states, state_configs, requests, nullptr);
+      now, initial_states, state_configs, requests, nullptr);
     const double optimal_cost = task_planner.compute_cost(optimal_assignments);
   
     display_solution("Greedy", greedy_assignments, greedy_cost);