Refactor and add dropzero

scripts/plot_lite6_csv/plot_lite6_csv.py

@@ -71,7 +71,7 @@ img1 = ax.scatter(x_succ, y_succ, z_succ, marker='s',
                  s=2, color='green')
   
 img2 = ax.scatter(x_fail, y_fail, z_fail, marker='s',
-                 s=0.1, color='red')
+                 s=1, color='red')
 # adding title and labels
 ax.set_title("3D Heatmap")
 ax.set_xlabel('X-axis')

src/drawing_controller/drawing_controller/svg_processor.py

@@ -165,6 +165,13 @@ class SVGProcessor():
             nonlocal y
             p = self.map_point(x,y)
             output.append((p[0],p[1],0.0))
+        def dropzeros(points):
+            out = []
+            for x,y in points:
+                if x <= 0.0 or y <= 0.0:
+                    continue
+                out.append([x,y])
+            return out
         def appendpoints(points):
             nonlocal output
             for x,y in points:
@@ -178,6 +185,36 @@ class SVGProcessor():
             x = xn
             y = yn
             setpointdown()
+        def bezier(control_points):
+            nonlocal x
+            nonlocal y
+            control_points = np.array(control_points)
+            #maxval = np.amax(np.absolute(control_points))
+            #print('inpput', control_points)
+            maxval = np.amax(np.absolute(control_points))
+            #print('maxxv', maxval)
+            control_points = control_points / maxval #normalize values
+            n = 10
+            curve = cf.cubic_curve(control_points)
+            lin = np.linspace(curve.start(0), curve.end(0), n)
+            coordinates = curve(lin)
+            coordinates = np.nan_to_num(coordinates)
+            coordinates = coordinates * maxval #denormalize values
+            coordinates = dropzeros(coordinates)
+            #self.logger.info("Appending curve points: {}".format(coordinates))
+            #print(coordinates)
+            #input()
+
+            #print('start', curve.start(0))
+            #print('end', curve.end(0))
+            #print('curve', curve)
+            #print('lin', lin)
+            #print('curvelin', curve(lin))
+            #input()
+            if len(coordinates) > 0:
+                x = coordinates[-1][0]
+                y = coordinates[-1][1]
+            return coordinates
 
         while i < len(path):
             w = path[i]
@@ -255,20 +292,7 @@ class SVGProcessor():
                                       (getnum(),getnum()),
                                       (getnum(),getnum()),
                                       (getnum(),getnum())]
-                    control_points = np.array(control_points)
+                    coordinates = bezier(control_points)
-                    maxval = np.amax(np.absolute(control_points))
-                    control_points = control_points / maxval #normalize values
-                    n = 500
-                    curve = cf.cubic_curve(control_points)
-                    lin = np.linspace(curve.start(0), curve.end(0), n)
-                    coordinates = curve(lin)
-                    coordinates = np.nan_to_num(coordinates)
-                    coordinates = coordinates * maxval #denormalize values
-                    #self.logger.info("Appending curve points: {}".format(coordinates))
-                    #print(coordinates)
-                    #input()
-                    x = coordinates[-1][0]
-                    y = coordinates[-1][1]
                     appendpoints(coordinates)
                     if not nextisnum():
                         break
@@ -281,20 +305,7 @@ class SVGProcessor():
                                       (x + getnum(), y + getnum()),
                                       (x + getnum(), y + getnum()),
                                       (x + getnum(), y + getnum())]
-                    control_points = np.array(control_points)
+                    coordinates = bezier(control_points)
-                    maxval = np.amax(np.absolute(control_points))
-                    control_points = control_points / maxval #normalize values
-                    n = 50
-                    curve = cf.cubic_curve(control_points)
-                    lin = np.linspace(curve.start(0), curve.end(0), n)
-                    coordinates = curve(lin)
-                    coordinates = np.nan_to_num(coordinates)
-                    coordinates = coordinates * maxval #denormalize values
-                    #print("got:", coordinates)
-                    #exit()
-                    #self.logger.info("Appending curve points: {}".format(coordinates))
-                    x = coordinates[-1][0]
-                    y = coordinates[-1][1]
                     appendpoints(coordinates)
                     if not nextisnum():
                         break
@@ -434,7 +445,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 = []

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(

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 = 6e-7; //TODO make configurable
-      msi.blend_radius = 1e-15; //TODO make configurable
+      //msi.blend_radius = 0.000001; //TODO make configurable
-      if (coincidentPoints(&pose.pose.position, &previous_point, msi.blend_radius * 1e12))
+      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