Fix loading numbers in form 'Xe-Y'

Dockerfile

@@ -25,19 +25,20 @@ RUN apt-get update && \
     apt-get install -yq python3-pil.imagetk && \
     apt-get install -yq ros-${ROS_DISTRO}-pilz-industrial-motion-planner && \
     apt-get install -yq tmux && \
+    apt-get install -yq python3-pip && \
     apt-get install -yq ros-${ROS_DISTRO}-desktop && \
     apt-get install -yq ros-${ROS_DISTRO}-rclcpp-components
 
 ### Install AxiDraw
-RUN apt-get update && \
-    apt-get install -yq python3-pip && \
-    pip install --upgrade --upgrade-strategy eager packaging && \
-    pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
+#RUN apt-get update && \
+#    apt-get install -yq python3-pip && \
+#    pip install --upgrade --upgrade-strategy eager packaging && \
+#    pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
 
 ### Install splipy
-RUN apt-get update && \
-    apt-get install -yq python3-pip && \
-    pip install --upgrade --upgrade-strategy eager splipy
+#RUN apt-get update && \
+#    apt-get install -yq python3-pip && \
+#    pip install --upgrade --upgrade-strategy eager splipy
 
 # Build interfaces and generic controller first
 COPY ./src/robot_interfaces ${WS_SRC_DIR}/robot_interfaces
@@ -50,6 +51,8 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
 COPY ./src/draw_svg ${WS_SRC_DIR}/draw_svg
 COPY ./src/drawing_controller ${WS_SRC_DIR}/drawing_controller
 COPY ./src/axidraw_controller ${WS_SRC_DIR}/axidraw_controller
+RUN pip install -r ${WS_SRC_DIR}/drawing_controller/requirements.txt
+RUN pip install -r ${WS_SRC_DIR}/axidraw_controller/requirements.txt
 RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     source "${WS_INSTALL_DIR}/local_setup.bash" && \
     colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/draw_svg ${WS_SRC_DIR}/drawing_controller ${WS_SRC_DIR}/axidraw_controller && \

README.md

@@ -77,6 +77,104 @@ ros2 run drawing_controller drawing_controller svg/test.svg
 ```
 This will draw the svg image given as the last argument.
 
+## SVG compatibility info
+Tested with SVG from the following programs
+- Inkscape
+- Inkpad
+- Affinitydraw
+- vtracer
+
+Delimiter characters seem to vary somewhat.
+The following examples work:
+TODO ADD EXAMPLES OF SVG PATHS
+
+The following SVG primitives are supported:
+| Primitive                           | Support  |
+|-------------------------------------|----------|
+| a                                  | no       |
+| animate                            | no       |
+| animateMotion                      | no       |
+| animateTransform                   | no       |
+| circle                             | no       |
+| clipPath                           | no       |
+| defs                               | no       |
+| desc                               | no       |
+| discard                            | no       |
+| ellipse                            | no       |
+| feBlend                            | no       |
+| feColorMatrix                      | no       |
+| feComponentTransfer                | no       |
+| feComposite                        | no       |
+| feConvolveMatrix                   | no       |
+| feDiffuseLighting                  | no       |
+| feDisplacementMap                  | no       |
+| feDistantLight                     | no       |
+| feDropShadow                       | no       |
+| feFlood                            | no       |
+| feFuncA                            | no       |
+| feFuncB                            | no       |
+| feFuncG                            | no       |
+| feFuncR                            | no       |
+| feGaussianBlur                     | no       |
+| feImage                            | no       |
+| feMerge                            | no       |
+| feMergeNode                        | no       |
+| feMorphology                       | no       |
+| feOffset                           | no       |
+| fePointLight                       | no       |
+| feSpecularLighting                 | no       |
+| feSpotLight                        | no       |
+| feTile                             | no       |
+| feTurbulence                       | no       |
+| filter                             | no       |
+| foreignObject                      | no       |
+| g                                  | yes      |
+| hatch                              | no       |
+| hatchpath                          | no       |
+| image                              | no       |
+| line                               | yes      |
+| linearGradient                     | no       |
+| marker                             | no       |
+| mask                               | no       |
+| metadata                           | no       |
+| mpath                              | no       |
+| path                               | partial  |
+| pattern                            | no       |
+| polygon                            | yes      |
+| polyline                           | yes      |
+| radialGradient                     | no       |
+| rect                               | no       |
+| script                             | no       |
+| set                                | no       |
+| stop                               | no       |
+| style                              | no       |
+| svg                                | no       |
+| switch                             | no       |
+| symbol                             | no       |
+| text                               | no       |
+| textPath                           | no       |
+| title                              | no       |
+| tspan                              | no       |
+| use                                | no       |
+| view                               | no       |
+
+And the following SVG path commands are supported:
+| Command type           | Supported         | Unsupported |
+|------------------------|-------------------|-------------|
+| MoveTo                 |  M, m             |             |
+| LineTo                 |  L, l, H, h, V, v |             |
+| Cubic Bézier Curve     |  C, c, S, s       |             |
+| Quadratic Bézier Curve |                   | Q, q, T, t  |
+| Elliptical Arc Curve   |                   | A, a        |
+| ClosePath              |  Z, z             |             |
+
+
+## Axidraw concerns
+## xArm concerns
+TODO make TCP height diagram
+
+The following paths work, notic
+
 ## Creating compatible SVG images
 https://github.com/visioncortex/vtracer
 

scripts/convertlogtimestamp.sh

@@ -1,5 +1,6 @@
 #!/usr/bin/env sh
 # Converts ROS2 log data timestamps from stdin to more readable format
+# Reads lines from stdin, pipe input to this script
 
 
 while IFS= read -r string; do

scripts/gen-docs.sh

@@ -1,4 +1,7 @@
 #!/usr/bin/env sh
+# generates docs
+
+pushd ..
 
 pushd src/robot_controller/
 doxygen
@@ -13,3 +16,5 @@ doxygen
 popd
 
 make html
+
+

scripts/plot_lite6_csv/plot_lite6_csv.py

@@ -0,0 +1,82 @@
+# 3D Heatmap in Python using matplotlib
+  
+# to make plot interactive 
+#%matplotlib
+  
+# importing required libraries
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+import numpy as np
+from pylab import *
+
+import sys
+  
+print('arg:', sys.argv)
+if len(sys.argv) <= 1:
+    print('Give file path as arg')
+    exit()
+
+filepath = sys.argv[1]
+f = open(filepath, "r")
+data = f.read()
+
+# creating a dataset
+x_succ = []
+y_succ = []
+z_succ = []
+
+x_fail = []
+y_fail = []
+z_fail = []
+
+for l in data.split("\n"):
+    d = l.split(",")
+    if len(d) == 0:
+        continue
+    s = d[0]
+    for p in d[1:]:
+        p = p.split(" ")
+        if len(p) < 3:
+            continue
+        if s == 'success':
+           x_succ.append(float(p[0])) 
+           y_succ.append(float(p[1]))
+           z_succ.append(float(p[2]))
+        else:
+           x_fail.append(float(p[0]))
+           y_fail.append(float(p[1]))
+           z_fail.append(float(p[2]))
+
+x_succ = np.array(x_succ)
+y_succ = np.array(y_succ)
+z_succ = np.array(z_succ)
+
+x_fail = np.array(x_fail)
+y_fail = np.array(y_fail)
+z_fail = np.array(z_fail)
+  
+# creating figures
+fig = plt.figure(figsize=(10, 10))
+ax = fig.add_subplot(111, projection='3d')
+
+# setting color bar
+color_map1 = cm.ScalarMappable(cmap=cm.Greens_r)
+color_map1.set_array([x_succ + y_succ + z_succ])
+  
+color_map2 = cm.ScalarMappable(cmap=cm.Reds_r)
+color_map2.set_array([x_fail + y_fail + z_fail])
+
+# creating the heatmap
+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')
+# adding title and labels
+ax.set_title("3D Heatmap")
+ax.set_xlabel('X-axis')
+ax.set_ylabel('Y-axis')
+ax.set_zlabel('Z-axis')
+  
+# displaying plot
+plt.show()

scripts/plot_lite6_csv/shell.nix

@@ -0,0 +1,11 @@
+# shell.nix
+{ pkgs ? import <nixpkgs> {} }:
+let
+  my-python-packages = p: with p; [
+    matplotlib
+    numpy
+    # other python packages
+  ];
+  my-python = pkgs.python3.withPackages my-python-packages;
+in my-python.env
+

src/axidraw_controller/requirements.txt

@@ -0,0 +1 @@
+axicli @ https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -78,8 +78,13 @@ class AxidrawController : public RobotController
   {
     float lspan = lmax - lmin;
     float rspan = rmax - rmin;
-    val = (val - lmin) / lspan;
-    return rmin + (val * rspan);
+    float out = (val - lmin) / lspan;
+    out = rmin + (val * rspan);
+
+    // Ensure that output is within bounds
+    out = std::max(rmin, out);
+    out = std::min(rmax, out);
+    return out;
   }
 
   /**

src/drawing_controller/drawing_controller/svg_processor.py

@@ -4,6 +4,7 @@ import lxml.etree as ET
 import splipy.curve_factory as cf
 import numpy as np
 import math
+import simplification.cutil
 
 class SVGProcessor():
     """
@@ -115,15 +116,21 @@ class SVGProcessor():
                 path.append(c)
 
             # Numbers
-            if c == '-' or c.isdecimal():
+            if c == '+' or c == '-' or c.isdecimal():
                 s = c
-                while i < len(pathstr) and not (c.isspace() or c == ','):
+                isdelim = lambda x: x.isspace() or (x.isalpha() and c != 'e') or x in [',', '+']
+                while i < len(pathstr) and not isdelim(c):
                     c = pathstr[i]
-                    if c != ',' and not c.isspace():
+                    if not isdelim(c):
                         s = s + c
+                    if c.isalpha() and c != 'e':
+                        break
                     i += 1
                 path.append(s)
 
+        #print(path)
+        #input()
+
         # Parser
         self.logger.info("Parsing path :'{}...' with {} tokens".format(path[:20], len(path)))
         x = 0.0
@@ -136,7 +143,6 @@ class SVGProcessor():
             return float(path[i])
 
         def isfloat(element):
-            #If you expect None to be passed:
             if element is None:
                 return False
             try:
@@ -149,10 +155,14 @@ class SVGProcessor():
             return i + 1 < len(path) and isfloat(path[i + 1])
         def setpointup():
             nonlocal output
+            nonlocal x
+            nonlocal y
             p = self.map_point(x,y)
             output.append((p[0],p[1],1.0))
         def setpointdown():
             nonlocal output
+            nonlocal x
+            nonlocal y
             p = self.map_point(x,y)
             output.append((p[0],p[1],0.0))
         def appendpoints(points):
@@ -160,7 +170,14 @@ class SVGProcessor():
             for x,y in points:
                 p = self.map_point(x,y)
                 output.append((p[0],p[1],0.0))
-
+        def lineto(xn,yn):
+            nonlocal output
+            nonlocal x
+            nonlocal y
+            setpointdown()
+            x = xn
+            y = yn
+            setpointdown()
 
         while i < len(path):
             w = path[i]
@@ -181,17 +198,55 @@ class SVGProcessor():
                 continue
             # LineTo commands
             if (w == "L"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    xn = getnum()
+                    yn = getnum()
+                    lineto(xn, yn)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "l"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    xn = x + getnum()
+                    yn = y + getnum()
+                    lineto(xn, yn)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "H"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    xn = getnum()
+                    lineto(xn, y)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "h"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    xn = x + getnum()
+                    lineto(xn, y)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "V"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    yn = getnum()
+                    lineto(x, yn)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "v"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    yn = y + getnum()
+                    lineto(x, yn)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             # Cubic Bézier Curve commands
             if (w == "C"):
                 while True:
@@ -201,12 +256,17 @@ class SVGProcessor():
                                       (getnum(),getnum()),
                                       (getnum(),getnum())]
                     control_points = np.array(control_points)
-                    n = 50
+                    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)
@@ -222,11 +282,16 @@ class SVGProcessor():
                                       (x + getnum(), y + getnum()),
                                       (x + getnum(), y + getnum())]
                     control_points = np.array(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]
@@ -283,6 +348,8 @@ class SVGProcessor():
 
             if 'viewBox' in svg.attrib:
                 vb = svg.get('viewBox').split(' ')
+                if (len(vb) < 4): # handle case were comma is delim
+                    vb = vb[0].split(',')
                 self.map_point = self.map_point_function(float(vb[2]),
                                                          float(vb[3]))
                 self.logger.info("Got width:{} and height:{} from viewBox".format(vb[2],vb[3]))
@@ -320,6 +387,10 @@ class SVGProcessor():
 
                 mm = self.remove_homes(m)
                 mm = self.remove_redundant(mm)
+                #print('before:', len(mm))
+                mm = self.simplify(mm)
+                #print('after:', len(mm))
+                #input()
 
                 #self.logger.info("Refining:'{}...'".format(m[:3]))
                 motions_refined.append(self.down_and_up(mm))
@@ -358,6 +429,33 @@ class SVGProcessor():
             mm.append(p)
         return mm
 
+    def simplify(self, motion):
+        """
+        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
+
+        tmp = []
+        out = []
+        lastup = True
+        sf = lambda l: [ (p[0],p[1],0.0) for p in simplification.cutil.simplify_coords(l, epsilon) ]
+        for p in motion:
+            penup = p[2] > 0
+            if penup and not lastup:
+                out += sf(tmp)
+                tmp = []
+            if penup:
+                out.append(p)
+            else:
+                tmp.append(list(p)[:-1])
+            lastup = penup
+
+        if (len(tmp) > 0):
+            out += sf(tmp)
+
+        return out
+
     def translate(self, val, lmin, lmax, rmin, rmax):
         lspan = lmax - lmin
         rspan = rmax - rmin

src/drawing_controller/requirements.txt

@@ -0,0 +1,4 @@
+lxml==4.9.2
+numpy==1.24.2
+Splipy==1.6.0
+simplification==0.6.2

src/lite6_controller/src/lite6_controller.cpp

@@ -2,6 +2,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <robot_controller/robot_controller.hpp>
 #include <chrono>
+#include <cstdlib>
 //#include <queue>
 
 #include <fstream>
@@ -79,11 +80,11 @@ public:
   // Set limits for A4 paper
   // 297x210
   float xlim_lower = 0.10;
-  float xlim_upper = 0.30;
-  float ylim_lower = -0.14;
-  float ylim_upper = 0.14;
-  float zlim_lower = 0.19;
-  float zlim_upper = 0.21;
+  float xlim_upper = 0.305;
+  float ylim_lower = -0.1475;
+  float ylim_upper = 0.1475;
+  float zlim_lower = 0.208;
+  float zlim_upper = 0.218;
 
   //bool moved = false;
   //
@@ -187,6 +188,17 @@ public:
     return pose;
   }
 
+  /**
+   * Checks if points are within a radius of eachother, specified by the tolerance argument
+   */
+  bool coincidentPoints(geometry_msgs::msg::Point *p1, geometry_msgs::msg::Point *p2, double tolerance)
+  {
+    bool x = std::abs(p1->x - p2->x) <= tolerance;
+    bool y = std::abs(p1->y - p2->y) <= tolerance;
+    bool z = std::abs(p1->z - p2->z) <= tolerance;
+    return x && y && z;
+  }
+
   /**
    *
    */
@@ -196,6 +208,10 @@ public:
     //waypoints.push_back(move_group.getCurrentPose().pose);
     std::string ee_link = move_group.getLinkNames().back();
     RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str());
+    geometry_msgs::msg::Point previous_point;
+    //previous_point.point.x = -1.0;
+    //previous_point.point.y = -1.0;
+    //previous_point.point.z = -1.0;
     for (auto p : *path)
     {
       //RCLCPP_INFO(this->get_logger(), "Creating MSI");
@@ -204,41 +220,51 @@ public:
       planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
       mpr.planner_id = "PTP";
       mpr.group_name = move_group.getName();
-      mpr.max_velocity_scaling_factor = 0.5;
-      mpr.max_acceleration_scaling_factor = 0.5;
+      mpr.max_velocity_scaling_factor = 1.0;
+      mpr.max_acceleration_scaling_factor = 1.0;
       mpr.allowed_planning_time = 10;
-      mpr.max_cartesian_speed = 1; // m/s
+      mpr.max_cartesian_speed = 2; // m/s
       //mpr.goal_constraints.position_constraints.header.frame_id = "world";
 
       // A tolerance of 0.01 m is specified in position
       // and 0.01 radians in orientation
-      std::vector<double> tolerance_pose(3, 0.01);
-      std::vector<double> tolerance_angle(3, 0.01);
+      //std::vector<double> tolerance_pose(3, 0.01);
+      //std::vector<double> tolerance_angle(3, 0.01);
 
 
       // Set motion goal of end effector link
       //std::string ee_link = moveit_cpp_->getRobotModel()->getJointModelGroup(planning_component_->getPlanningGroupName())->getLinkModelNames().back();
-      //RCLCPP_INFO(this->get_logger(), "Got ee_link");
 
 
       //moveit_msgs::msg::Constraints pose_goal =
       //  kinematic_constraints::constructGoalConstraints(ee_link, p, tolerance_pose, tolerance_angle);
       //kinematic_constraints::constructGoalConstraints(ee_link, pose, tolerance_pose, tolerance_angle);
 
-      geometry_msgs::msg::PointStamped point;
-      auto position = translatePose(p).pose.position;
-      point.point = position;
+      //geometry_msgs::msg::PointStamped point;
+      geometry_msgs::msg::Point point;
+      auto pose = translatePose(p);
       moveit_msgs::msg::Constraints pose_goal =
+        kinematic_constraints::constructGoalConstraints(ee_link, pose, 1e-3, 1e-2);
       //kinematic_constraints::constructGoalConstraints(ee_link, point, 1e-5);
-      kinematic_constraints::constructGoalConstraints(ee_link, translatePose(p), 1e-3, 1e-2);
       //kinematic_constraints::constructGoalConstraints(ee_link, p, 1.0, 1.0);
       //kinematic_constraints::constructGoalConstraints(ee_link, p, 1e-3, 1e-2);
 
       mpr.goal_constraints.push_back(pose_goal);
 
       msi.req = mpr;
-      msi.blend_radius = 0.0; //TODO make configurable
-      //msi.blend_radius = 0.000000001; //TODO make configurable
+      //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))
+      {
+        RCLCPP_INFO(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
+        if (msr.items.size() > 0)
+          msr.items.back().blend_radius = 0.0;
+      }
+      previous_point = pose.pose.position;
+
       msr.items.push_back(msi);
     }
     msr.items.back().blend_radius = 0.0; // Last element blend must be 0
@@ -384,7 +410,7 @@ public:
       move_group.execute(ts[0]);
 
       status = status + "," + pointsToString(&goal->motion.path,0,0,0);
-      //appendLineToFile("OUTPUT.csv", status);
+      appendLineToFile("OUTPUT.csv", status);
 
       result->result = "success";
       goal_handle->succeed(result);
@@ -394,7 +420,7 @@ public:
 
     status = "failure";
     status = status + "," + pointsToString(&goal->motion.path,0,0,0);
-    //appendLineToFile("OUTPUT.csv", status);
+    appendLineToFile("OUTPUT.csv", status);
 
     RCLCPP_ERROR(this->get_logger(), "Planner failed to return trajectory in time");
     result->result = "failure";