diff --git a/guided_mrmp/tests/traj_from_points_test.py b/guided_mrmp/tests/traj_from_points_test.py
index b323a96c0bed71f6fb4d22a4b5312031a815fc74..5d74788b6465097432821585ab4d6d2e7d283497 100644
--- a/guided_mrmp/tests/traj_from_points_test.py
+++ b/guided_mrmp/tests/traj_from_points_test.py
@@ -41,7 +41,11 @@ def test_traj_from_points():
                 xs.append(node[0])
                 ys.append(node[1])
 
-            x,y,h,path = get_traj_from_points(np.array([start[0], start[1],0]), dynamics, target_velocity, T, DT, [xs, ys])
+            dx = xs[1] - xs[0]
+            dy = ys[1] - ys[0]
+            theta = np.arctan2(dy, dx)
+
+            x,y,h,path = get_traj_from_points(np.array([start[0], start[1],theta]), dynamics, target_velocity, T, DT, [xs, ys])
             x_hists.append(x)
             y_hists.append(y)
             h_hists.append(h)
diff --git a/guided_mrmp/utils/traj_from_points.py b/guided_mrmp/utils/traj_from_points.py
index 4d67e5e594cc654c20cb22c403c3a35f0fa61629..3e06cd2ab116da06bc1ebf94d17c34fea6ce8b8e 100644
--- a/guided_mrmp/utils/traj_from_points.py
+++ b/guided_mrmp/utils/traj_from_points.py
@@ -150,7 +150,7 @@ if __name__ == "__main__":
 
     env = Env([0,10], [0,10], [], [])
 
-    rrtstar = RRTStar(env, x_start, x_goal, 0.5, 0.05, 500, r=2.0)
+    rrtstar = RRTStar(env, x_start, x_goal, 0.5, 0.05, 1000, r=2.0)
     rrtstarpath = rrtstar.run()
     rrtstarpath = list(reversed(rrtstarpath))
     xs = []