diff --git a/avoidance/include/avoidance/common.h b/avoidance/include/avoidance/common.h
index 8a274f54f..2a1e191ef 100644
--- a/avoidance/include/avoidance/common.h
+++ b/avoidance/include/avoidance/common.h
@@ -85,7 +85,7 @@ struct ModelParameters {
   float param_acc_up_max = NAN;   // Maximum vertical acceleration in velocity controlled modes upward
   float param_mpc_z_vel_max_up = NAN;   // Maximum vertical ascent velocity
   float param_mpc_acc_down_max = NAN; // Maximum vertical acceleration in velocity controlled modes down
-  float param_mpc_vel_max_dn = NAN; // Maximum vertical descent velocity
+  float param_mpc_z_vel_max_dn = NAN; // Maximum vertical descent velocity
   float param_mpc_acc_hor = NAN;  // Maximum horizontal acceleration for auto mode and
                       // maximum deceleration for manual mode
   float param_mpc_xy_cruise = NAN;   // Desired horizontal velocity in mission
@@ -352,6 +352,27 @@ pcl::PointCloud<pcl::PointXYZ> removeNaNAndGetMaxima(pcl::PointCloud<pcl::PointX
 *                  bigger FOV than previously thought
 **/
 void updateFOVFromMaxima(FOV& fov, const pcl::PointCloud<pcl::PointXYZ>& maxima);
+/**
+* @brief     compute the maximum speed allowed based on sensor range and vehicle tuning
+* @param     jerk, vehicle maximum jerk
+* @param     accel, vehicle maximum horizontal acceleration
+* @param     braking_distance, maximum sensor range
+* @returns   maximum speed
+**/
+float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance);
+
+/**
+* @brief     compute if the cruise speed requested by paramters or mission item is feasible based on vehicle dynamics
+*and sesnor range
+* @param     jerk, vehicle maximum jerk
+* @param     accel, vehicle maximum horizontal acceleration
+* @param     braking_distance, maximum sensor range
+* @param     mpc_xy_cruise, desired speed set from parameter
+* @param     mission_item_speed, desired speed set from mission item
+* @returns   maximum speed
+**/
+float getMaxSpeed(const float jerk, const float accel, const float braking_distance, const float mpc_xy_cruise,
+                  const float mission_item_speed);
 
 inline Eigen::Vector3f toEigen(const geometry_msgs::Point& p) {
   Eigen::Vector3f ev3(p.x, p.y, p.z);
diff --git a/avoidance/src/avoidance_node.cpp b/avoidance/src/avoidance_node.cpp
index f5c47c468..a2d2e04cc 100644
--- a/avoidance/src/avoidance_node.cpp
+++ b/avoidance/src/avoidance_node.cpp
@@ -113,7 +113,7 @@ void AvoidanceNode::px4ParamsCallback(const mavros_msgs::Param& msg) {
   parse_param_f("MPC_LAND_SPEED", px4_.param_mpc_land_speed) ||
   parse_param_f("MPC_TKO_SPEED", px4_.param_mpc_tko_speed) ||
   parse_param_f("MPC_XY_CRUISE", px4_.param_mpc_xy_cruise) ||
-  parse_param_f("MPC_Z_VEL_MAX_DN", px4_.param_mpc_vel_max_dn) ||
+  parse_param_f("MPC_Z_VEL_MAX_DN", px4_.param_mpc_z_vel_max_dn) ||
   parse_param_f("MPC_Z_VEL_MAX_UP", px4_.param_mpc_z_vel_max_up) ||
   parse_param_f("MPC_COL_PREV_D", px4_.param_mpc_col_prev_d) ||
   parse_param_f("NAV_ACC_RAD", px4_.param_nav_acc_rad);
@@ -134,6 +134,10 @@ void AvoidanceNode::checkPx4Parameters() {
     request_param("MPC_COL_PREV_D", px4_.param_mpc_col_prev_d);
     request_param("MPC_LAND_SPEED", px4_.param_mpc_land_speed);
     request_param("NAV_ACC_RAD", px4_.param_nav_acc_rad);
+    request_param("MPC_Z_VEL_MAX_DN", px4_.param_mpc_z_vel_max_dn);
+    request_param("MPC_Z_VEL_MAX_UP", px4_.param_mpc_z_vel_max_up);
+    request_param("MPC_ACC_HOR", px4_.param_mpc_acc_hor);
+    request_param("MPC_JERK_MAX", px4_.param_mpc_jerk_max);
 
     if (!std::isfinite(px4_.param_mpc_xy_cruise) || !std::isfinite(px4_.param_mpc_col_prev_d) ||
         !std::isfinite(px4_.param_mpc_land_speed) || !std::isfinite(px4_.param_nav_acc_rad)) {
diff --git a/avoidance/src/common.cpp b/avoidance/src/common.cpp
index fdf4e6ab5..b9c49408a 100644
--- a/avoidance/src/common.cpp
+++ b/avoidance/src/common.cpp
@@ -418,4 +418,22 @@ void updateFOVFromMaxima(FOV& fov, const pcl::PointCloud<pcl::PointXYZ>& maxima)
   }
 }
 
+float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance) {
+  // Calculate maximum speed given the sensor range and vehicle parameters
+  // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from
+  // opposite max acceleration)
+  // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
+  float b = 4.f * accel * accel / jerk;
+  float c = -2.f * accel * braking_distance;
+
+  return 0.5f * (-b + sqrtf(b * b - 4.f * c));
+}
+
+float getMaxSpeed(const float jerk, const float accel, const float braking_distance, const float mpc_xy_cruise,
+                  const float mission_item_speed) {
+  float limited_speed = computeMaxSpeedFromBrakingDistance(jerk, accel, braking_distance);
+  float speed = std::isfinite(mission_item_speed) ? mission_item_speed : mpc_xy_cruise;
+  return std::min(speed, limited_speed);
+}
+
 }  // namespace avoidance
diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg
index 033ba6834..d8079ddc1 100755
--- a/local_planner/cfg/local_planner_node.cfg
+++ b/local_planner/cfg/local_planner_node.cfg
@@ -9,11 +9,12 @@ gen = ParameterGenerator()
 
 gen.add("max_sensor_range_",    double_t,    0, "Data points farther away will be discarded", 15.0,  0, 40)
 gen.add("min_sensor_range_", double_t, 0, "Discard points closer than that", 0.2, 0, 10)
-gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 25.0, 0, 30.0)
+gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 50.0, 0, 100.0)
 gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 3.0, 0, 20.0)
-gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 6000, 0.0, 50000.0)
-gen.add("obstacle_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 8.5, 0.0, 30.0)
+gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 3000, 0.0, 50000.0)
+gen.add("obstacle_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 5.0, 0.0, 30.0)
 gen.add("tree_heuristic_weight_", double_t, 0, "Weight for the tree heuristic cost", 35.0, 0.0, 50.0)
+gen.add("tree_step_size_s_", double_t, 0, "Tree time step size in seconds", 0.05, 0.01, 5.0)
 gen.add("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.0)
 gen.add("timeout_startup_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 5, 0, 60)
 gen.add("timeout_critical_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 0.5, 0, 10)
@@ -27,6 +28,6 @@ gen.add("smoothing_margin_degrees_", double_t, 0, "smoothing radius for obstacle
 # star_planner
 gen.add("children_per_node_",    int_t,    0, "Branching factor of the search tree", 8,  0, 100)
 gen.add("n_expanded_nodes_",    int_t,    0, "Number of nodes expanded in complete tree", 40,  0, 200)
-gen.add("tree_node_distance_",    double_t,    0, "Distance between nodes", 2,  0, 20)
+gen.add("tree_node_duration_",    double_t,    0, "Distance in seconds between nodes", 0.5,  0.01, 2)
 
 exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode"))
diff --git a/local_planner/cfg/vehicle.yaml b/local_planner/cfg/vehicle.yaml
index eef20600f..93512c76c 100644
--- a/local_planner/cfg/vehicle.yaml
+++ b/local_planner/cfg/vehicle.yaml
@@ -1,54 +1,53 @@
-# This is a sample config file for a real vehicle
 !!python/object/new:dynamic_reconfigure.encoding.Config
 dictitems:
-  adapt_cost_params_: true
-  box_radius_: 12.0
-  children_per_node_: 50
-  goal_cost_param_: 10.0
+  children_per_node_: 8
   goal_z_param: 3.0
   groups: !!python/object/new:dynamic_reconfigure.encoding.Config
     dictitems:
-      adapt_cost_params_: true
-      box_radius_: 12.0
-      children_per_node_: 50
-      goal_cost_param_: 10.0
+      children_per_node_: 8
       goal_z_param: 3.0
       groups: !!python/object/new:dynamic_reconfigure.encoding.Config
         state: []
-      heading_cost_param_: 0.5
       id: 0
       max_point_age_s_: 20.0
-      min_num_points_per_cell_: 25 # Tune to your sensor requirements!
+      min_num_points_per_cell_: 1
       min_sensor_range_: 0.2
-      n_expanded_nodes_: 10
+      n_expanded_nodes_: 40
       name: Default
-      no_progress_slope_: -0.0007
+      obstacle_cost_param_: 8.5
       parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
         state: []
       parent: 0
-      smooth_cost_param_: 1.5
+      pitch_cost_param_: 25.0
+      sensor_range_: 15.0
       smoothing_margin_degrees_: 40.0
       smoothing_speed_xy_: 10.0
       smoothing_speed_z_: 3.0
       state: true
       timeout_critical_: 0.5
+      timeout_startup_: 5.0
       timeout_termination_: 15.0
-      tree_discount_factor_: 0.8
-      tree_node_distance_: 1.0
+      tree_heuristic_weight_: 35.0
+      tree_node_duration_: 0.5
       type: ''
+      velocity_cost_param_: 6000.0
+      yaw_cost_param_: 3.0
     state: []
-  heading_cost_param_: 0.5
   max_point_age_s_: 20.0
-  min_num_points_per_cell_: 25 # Tune to your sensor requirements!
+  min_num_points_per_cell_: 1
   min_sensor_range_: 0.2
-  n_expanded_nodes_: 10
-  no_progress_slope_: -0.0007
-  smooth_cost_param_: 1.5
+  n_expanded_nodes_: 40
+  obstacle_cost_param_: 5.0
+  pitch_cost_param_: 25.0
+  max_sensor_range_: 15.0
   smoothing_margin_degrees_: 40.0
   smoothing_speed_xy_: 10.0
   smoothing_speed_z_: 3.0
   timeout_critical_: 0.5
+  timeout_startup_: 5.0
   timeout_termination_: 15.0
-  tree_discount_factor_: 0.8
-  tree_node_distance_: 1.0
+  tree_heuristic_weight_: 35.0
+  tree_node_duration_: 0.5
+  velocity_cost_param_: 6000.0
+  yaw_cost_param_: 3.0
 state: []
diff --git a/local_planner/include/local_planner/avoidance_output.h b/local_planner/include/local_planner/avoidance_output.h
index f9309bcfb..b057863cc 100644
--- a/local_planner/include/local_planner/avoidance_output.h
+++ b/local_planner/include/local_planner/avoidance_output.h
@@ -11,16 +11,13 @@ enum waypoint_choice { hover, tryPath, direct, reachHeight };
 
 struct avoidanceOutput {
   waypoint_choice waypoint_type = hover;
-  bool obstacle_ahead;       // true is there is an obstacle ahead of the vehicle
-  float cruise_velocity;     // mission cruise velocity
+  bool obstacle_ahead;    // true is there is an obstacle ahead of the vehicle
+  float cruise_velocity;  // mission cruise velocity
+  float tree_node_duration;
   ros::Time last_path_time;  // finish built time for the VFH+* tree
 
   Eigen::Vector3f take_off_pose;  // last vehicle position when not armed
 
-  std::vector<Eigen::Vector3f> path_node_positions;  // array of tree nodes
-                                                     // position, each node
-                                                     // is the minimum cost
-                                                     // node for each tree
-                                                     // depth level
+  std::vector<Eigen::Vector3f> path_node_setpoints;  // array of setpoints
 };
 }
diff --git a/local_planner/include/local_planner/candidate_direction.h b/local_planner/include/local_planner/candidate_direction.h
index 06e58b592..85e70d967 100644
--- a/local_planner/include/local_planner/candidate_direction.h
+++ b/local_planner/include/local_planner/candidate_direction.h
@@ -1,5 +1,9 @@
 #pragma once
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <cmath>
 #include "avoidance/common.h"
+#include "local_planner/tree_node.h"
 
 namespace avoidance {
 
@@ -7,13 +11,26 @@ struct candidateDirection {
   float cost;
   float elevation_angle;
   float azimuth_angle;
+  TreeNode tree_node;
 
-  candidateDirection(float c, float e, float z) : cost(c), elevation_angle(e), azimuth_angle(z){};
+  candidateDirection(float c, float e, float z) : cost(c), elevation_angle(e), azimuth_angle(z) {
+    simulation_state start_state;
+    start_state.position = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+    start_state.velocity = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+    start_state.acceleration = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+    start_state.time = ros::Time::now().toSec();
+    tree_node = TreeNode(0, start_state, Eigen::Vector3f::Zero());
+  };
 
   bool operator<(const candidateDirection& y) const { return cost < y.cost; }
 
   bool operator>(const candidateDirection& y) const { return cost > y.cost; }
 
   PolarPoint toPolar(float r) const { return PolarPoint(elevation_angle, azimuth_angle, r); }
+  Eigen::Vector3f toEigen() const {
+    return Eigen::Vector3f(std::cos(elevation_angle * DEG_TO_RAD) * std::sin(azimuth_angle * DEG_TO_RAD),
+                           std::cos(elevation_angle * DEG_TO_RAD) * std::cos(azimuth_angle * DEG_TO_RAD),
+                           std::sin(elevation_angle * DEG_TO_RAD));
+  }
 };
 }
diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h
index 7b0c7b764..2a1e3b4cb 100644
--- a/local_planner/include/local_planner/local_planner.h
+++ b/local_planner/include/local_planner/local_planner.h
@@ -7,6 +7,7 @@
 #include "candidate_direction.h"
 #include "cost_parameters.h"
 #include "planner_functions.h"
+#include "trajectory_simulator.h"
 
 #include <dynamic_reconfigure/server.h>
 #include <local_planner/LocalPlannerNodeConfig.h>
@@ -44,6 +45,7 @@ class LocalPlanner {
   float max_point_age_s_ = 10;
   float yaw_fcu_frame_deg_ = 0.0f;
   float pitch_fcu_frame_deg_ = 0.0f;
+  float tree_node_duration_ = 0.5f;
 
   std::vector<FOV> fov_fcu_frame_;
 
@@ -174,11 +176,11 @@ class LocalPlanner {
   /**
   * @brief     getter method to visualize the tree in rviz
   * @param[in] tree, the whole tree built during planning (vector of nodes)
-  * @param[in] closed_set, velocity message coming from the FCU
-  * @param[in] path_node_positions, velocity message coming from the FCU
+  * @param[in] closed_set
+  * @param[in] path_node_setpoints
   **/
   void getTree(std::vector<TreeNode>& tree, std::vector<int>& closed_set,
-               std::vector<Eigen::Vector3f>& path_node_positions) const;
+               std::vector<Eigen::Vector3f>& path_node_setpoints) const;
   /**
   * @brief     getter method for obstacle distance information
   * @param     obstacle_distance, obstacle distance message to fill
diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/local_planner/include/local_planner/local_planner_visualization.h
index dfdc58085..23c268bb8 100644
--- a/local_planner/include/local_planner/local_planner_visualization.h
+++ b/local_planner/include/local_planner/local_planner_visualization.h
@@ -36,11 +36,11 @@ class LocalPlannerVisualization {
   *              chosen
   * @params[in]  tree, the complete calculated search tree
   * @params[in]  closed_set, the closed set (all expanded nodes)
-  * @params[in]  path_node_positions, the positions of all nodes belonging to
+  * @params[in]  path_node_setpoints, the setpoints of all nodes belonging to
   *              the chosen best path
   **/
   void publishTree(const std::vector<TreeNode>& tree, const std::vector<int>& closed_set,
-                   const std::vector<Eigen::Vector3f>& path_node_positions) const;
+                   const std::vector<Eigen::Vector3f>& path_node_setpoints) const;
 
   /**
   * @brief       Visualization of the goal position
@@ -113,6 +113,7 @@ class LocalPlannerVisualization {
   void publishFOV(const std::vector<FOV>& fov, float max_range) const;
 
   void publishRangeScan(const sensor_msgs::LaserScan& scan, const geometry_msgs::PoseStamped& newest_pose) const;
+  std::tuple<float, float, float> HSVtoRGB(std::tuple<float, float, float> hsv) const;
 
  private:
   ros::Publisher local_pointcloud_pub_;
@@ -135,6 +136,7 @@ class LocalPlannerVisualization {
   ros::Publisher deg60_point_pub_;
   ros::Publisher fov_pub_;
   ros::Publisher range_scan_pub_;
+  ros::Publisher tree_cost_pub_;
 
   int path_length_ = 0;
 };
diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h
index a3ca8cd8f..c3e7a48a4 100644
--- a/local_planner/include/local_planner/planner_functions.h
+++ b/local_planner/include/local_planner/planner_functions.h
@@ -143,11 +143,12 @@ void printHistogram(const Histogram& histogram);
 * @brief      Returns a setpoint that lies on the given path
 * @param[in]  vector of nodes defining the path, with the last node of the path at index 0
 * @param[in]  ros time of path generation
-* @param[in]  velocity, scalar value for the norm of the current vehicle velocity
+* @param[in]  tree_node_duration, scalar value for length in time each path segment is to be used
 * @param[out] setpoint on the tree toward which the drone should fly
 * @returns    boolean indicating whether the tree was valid
 **/
-bool getSetpointFromPath(const std::vector<Eigen::Vector3f>& path, const ros::Time& path_generation_time,
-                         float velocity, Eigen::Vector3f& setpoint);
+bool interpolateBetweenSetpoints(const std::vector<Eigen::Vector3f>& setpoint_array,
+                                 const ros::Time& path_generation_time, float tree_node_duration,
+                                 Eigen::Vector3f& setpoint);
 }
 #endif  // LOCAL_PLANNER_FUNCTIONS_H
diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h
index d5fb4ba68..05af19bf5 100644
--- a/local_planner/include/local_planner/star_planner.h
+++ b/local_planner/include/local_planner/star_planner.h
@@ -3,6 +3,7 @@
 
 #include "avoidance/histogram.h"
 #include "cost_parameters.h"
+#include "trajectory_simulator.h"
 
 #include <Eigen/Dense>
 
@@ -14,18 +15,34 @@
 #include <dynamic_reconfigure/server.h>
 #include <local_planner/LocalPlannerNodeConfig.h>
 
+#include <queue>
 #include <vector>
 
 namespace avoidance {
+
+template <typename T, typename Container = std::deque<T>, typename Compare = std::less<typename Container::value_type> >
+class iterable_priority_queue : public std::priority_queue<T, Container, Compare> {
+ public:
+  typedef typename Container::iterator iterator;
+  typedef typename Container::const_iterator const_iterator;
+
+  iterator begin() { return this->c.begin(); }
+  iterator end() { return this->c.end(); }
+  const_iterator begin() const { return this->c.begin(); }
+  const_iterator end() const { return this->c.end(); }
+};
+
 class TreeNode;
 
 class StarPlanner {
-  int children_per_node_ = 1;
-  int n_expanded_nodes_ = 5;
-  float tree_node_distance_ = 1.0f;
-  float max_path_length_ = 4.f;
-  float smoothing_margin_degrees_ = 30.f;
-  float tree_heuristic_weight_ = 10.0f;
+  int children_per_node_ = 8;
+  int n_expanded_nodes_ = 40;
+  float tree_node_duration_ = 0.5f;
+  float max_path_length_ = 15.f;
+  float smoothing_margin_degrees_ = 40.f;
+  float tree_heuristic_weight_ = 35.f;
+  float acceptance_radius_ = 2.f;
+  float tree_step_size_s_ = 0.05f;
 
   pcl::PointCloud<pcl::PointXYZI> cloud_;
 
@@ -33,6 +50,7 @@ class StarPlanner {
   Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN);
   Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN);
   costParameters cost_params_;
+  simulation_limits lims_;
 
  protected:
   /**
@@ -43,7 +61,7 @@ class StarPlanner {
   float treeHeuristicFunction(int node_number) const;
 
  public:
-  std::vector<Eigen::Vector3f> path_node_positions_;
+  std::vector<Eigen::Vector3f> path_node_setpoints_;
   std::vector<int> closed_set_;
   std::vector<TreeNode> tree_;
 
@@ -53,8 +71,9 @@ class StarPlanner {
   /**
   * @brief     setter method for costMatrix paramters
   * @param[in] cost_params, parameters for the histogram cost function
+  * @param[in] simulation limits defining maximum acceleration, velocity, and jerk
   **/
-  void setParams(costParameters cost_params);
+  void setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad);
 
   /**
   * @brief     setter method for star_planner pointcloud
diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/local_planner/include/local_planner/trajectory_simulator.h
index 138c8118b..ecded1e62 100644
--- a/local_planner/include/local_planner/trajectory_simulator.h
+++ b/local_planner/include/local_planner/trajectory_simulator.h
@@ -6,13 +6,24 @@
 namespace avoidance {
 
 struct simulation_state {
-  float time = NAN;
-  Eigen::Vector3f position = NAN * Eigen::Vector3f::Ones();
-  Eigen::Vector3f velocity = NAN * Eigen::Vector3f::Ones();
-  Eigen::Vector3f acceleration = NAN * Eigen::Vector3f::Ones();
+  simulation_state(float t = NAN, const Eigen::Vector3f& p = NAN * Eigen::Vector3f::Zero(),
+                   const Eigen::Vector3f& v = NAN * Eigen::Vector3f::Zero(),
+                   const Eigen::Vector3f& a = NAN * Eigen::Vector3f::Zero())
+      : time(t), position(p), velocity(v), acceleration(a){};
+  float time;
+  Eigen::Vector3f position;
+  Eigen::Vector3f velocity;
+  Eigen::Vector3f acceleration;
 };
 
 struct simulation_limits {
+  simulation_limits(float max_z, float min_z, float max_xy_n, float max_acc_n, float max_jerk_n)
+      : max_z_velocity(max_z),
+        min_z_velocity(min_z),
+        max_xy_velocity_norm(max_xy_n),
+        max_acceleration_norm(max_acc_n),
+        max_jerk_norm(max_jerk_n){};
+  simulation_limits(){};
   float max_z_velocity = NAN;
   float min_z_velocity = NAN;
   float max_xy_velocity_norm = NAN;
@@ -25,6 +36,7 @@ class TrajectorySimulator {
   TrajectorySimulator(const simulation_limits& config, const simulation_state& start, float step_time = 0.1f);
 
   std::vector<simulation_state> generate_trajectory(const Eigen::Vector3f& goal_direction, float simulation_duration);
+  simulation_state generate_trajectory_endpoint(const Eigen::Vector3f& goal_direction, float simulation_duration);
 
  protected:
   const simulation_limits config_;
@@ -37,6 +49,10 @@ class TrajectorySimulator {
   static Eigen::Vector3f jerk_for_velocity_setpoint(float P_constant, float D_constant, float max_jerk_norm,
                                                     const Eigen::Vector3f& desired_velocity,
                                                     const simulation_state& state);
+
+ private:
+  simulation_state generate_trajectory(const Eigen::Vector3f& goal_direction, int num_steps,
+                                       std::vector<simulation_state>* timepoints);
 };
 
 // templated helper function
diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h
index 4f71a42d0..78e8225bb 100644
--- a/local_planner/include/local_planner/tree_node.h
+++ b/local_planner/include/local_planner/tree_node.h
@@ -3,21 +3,21 @@
 
 #include <Eigen/Core>
 #include <vector>
+#include "trajectory_simulator.h"
 
 namespace avoidance {
 
 class TreeNode {
-  Eigen::Vector3f position_;
-  Eigen::Vector3f velocity_;
-
  public:
   float total_cost_;
   float heuristic_;
   int origin_;
   bool closed_;
+  simulation_state state;    // State containing position, velocity and time of the drone
+  Eigen::Vector3f setpoint;  // Setpoint required to send to PX4 in order to get to this state
 
-  TreeNode();
-  TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel);
+  TreeNode() = default;
+  TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp);
   ~TreeNode() = default;
 
   /**
@@ -28,11 +28,15 @@ class TreeNode {
   void setCosts(float h, float c);
 
   /**
-  * @brief     getter method for tree node position
+  * @defgroup  getterFunctions
+  * @brief     getter methods for tree node position, velocity and setpoint
   * @returns   node position in 3D cartesian coordinates
+  * @{
   **/
   Eigen::Vector3f getPosition() const;
   Eigen::Vector3f getVelocity() const;
+  Eigen::Vector3f getSetpoint() const;
+  /** @} */  // end of doxygen group getterFunctions
 };
 }
 
diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp
index 0d56369da..5539093af 100644
--- a/local_planner/src/nodes/local_planner.cpp
+++ b/local_planner/src/nodes/local_planner.cpp
@@ -37,6 +37,7 @@ void LocalPlanner::dynamicReconfigureSetParams(avoidance::LocalPlannerNodeConfig
   children_per_node_ = config.children_per_node_;
   n_expanded_nodes_ = config.n_expanded_nodes_;
   smoothing_margin_degrees_ = static_cast<float>(config.smoothing_margin_degrees_);
+  tree_node_duration_ = static_cast<float>(config.tree_node_duration_);
 
   if (getGoal().z() != config.goal_z_param) {
     auto goal = getGoal();
@@ -124,7 +125,17 @@ void LocalPlanner::determineStrategy() {
     getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, cost_matrix_,
                   cost_image_data_);
 
-    star_planner_->setParams(cost_params_);
+    simulation_limits lims;
+    lims.max_z_velocity = px4_.param_mpc_z_vel_max_up;
+    lims.min_z_velocity = -1.0f * px4_.param_mpc_z_vel_max_dn;
+    lims.max_xy_velocity_norm =
+        std::min(getMaxSpeed(px4_.param_mpc_jerk_max, px4_.param_mpc_acc_hor, max_sensor_range_,
+                             px4_.param_mpc_xy_cruise, mission_item_speed_),
+                 getMaxSpeed(px4_.param_mpc_jerk_max, px4_.param_mpc_acc_hor, (goal_ - position_).norm(),
+                             px4_.param_mpc_xy_cruise, mission_item_speed_));
+    lims.max_acceleration_norm = px4_.param_mpc_acc_hor;
+    lims.max_jerk_norm = px4_.param_mpc_jerk_max;
+    star_planner_->setParams(cost_params_, lims, px4_.param_nav_acc_rad);
     star_planner_->setPointcloud(final_cloud_);
 
     // build search tree
@@ -180,19 +191,20 @@ void LocalPlanner::setDefaultPx4Parameters() {
   px4_.param_acc_up_max = 10.f;
   px4_.param_mpc_z_vel_max_up = 3.f;
   px4_.param_mpc_acc_down_max = 10.f;
-  px4_.param_mpc_vel_max_dn = 1.f;
+  px4_.param_mpc_z_vel_max_dn = 1.f;
   px4_.param_mpc_acc_hor = 5.f;
   px4_.param_mpc_xy_cruise = 3.f;
   px4_.param_mpc_tko_speed = 1.f;
   px4_.param_mpc_land_speed = 0.7f;
   px4_.param_mpc_col_prev_d = 4.f;
+  px4_.param_nav_acc_rad = 2.f;
 }
 
 void LocalPlanner::getTree(std::vector<TreeNode>& tree, std::vector<int>& closed_set,
-                           std::vector<Eigen::Vector3f>& path_node_positions) const {
+                           std::vector<Eigen::Vector3f>& path_node_setpoints) const {
   tree = star_planner_->tree_;
   closed_set = star_planner_->closed_set_;
-  path_node_positions = star_planner_->path_node_positions_;
+  path_node_setpoints = star_planner_->path_node_setpoints_;
 }
 
 void LocalPlanner::getObstacleDistanceData(sensor_msgs::LaserScan& obstacle_distance) {
@@ -204,24 +216,13 @@ avoidanceOutput LocalPlanner::getAvoidanceOutput() const {
   out.waypoint_type = waypoint_type_;
   out.obstacle_ahead = !polar_histogram_.isEmpty();
 
-  // calculate maximum speed given the sensor range and vehicle parameters
-  // quadratic solve of 0 = u^2 + 2as, with s = u * |a/j| + r
-  // u = initial velocity, a = max acceleration
-  // s = stopping distance under constant acceleration
-  // j = maximum jerk, r = maximum range sensor distance
-  float accel_ramp_time = px4_.param_mpc_acc_hor / px4_.param_mpc_jerk_max;
-  float a = 1;
-  float b = 2 * px4_.param_mpc_acc_hor * accel_ramp_time;
-  float c = 2 * -px4_.param_mpc_acc_hor * max_sensor_range_;
-  float limited_speed = (-b + std::sqrt(b * b - 4 * a * c)) / (2 * a);
-
-  float speed = std::isfinite(mission_item_speed_) ? mission_item_speed_ : px4_.param_mpc_xy_cruise;
-  float max_speed = std::min(speed, limited_speed);
+  float max_speed = getMaxSpeed(px4_.param_mpc_jerk_max, px4_.param_mpc_acc_hor, max_sensor_range_,
+                                px4_.param_mpc_xy_cruise, mission_item_speed_);
 
   out.cruise_velocity = max_speed;
   out.last_path_time = last_path_time_;
-
-  out.path_node_positions = star_planner_->path_node_positions_;
+  out.tree_node_duration = tree_node_duration_;
+  out.path_node_setpoints = star_planner_->path_node_setpoints_;
   return out;
 }
 }
diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp
index 37ff83d38..47942fddc 100644
--- a/local_planner/src/nodes/local_planner_visualization.cpp
+++ b/local_planner/src/nodes/local_planner_visualization.cpp
@@ -31,6 +31,7 @@ void LocalPlannerVisualization::initializePublishers(ros::NodeHandle& nh) {
   deg60_point_pub_ = nh.advertise<visualization_msgs::Marker>("/deg60_point", 1);
   fov_pub_ = nh.advertise<visualization_msgs::Marker>("/fov", 4);
   range_scan_pub_ = nh.advertise<visualization_msgs::Marker>("/range_scan", 1);
+  tree_cost_pub_ = nh.advertise<visualization_msgs::MarkerArray>("/tree_cost", 1);
 }
 
 void LocalPlannerVisualization::visualizePlannerData(const LocalPlanner& planner,
@@ -44,9 +45,9 @@ void LocalPlannerVisualization::visualizePlannerData(const LocalPlanner& planner
   // visualize tree calculation
   std::vector<TreeNode> tree;
   std::vector<int> closed_set;
-  std::vector<Eigen::Vector3f> path_node_positions;
-  planner.getTree(tree, closed_set, path_node_positions);
-  publishTree(tree, closed_set, path_node_positions);
+  std::vector<Eigen::Vector3f> path_node_setpoints;
+  planner.getTree(tree, closed_set, path_node_setpoints);
+  publishTree(tree, closed_set, path_node_setpoints);
 
   // visualize goal
   publishGoal(toPoint(planner.getGoal()));
@@ -190,7 +191,7 @@ void LocalPlannerVisualization::publishOfftrackPoints(Eigen::Vector3f& closest_p
 }
 
 void LocalPlannerVisualization::publishTree(const std::vector<TreeNode>& tree, const std::vector<int>& closed_set,
-                                            const std::vector<Eigen::Vector3f>& path_node_positions) const {
+                                            const std::vector<Eigen::Vector3f>& path_node_setpoints) const {
   visualization_msgs::Marker tree_marker;
   tree_marker.header.frame_id = "local_origin";
   tree_marker.header.stamp = ros::Time::now();
@@ -227,16 +228,110 @@ void LocalPlannerVisualization::publishTree(const std::vector<TreeNode>& tree, c
     tree_marker.points.push_back(p2);
   }
 
-  path_marker.points.reserve(path_node_positions.size() * 2);
-  for (size_t i = 1; i < path_node_positions.size(); i++) {
-    path_marker.points.push_back(toPoint(path_node_positions[i - 1]));
-    path_marker.points.push_back(toPoint(path_node_positions[i]));
+  // Visualizing the setpoints is a hack: they are actually in the body frame. This is an approximate visualization
+  // that just accumulates them to illustrate them as a path
+  if (path_node_setpoints.size() > 0) {
+    path_marker.points.reserve(path_node_setpoints.size() * 2);
+    Eigen::Vector3f p1;
+    Eigen::Vector3f p2 = tree[closed_set.front()].getPosition();
+    for (int i = path_node_setpoints.size() - 1; i >= 1; --i) {
+      float scale = (tree[closed_set[i]].getPosition() - tree[closed_set[i - 1]].getPosition()).norm();
+      p1 = p2;
+      p2 = p1 + scale * path_node_setpoints[i];
+      path_marker.points.push_back(toPoint(p1));
+      path_marker.points.push_back(toPoint(p2));
+    }
   }
 
+  visualization_msgs::MarkerArray marker_array;
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "local_origin";
+  marker.header.stamp = ros::Time::now();
+  marker.id = 0;
+  marker.type = visualization_msgs::Marker::SPHERE;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 0.2;
+  marker.scale.y = 0.2;
+  marker.scale.z = 0.2;
+  marker.color.a = 1.0;
+  marker.color.r = 0.0;
+  marker.color.g = 0.0;
+
+  float variance_max_value = -1.f;
+  float variance_min_value = FLT_MAX;
+  float range_max = 120.f;
+  float range_min = 0.f;
+
+  for (size_t i = 0; i < closed_set.size(); i++) {
+    int node_nr = closed_set[i];
+    variance_max_value = std::max(variance_max_value, tree[node_nr].total_cost_);
+    variance_min_value = std::min(variance_min_value, tree[node_nr].total_cost_);
+  }
+  for (size_t i = 0; i < closed_set.size(); i++) {
+    int node_nr = closed_set[i];
+    marker.pose.position = toPoint(tree[node_nr].getPosition());
+    float cost = tree[node_nr].total_cost_;
+    float heuristic = tree[i].heuristic_;
+
+    float h =
+        ((range_max - range_min) * (cost - variance_min_value) / (variance_max_value - variance_min_value)) + range_min;
+    float red, green, blue;
+    float max_aa = 1.f;
+    std::tuple<float, float, float> test(h, 1.f, 1.f);
+    std::tie(marker.color.r, marker.color.g, marker.color.b) = HSVtoRGB(test);
+    marker_array.markers.push_back(marker);
+    marker.id += 1;
+  }
+
+  tree_cost_pub_.publish(marker_array);
+
   complete_tree_pub_.publish(tree_marker);
   tree_path_pub_.publish(path_marker);
 }
+std::tuple<float, float, float> LocalPlannerVisualization::HSVtoRGB(std::tuple<float, float, float> hsv) const {
+  std::tuple<float, float, float> rgb;
+  float fC = std::get<2>(hsv) * std::get<1>(hsv);  // fV * fS;  // Chroma
+  float fHPrime = fmod(std::get<0>(hsv) / 60.0, 6);
+  float fX = fC * (1 - fabs(fmod(fHPrime, 2) - 1));
+  float fM = std::get<2>(hsv) - fC;
+
+  if (0 <= fHPrime && fHPrime < 1) {
+    std::get<0>(rgb) = fC;
+    std::get<1>(rgb) = fX;
+    std::get<2>(rgb) = 0;
+  } else if (1 <= fHPrime && fHPrime < 2) {
+    std::get<0>(rgb) = fX;
+    std::get<1>(rgb) = fC;
+    std::get<2>(rgb) = 0;
+  } else if (2 <= fHPrime && fHPrime < 3) {
+    std::get<0>(rgb) = 0;
+    std::get<1>(rgb) = fC;
+    std::get<2>(rgb) = fX;
+  } else if (3 <= fHPrime && fHPrime < 4) {
+    std::get<0>(rgb) = 0;
+    std::get<1>(rgb) = fX;
+    std::get<2>(rgb) = fC;
+  } else if (4 <= fHPrime && fHPrime < 5) {
+    std::get<0>(rgb) = fX;
+    std::get<1>(rgb) = 0;
+    std::get<2>(rgb) = fC;
+  } else if (5 <= fHPrime && fHPrime < 6) {
+    std::get<0>(rgb) = fC;
+    std::get<1>(rgb) = 0;
+    std::get<2>(rgb) = fX;
+  } else {
+    std::get<0>(rgb) = 0;
+    std::get<1>(rgb) = 0;
+    std::get<2>(rgb) = 0;
+  }
+
+  std::get<0>(rgb) += fM;
+  std::get<1>(rgb) += fM;
+  std::get<2>(rgb) += fM;
 
+  return rgb;
+}
 void LocalPlannerVisualization::publishGoal(const geometry_msgs::Point& goal) const {
   visualization_msgs::MarkerArray marker_goal;
   visualization_msgs::Marker m;
diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp
index 93212f950..26d175320 100644
--- a/local_planner/src/nodes/planner_functions.cpp
+++ b/local_planner/src/nodes/planner_functions.cpp
@@ -323,33 +323,36 @@ std::pair<float, float> costFunction(const PolarPoint& candidate_polar, float ob
   return std::pair<float, float>(distance_cost, velocity_cost + yaw_cost + pitch_cost);
 }
 
-bool getSetpointFromPath(const std::vector<Eigen::Vector3f>& path, const ros::Time& path_generation_time,
-                         float velocity, Eigen::Vector3f& setpoint) {
-  int i = path.size();
+bool interpolateBetweenSetpoints(const std::vector<Eigen::Vector3f>& setpoint_array,
+                                 const ros::Time& path_generation_time, float tree_node_duration,
+                                 Eigen::Vector3f& setpoint) {
+  int i = setpoint_array.size();
   // path contains nothing meaningful
   if (i < 2) {
+    ROS_WARN("Path contains fewer than two nodes, this is not a path!");
     return false;
   }
 
   // path only has one segment: return end of that segment as setpoint
   if (i == 2) {
-    setpoint = path[0];
+    ROS_INFO("Path contains only two nodes, using second node as setpoint!");
+    setpoint = setpoint_array[0];
     return true;
   }
 
   // step through the path until the point where we should be if we had traveled perfectly with velocity along it
-  Eigen::Vector3f path_segment = path[i - 3] - path[i - 2];
-  float distance_left = (ros::Time::now() - path_generation_time).toSec() * velocity;
-  setpoint = path[i - 2] + (distance_left / path_segment.norm()) * path_segment;
-
-  for (i = path.size() - 3; i > 0 && distance_left > path_segment.norm(); --i) {
-    distance_left -= path_segment.norm();
-    path_segment = path[i - 1] - path[i];
-    setpoint = path[i] + (distance_left / path_segment.norm()) * path_segment;
+  const int seg = i - 2 - std::floor((ros::Time::now() - path_generation_time).toSec() / tree_node_duration);
+
+  // path expired
+  if (seg < 1) {
+    ROS_WARN("Path has expired!");
+    return false;
   }
 
-  // If we excited because we're past the last node of the path, the path is no longer valid!
-  return distance_left < path_segment.norm();
+  Eigen::Vector3f path_segment = setpoint_array[seg - 1] - setpoint_array[seg];
+  float distance_left = fmod((ros::Time::now() - path_generation_time).toSec(), tree_node_duration);
+  setpoint = setpoint_array[seg] + (distance_left / tree_node_duration) * path_segment;
+  return true;
 }
 
 void printHistogram(Histogram& histogram) {
diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp
index ec1ce3811..ff6170d01 100644
--- a/local_planner/src/nodes/star_planner.cpp
+++ b/local_planner/src/nodes/star_planner.cpp
@@ -14,13 +14,18 @@ StarPlanner::StarPlanner() {}
 void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerNodeConfig& config, uint32_t level) {
   children_per_node_ = config.children_per_node_;
   n_expanded_nodes_ = config.n_expanded_nodes_;
-  tree_node_distance_ = static_cast<float>(config.tree_node_distance_);
+  tree_node_duration_ = static_cast<float>(config.tree_node_duration_);
   max_path_length_ = static_cast<float>(config.max_sensor_range_);
   smoothing_margin_degrees_ = static_cast<float>(config.smoothing_margin_degrees_);
   tree_heuristic_weight_ = static_cast<float>(config.tree_heuristic_weight_);
+  tree_step_size_s_ = static_cast<float>(config.tree_step_size_s_);
 }
 
-void StarPlanner::setParams(costParameters cost_params) { cost_params_ = cost_params; }
+void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad) {
+  cost_params_ = cost_params;
+  lims_ = limits;
+  acceptance_radius_ = acc_rad;
+}
 
 void StarPlanner::setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel) {
   position_ = pos;
@@ -37,7 +42,6 @@ float StarPlanner::treeHeuristicFunction(int node_number) const {
 
 void StarPlanner::buildLookAheadTree() {
   std::clock_t start_time = std::clock();
-
   Histogram histogram(ALPHA_RES);
   std::vector<uint8_t> cost_image_data;
   std::vector<candidateDirection> candidate_vector;
@@ -49,7 +53,12 @@ void StarPlanner::buildLookAheadTree() {
   closed_set_.clear();
 
   // insert first node
-  tree_.push_back(TreeNode(0, position_, velocity_));
+  simulation_state start_state;
+  start_state.position = position_;
+  start_state.velocity = velocity_;
+  start_state.acceleration = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+  start_state.time = ros::Time::now().toSec();
+  tree_.push_back(TreeNode(0, start_state, Eigen::Vector3f::Zero()));
   tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0));
 
   int origin = 0;
@@ -68,39 +77,56 @@ void StarPlanner::buildLookAheadTree() {
     candidate_vector.clear();
     getCostMatrix(histogram, goal_, origin_position, origin_velocity, cost_params_, smoothing_margin_degrees_,
                   cost_matrix, cost_image_data);
-    getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector);
-
-    // add candidates as nodes
-    if (candidate_vector.empty()) {
-      tree_[origin].total_cost_ = HUGE_VAL;
-    } else {
-      // insert new nodes
-      int children = 0;
-      for (candidateDirection candidate : candidate_vector) {
-        PolarPoint candidate_polar = candidate.toPolar(tree_node_distance_);
-        Eigen::Vector3f node_location = polarHistogramToCartesian(candidate_polar, origin_position);
-        Eigen::Vector3f node_velocity = node_location - origin_position;  // todo: simulate!
-
-        // check if another close node has been added
+
+    simulation_limits limits = lims_;
+    simulation_state state = tree_[origin].state;
+    limits.max_xy_velocity_norm =
+        std::min(computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm,
+                                                    (state.position - goal_).head<2>().norm()),
+                 lims_.max_xy_velocity_norm);
+    TrajectorySimulator sim(limits, state, tree_step_size_s_);
+    iterable_priority_queue<candidateDirection, std::vector<candidateDirection>, std::less<candidateDirection>> queue;
+    for (int row_index = 0; row_index < cost_matrix.rows(); row_index++) {
+      for (int col_index = 0; col_index < cost_matrix.cols(); col_index++) {
+        PolarPoint p_pol = histogramIndexToPolar(row_index, col_index, ALPHA_RES, 1.0);
+        float cost = cost_matrix(row_index, col_index);
+        candidateDirection candidate(cost, p_pol.e, p_pol.z);
+        simulation_state trajectory_endpoint =
+            sim.generate_trajectory_endpoint(candidate.toEigen(), tree_node_duration_);
         int close_nodes = 0;
-        for (size_t i = 0; i < tree_.size(); i++) {
-          float dist = (tree_[i].getPosition() - node_location).norm();
+        for (auto it = queue.begin(); it != queue.end(); it++) {
+          float dist = ((*it).tree_node.getPosition() - trajectory_endpoint.position).norm();
           if (dist < 0.2f) {
             close_nodes++;
             break;
           }
         }
 
-        if (children < children_per_node_ && close_nodes == 0) {
-          tree_.push_back(TreeNode(origin, node_location, node_velocity));
-          float h = treeHeuristicFunction(tree_.size() - 1);
-          tree_.back().heuristic_ = h;
-          tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + candidate.cost + h;
-          children++;
+        if (queue.size() < children_per_node_) {
+          candidate.tree_node = TreeNode(origin, trajectory_endpoint, candidate.toEigen());
+          queue.push(candidate);
+        } else if (candidate < queue.top() && close_nodes == 0) {
+          candidate.tree_node = TreeNode(origin, trajectory_endpoint, candidate.toEigen());
+          queue.push(candidate);
+          queue.pop();
         }
       }
     }
 
+    int children = 0;
+    while (!queue.empty()) {
+      if (children < children_per_node_) {
+        tree_.push_back(queue.top().tree_node);
+        float h = treeHeuristicFunction(tree_.size() - 1);
+        tree_.back().heuristic_ = h;
+        tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + queue.top().cost + h;
+        queue.pop();
+        children++;
+      } else {
+        break;
+      }
+    }
+
     closed_set_.push_back(origin);
     tree_[origin].closed_ = true;
 
@@ -109,6 +135,14 @@ void StarPlanner::buildLookAheadTree() {
     is_expanded_node = false;
     for (size_t i = 0; i < tree_.size(); i++) {
       if (!(tree_[i].closed_)) {
+        // If we reach the acceptance radius, add goal as last node and exit
+        if (i > 1 && (tree_[i].getPosition() - goal_).norm() < acceptance_radius_) {
+          tree_.push_back(TreeNode(i, simulation_state(0.f, goal_), goal_ - tree_[i].getPosition()));
+          closed_set_.push_back(i);
+          closed_set_.push_back(tree_.size() - 1);
+          break;
+        }
+
         float node_distance = (tree_[i].getPosition() - position_).norm();
         if (tree_[i].total_cost_ < minimal_cost && node_distance < max_path_length_) {
           minimal_cost = tree_[i].total_cost_;
@@ -121,24 +155,14 @@ void StarPlanner::buildLookAheadTree() {
     cost_image_data.clear();
     candidate_vector.clear();
   }
-  // smoothing between trees
+
+  // Get setpoints into member vector
   int tree_end = origin;
-  path_node_positions_.clear();
+  path_node_setpoints_.clear();
   while (tree_end > 0) {
-    path_node_positions_.push_back(tree_[tree_end].getPosition());
+    path_node_setpoints_.push_back(tree_[tree_end].getSetpoint());
     tree_end = tree_[tree_end].origin_;
   }
-  path_node_positions_.push_back(tree_[0].getPosition());
-
-  ROS_INFO("\033[0;35m[SP]Tree (%lu nodes, %lu path nodes, %lu expanded) calculated in %2.2fms.\033[0m", tree_.size(),
-           path_node_positions_.size(), closed_set_.size(),
-           static_cast<double>((std::clock() - start_time) / static_cast<double>(CLOCKS_PER_SEC / 1000)));
-
-#ifndef DISABLE_SIMULATION  // For large trees, this could be very slow!
-  for (int j = 0; j < path_node_positions_.size(); j++) {
-    ROS_DEBUG("\033[0;35m[SP] node %i : [ %f, %f, %f]\033[0m", j, path_node_positions_[j].x(),
-              path_node_positions_[j].y(), path_node_positions_[j].z());
-  }
-#endif
+  path_node_setpoints_.push_back(tree_[0].getSetpoint());
 }
 }
diff --git a/local_planner/src/nodes/tree_node.cpp b/local_planner/src/nodes/tree_node.cpp
index 7e1577804..329579854 100644
--- a/local_planner/src/nodes/tree_node.cpp
+++ b/local_planner/src/nodes/tree_node.cpp
@@ -2,22 +2,15 @@
 
 namespace avoidance {
 
-TreeNode::TreeNode() : total_cost_{0.0f}, heuristic_{0.0f}, origin_{0}, closed_{false} {
-  position_ = Eigen::Vector3f::Zero();
-  velocity_ = Eigen::Vector3f::Zero();
-}
-
-TreeNode::TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel)
-    : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false} {
-  position_ = pos;
-  velocity_ = vel;
-}
+TreeNode::TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp)
+    : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false}, state(start_state), setpoint(sp) {}
 
 void TreeNode::setCosts(float h, float c) {
   heuristic_ = h;
   total_cost_ = c;
 }
 
-Eigen::Vector3f TreeNode::getPosition() const { return position_; }
-Eigen::Vector3f TreeNode::getVelocity() const { return velocity_; }
+Eigen::Vector3f TreeNode::getPosition() const { return state.position; }
+Eigen::Vector3f TreeNode::getVelocity() const { return state.velocity; }
+Eigen::Vector3f TreeNode::getSetpoint() const { return setpoint; }
 }
diff --git a/local_planner/src/nodes/waypoint_generator.cpp b/local_planner/src/nodes/waypoint_generator.cpp
index c945fd32e..96ccc0fb5 100644
--- a/local_planner/src/nodes/waypoint_generator.cpp
+++ b/local_planner/src/nodes/waypoint_generator.cpp
@@ -34,10 +34,10 @@ void WaypointGenerator::calculateWaypoint() {
     }
 
     case tryPath: {
-      Eigen::Vector3f setpoint = position_;
-      if (getSetpointFromPath(planner_info_.path_node_positions, planner_info_.last_path_time,
-                              planner_info_.cruise_velocity, setpoint)) {
-        output_.goto_position = position_ + (setpoint - position_).normalized();
+      Eigen::Vector3f setpoint = Eigen::Vector3f::Zero();
+      if (interpolateBetweenSetpoints(planner_info_.path_node_setpoints, planner_info_.last_path_time,
+                                      planner_info_.tree_node_duration, setpoint)) {
+        output_.goto_position = position_ + setpoint;
         ROS_DEBUG("[WG] Using calculated tree\n");
       } else {
         ROS_DEBUG("[WG] No valid tree, going straight");
diff --git a/local_planner/src/utils/trajectory_simulator.cpp b/local_planner/src/utils/trajectory_simulator.cpp
index 46ce756ee..e094c64bd 100644
--- a/local_planner/src/utils/trajectory_simulator.cpp
+++ b/local_planner/src/utils/trajectory_simulator.cpp
@@ -20,20 +20,32 @@ TrajectorySimulator::TrajectorySimulator(const avoidance::simulation_limits& con
                                          const avoidance::simulation_state& start, float step_time)
     : config_(config), start_(start), step_time_(step_time) {}
 
+simulation_state TrajectorySimulator::generate_trajectory_endpoint(const Eigen::Vector3f& goal_direction,
+                                                                   float simulation_duration) {
+  int num_steps = static_cast<int>(std::ceil(simulation_duration / step_time_));
+  return generate_trajectory(goal_direction, num_steps, nullptr);
+}
+
 std::vector<simulation_state> TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direction,
                                                                        float simulation_duration) {
   int num_steps = static_cast<int>(std::ceil(simulation_duration / step_time_));
   std::vector<simulation_state> timepoints;
   timepoints.reserve(num_steps);
 
+  generate_trajectory(goal_direction, num_steps, &timepoints);
+
+  return timepoints;
+}
+
+simulation_state TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direction, int num_steps,
+                                                          std::vector<simulation_state>* timepoints) {
   const Eigen::Vector3f unit_goal = goal_direction.normalized();
   const Eigen::Vector3f desired_velocity =
       xy_norm_z_clamp(unit_goal * std::hypot(config_.max_xy_velocity_norm,
                                              unit_goal.z() > 0 ? config_.max_z_velocity : config_.min_z_velocity),
                       config_.max_xy_velocity_norm, config_.min_z_velocity, config_.max_z_velocity);
 
-  // calculate P and D constants such that they hit the jerk limit when
-  // doing accel from 0
+  // calculate P and D constants such that they hit the jerk limit when doing accel from 0
   float max_accel_norm = std::min(2 * std::sqrt(config_.max_jerk_norm), config_.max_acceleration_norm);
   float P_constant =
       (std::sqrt(sqr(max_accel_norm) + config_.max_jerk_norm * desired_velocity.norm()) - max_accel_norm) /
@@ -46,8 +58,7 @@ std::vector<simulation_state> TrajectorySimulator::generate_trajectory(const Eig
     const Eigen::Vector3f damped_jerk =
         jerk_for_velocity_setpoint(P_constant, D_constant, config_.max_jerk_norm, desired_velocity, run_state);
 
-    // limit time step to not exceed the maximum acceleration, but clamp
-    // jerk to 0 if at maximum acceleration already
+    // limit time step to not exceed the maximum acceleration, but clamp jerk to 0 if at maximum acceleration already
     const Eigen::Vector3f requested_accel = run_state.acceleration + single_step_time * damped_jerk;
     Eigen::Vector3f jerk = damped_jerk;
     if (requested_accel.squaredNorm() > sqr(max_accel_norm)) {
@@ -61,10 +72,11 @@ std::vector<simulation_state> TrajectorySimulator::generate_trajectory(const Eig
 
     // update the state based on motion equations with the final jerk
     run_state = simulate_step_constant_jerk(run_state, jerk, single_step_time);
-    timepoints.push_back(run_state);
+    if (timepoints != nullptr) {
+      timepoints->push_back(run_state);
+    }
   }
-
-  return timepoints;
+  return run_state;
 }
 
 simulation_state TrajectorySimulator::simulate_step_constant_jerk(const simulation_state& state,
diff --git a/local_planner/test/test_local_planner.cpp b/local_planner/test/test_local_planner.cpp
index afa5143f3..35718b129 100644
--- a/local_planner/test/test_local_planner.cpp
+++ b/local_planner/test/test_local_planner.cpp
@@ -122,14 +122,22 @@ TEST_F(LocalPlannerTests, all_obstacles) {
   avoidanceOutput output = planner.getAvoidanceOutput();
 
   EXPECT_TRUE(output.obstacle_ahead);
-  ASSERT_GE(output.path_node_positions.size(), 2);
+  ASSERT_GE(output.path_node_setpoints.size(), 2);
   float node_max_y = 0.f;
   float node_min_y = 0.f;
-  for (auto it = output.path_node_positions.rbegin(); it != output.path_node_positions.rend(); ++it) {
+  simulation_state state(0.f, Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero());
+  simulation_limits lims(3.f, -1.f, 3.f, 5.f, 20.f);
+  for (auto it = output.path_node_setpoints.rbegin() + 1; it != output.path_node_setpoints.rend(); ++it) {
     auto node = *it;
-    if (node.x() > distance) break;
-    if (node.y() > node_max_y) node_max_y = node.y();
-    if (node.y() < node_min_y) node_min_y = node.y();
+    TrajectorySimulator sim(lims, state, 0.05f);
+
+    std::vector<simulation_state> trajectory = sim.generate_trajectory(node, output.tree_node_duration);
+    for (auto& p : trajectory) {
+      if (p.position.x() > distance) break;
+      if (p.position.y() > node_max_y) node_max_y = p.position.y();
+      if (p.position.y() < node_min_y) node_min_y = p.position.y();
+    }
+    state = trajectory.back();
   }
 
   bool steer_clear = node_max_y > max_y || node_min_y < min_y;
@@ -183,9 +191,9 @@ TEST_F(LocalPlannerTests, obstacles_right) {
   avoidanceOutput output = planner.getAvoidanceOutput();
 
   EXPECT_TRUE(output.obstacle_ahead);
-  ASSERT_GE(output.path_node_positions.size(), 2);
+  ASSERT_GE(output.path_node_setpoints.size(), 2);
   float node_max_y = 0.f;
-  for (auto it = output.path_node_positions.rbegin(); it != output.path_node_positions.rend(); ++it) {
+  for (auto it = output.path_node_setpoints.rbegin(); it != output.path_node_setpoints.rend(); ++it) {
     auto node = *it;
     if (node.x() > distance) break;
     if (node.y() > node_max_y) node_max_y = node.y();
@@ -240,9 +248,9 @@ TEST_F(LocalPlannerTests, obstacles_left) {
   avoidanceOutput output = planner.getAvoidanceOutput();
 
   EXPECT_TRUE(output.obstacle_ahead);
-  ASSERT_GE(output.path_node_positions.size(), 2);
+  ASSERT_GE(output.path_node_setpoints.size(), 2);
   float node_min_y = 0.f;
-  for (auto it = output.path_node_positions.rbegin(); it != output.path_node_positions.rend(); ++it) {
+  for (auto it = output.path_node_setpoints.rbegin(); it != output.path_node_setpoints.rend(); ++it) {
     auto node = *it;
     if (node.x() > distance) break;
     if (node.y() < node_min_y) node_min_y = node.y();
diff --git a/local_planner/test/test_planner_functions.cpp b/local_planner/test/test_planner_functions.cpp
index 2b665546c..eaa9bd341 100644
--- a/local_planner/test/test_planner_functions.cpp
+++ b/local_planner/test/test_planner_functions.cpp
@@ -128,7 +128,7 @@ TEST(PlannerFunctionsTests, processPointcloud) {
   EXPECT_EQ(7, processed_cloud3.size());  // since memory point is inside FOV, it isn't remembered
 }
 
-TEST(PlannerFunctions, getSetpointFromPath) {
+TEST(PlannerFunctions, interpolateBetweenSetpoints) {
   // GIVEN: the node positions in a path and some possible vehicle positions
   float n1_x = 0.8f;
   float n2_x = 1.5f;
@@ -139,7 +139,7 @@ TEST(PlannerFunctions, getSetpointFromPath) {
   Eigen::Vector3f n2(n2_x, n1.y() + sqrtf(1 - powf(n2_x - n1.x(), 2)), 2.5f);
   Eigen::Vector3f n3(n3_x, n2.y() + sqrtf(1 - powf(n3_x - n2.x(), 2)), 2.5f);
   Eigen::Vector3f n4(n4_x, n3.y() + sqrtf(1 - powf(n4_x - n3.x(), 2)), 2.5f);
-  const std::vector<Eigen::Vector3f> path_node_positions = {n4, n3, n2, n1, n0};
+  const std::vector<Eigen::Vector3f> path_node_setpoints = {n4, n3, n2, n1, n0};
   const std::vector<Eigen::Vector3f> empty_path = {};
   ros::Time t1 = ros::Time::now();
   ros::Time t2 = t1 - ros::Duration(0.1);
@@ -150,10 +150,11 @@ TEST(PlannerFunctions, getSetpointFromPath) {
   Eigen::Vector3f sp1, sp2, sp3;
 
   // WHEN: we look for the best direction to fly towards
-  bool res = getSetpointFromPath(path_node_positions, t1, velocity, sp1);  // very short time should still return node 1
-  bool res1 = getSetpointFromPath(path_node_positions, t2, velocity, sp2);
-  bool res2 = getSetpointFromPath(path_node_positions, t3, velocity, sp3);  // should be second node on path
-  bool res3 = getSetpointFromPath(empty_path, t1, velocity, sp1);
+  bool res = interpolateBetweenSetpoints(path_node_setpoints, t1, velocity,
+                                         sp1);  // very short time should still return node 1
+  bool res1 = interpolateBetweenSetpoints(path_node_setpoints, t2, velocity, sp2);
+  bool res2 = interpolateBetweenSetpoints(path_node_setpoints, t3, velocity, sp3);  // should be second node on path
+  bool res3 = interpolateBetweenSetpoints(empty_path, t1, velocity, sp1);
 
   // THEN: we expect the setpoint in between node n1 and n2 for t1 and t2 between
   // node n2 and n3 for t3, and not to get an available path for the empty path
diff --git a/local_planner/test/test_star_planner.cpp b/local_planner/test/test_star_planner.cpp
index dbc001638..0c57610fd 100644
--- a/local_planner/test/test_star_planner.cpp
+++ b/local_planner/test/test_star_planner.cpp
@@ -28,7 +28,7 @@ class StarPlannerTests : public ::testing::Test {
     avoidance::LocalPlannerNodeConfig config = avoidance::LocalPlannerNodeConfig::__getDefault__();
     config.children_per_node_ = 2;
     config.n_expanded_nodes_ = 10;
-    config.tree_node_distance_ = 1.0;
+    config.tree_node_duration_ = 0.5f;
     star_planner.dynamicReconfigureSetStarParams(config, 1);
 
     position.x() = 1.2f;
@@ -50,8 +50,14 @@ class StarPlannerTests : public ::testing::Test {
       }
     }
     costParameters cost_params;
-
-    star_planner.setParams(cost_params);
+    simulation_limits lims;
+    lims.max_z_velocity = 3.f;
+    lims.min_z_velocity = -1.f;
+    lims.max_xy_velocity_norm = 3.f;
+    lims.max_acceleration_norm = 5.f;
+    lims.max_jerk_norm = 20.f;
+
+    star_planner.setParams(cost_params, lims, 2.0f);
     star_planner.setPointcloud(cloud);
     star_planner.setPose(position, velocity);
     star_planner.setGoal(goal);
diff --git a/local_planner/test/test_waypoint_generator.cpp b/local_planner/test/test_waypoint_generator.cpp
index 3ad7018cb..87c2eea10 100644
--- a/local_planner/test/test_waypoint_generator.cpp
+++ b/local_planner/test/test_waypoint_generator.cpp
@@ -31,6 +31,7 @@ class WaypointGeneratorTests : public ::testing::Test, public WaypointGenerator
     avoidance_output.obstacle_ahead = false;
     avoidance_output.cruise_velocity = 1.0;
     avoidance_output.last_path_time = ros::Time(0.28);
+    avoidance_output.tree_node_duration = 0.5f;
 
     PolarPoint p_pol = histogramIndexToPolar(15, 35, 6, 0.f);
 
@@ -45,7 +46,7 @@ class WaypointGeneratorTests : public ::testing::Test, public WaypointGenerator
     Eigen::Vector3f n3(n3_x, n2.y() + sqrtf(1 - powf(n3_x - n2.x(), 2)), 2.0f);
     Eigen::Vector3f n4(n4_x, n3.y() + sqrtf(1 - powf(n4_x - n3.x(), 2)), 2.0f);
     Eigen::Vector3f n5(n5_x, n4.y() + sqrtf(1 - powf(n5_x - n4.x(), 2)), 2.0f);
-    avoidance_output.path_node_positions = {n5, n4, n3, n2, n1, n0};
+    avoidance_output.path_node_setpoints = {n5, n4, n3, n2, n1, n0};
 
     position = Eigen::Vector3f(0.f, 0.f, 0.f);
     q = Eigen::Quaternionf(1.f, 0.f, 0.f, 0.f);