diff --git a/src/drawing_controller/drawing_controller/svg_processor.py b/src/drawing_controller/drawing_controller/svg_processor.py
index 0c6c1d2..261c235 100644
--- a/src/drawing_controller/drawing_controller/svg_processor.py
+++ b/src/drawing_controller/drawing_controller/svg_processor.py
@@ -434,7 +434,8 @@ class SVGProcessor():
         Simplify line with https://pypi.org/project/simplification/
         """
         # For RDP, Try an epsilon of 1.0 to start with. Other sensible values include 0.01, 0.001
-        epsilon = 0.001
+        #epsilon = 0.009
+        epsilon = 0.005
 
         tmp = []
         out = []
diff --git a/src/lite6_controller/launch/lite6_real.launch.py b/src/lite6_controller/launch/lite6_real.launch.py
index 56b1380..5e0acb2 100644
--- a/src/lite6_controller/launch/lite6_real.launch.py
+++ b/src/lite6_controller/launch/lite6_real.launch.py
@@ -188,7 +188,9 @@ def launch_setup(context, *args, **kwargs):
     # FIX acceleration limits
     for i in range(1,7):
         joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+        #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5
+        #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0
 
     add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
     add_prefix_to_moveit_params(
diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp
index 505ad41..4e6105f 100644
--- a/src/lite6_controller/src/lite6_controller.cpp
+++ b/src/lite6_controller/src/lite6_controller.cpp
@@ -219,9 +219,10 @@ public:
 
       planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
       mpr.planner_id = "PTP";
+      //mpr.planner_id = "LIN";
       mpr.group_name = move_group.getName();
       mpr.max_velocity_scaling_factor = 1.0;
-      mpr.max_acceleration_scaling_factor = 1.0;
+      mpr.max_acceleration_scaling_factor = 0.98;
       mpr.allowed_planning_time = 10;
       mpr.max_cartesian_speed = 2; // m/s
       //mpr.goal_constraints.position_constraints.header.frame_id = "world";
@@ -252,11 +253,12 @@ public:
       mpr.goal_constraints.push_back(pose_goal);
 
       msi.req = mpr;
-      //msi.blend_radius = 0.0; //TODO make configurable
-      msi.blend_radius = 1e-15; //TODO make configurable
-      if (coincidentPoints(&pose.pose.position, &previous_point, msi.blend_radius * 1e12))
+      //msi.blend_radius = 6e-7; //TODO make configurable
+      //msi.blend_radius = 0.000001; //TODO make configurable
+      msi.blend_radius = 0.0; //TODO make configurable
+      if (coincidentPoints(&pose.pose.position, &previous_point, msi.blend_radius + 1e-5))
       {
-        RCLCPP_INFO(this->get_logger(), "Detected coincident points, setting blend radius to 0.0");
+        RCLCPP_ERROR(this->get_logger(), "Detected coincident points, setting blend radius to 0.0");
         // if points are too close, set blend radius to zero.
         msi.blend_radius = 0.0;
         // also set previous to 0
@@ -265,6 +267,7 @@ public:
       }
       previous_point = pose.pose.position;
 
+      //RCLCPP_ERROR(this->get_logger(), "Appending point with blend_radius:%f", msi.blend_radius);
       msr.items.push_back(msi);
     }
     msr.items.back().blend_radius = 0.0; // Last element blend must be 0