We began by modifying the previously provided AR tracker launch files defining the AR tags with the dimensions of our custom-printed, 6 cm tags, as well as telling the robot to use the head camera instead of the right hand one.

We then developed code that performs the following tasks in sequence:

1. Image capture

2. Image processing

3. Contour to path conversion

4. Target paper identification

5. Coordinate system conversion

6. Image drawing

Each step in the software is discussed in more detail as follows.

The arm is first commanded to tuck, to give the wrist camera full view of the table, then the wrist camera takes and saves an image. This code was modified from provided camera feed example code to save a snapshot to a .png image file and close the feed after 1 second.

The largest bright region of the image is identified as the paper (a black tablecloth is draped over the workspace). If this detected region is not a quadrilateral, a quadrilateral approximation of the region is made (to handle if parts of the paper are folded, or if portions of the paper were not detected due to bad lighting). Then the quadrilateral is scaled to a 1100x850 pixel rectangle. This allows it to handle images that may have been taken at an oblique angle.

First a gaussian blur and adaptive threshold are applied to the image to convert it to black and white with a good estimate of where the lines are. A MORPH_CLOSE morphological transform is then applied to remove black dots and other fuzziness around the lines that we wish to identify. Next, skeletonization is conducted to convert areas of white in the image to 1-pixel-wide lines. Finally, the image is denoised. 

The program next uses a depth-first search approach to find individual line segments in the image, starting from a random white pixel and visiting adjacent white pixels until it cannot anymore. It then restarts from a new, unvisited white pixel until all of them have been visited. After this set of line segments is created, a brute-force approach is taken to find the shortest distance between endpoints of line segments to determine an order in which to draw the lines. Finally, the Ramer-Douglas-Pecker (RDP) algorithm is used to approximate each contour segment with fewer points, since we do not need the robot arm to visit each individual pixel (if this is not done, the arm's movement becomes much slower and more jagged).

We find the AR tag locations relative to the Sawyer's base frame using tf2 and ar_track_alvar. We average their z-coordinates to find the table height, and use their x and y coordinates and the known 'base' angle of the diagonal between the 2 AR tags (as if the paper were oriented landscape, looking at it from the Sawyer's perspective) to find the angle of the paper's long axis, which we use to create new basis vectors for the direction of the paper, knowing the paper's short axis also lies on the plane and is perpendicular to the long axis. We select one of the AR tags as congruent to the origin of the planned contours, and rotate the image. We then apply an offset to account for the fact that the paper lines up with the corner of the AR tag, rather than its center. We also set the scale based off of the difference in AR tag center distance to ensure we are scaled to the real size of the paper, which is known since we know the paper and AR tag dimensions, with the assumption that ar_track_alvar senses the position of the center of the AR tags. We precompute the real world coordinate of each point in each contour, and calculate the coordinates for new contours after each previous contour is completed.

We iterate through our list of precomputed coordinates for drawing the image to convert them to cartesian paths, inially with an actuator height that holds the marker above the table, before lowering for the marker to touch the paper and complete the contour. At the end of the contour, the height is again set to lift the marker off of the paper, before moving to the starting point of the next contour. The motion is output to MoveIt for the robot to execute the trajectory. This repeats until every contour has been iterated through, and the movement ends. 

We designed and built a pen holder which used a locking spring mechanism that attached onto the robot gripper actuator, using the mounting holes for the gripper to screw on. There are 5 major components. 

1. The base. This contains the spring, and has 2 protrusions with 2 holes each for mounting the holder onto the Sawyer. There are slots built into the raised cylindrical part, which have 0 tolerance in design for fitting with the tabs on the marker tube, ensuring a friction-fit. Additionally, the spring force assists with increasing friction to prevent the barrel from rotating.

2. The spring. There are 2 thin-wire springs in series, to ensure the marker always pushes against the page when lowered far enough. This was chosen after observing that thicker wire single springs built up force way too quickly through the marker's compression distance. Thin wires and a long total wire length both act provide a smaller spring rate. 

3. The marker tube. This tube has tabs which allow for interfacing with the slots in the base, and it has a hole for the marker. A portion of the tube assembles concentrically with the base's spring hole, which helps reduce play in the assembly.

4. There is a donut-shaped piece which press-fits onto the back of the marker. This stops the marker from falling out of the tube, and provides a base for the spring to push against.

5. The marker. It's a standard Crayola thin marker.

The disassembled attachment is shown in the 2nd image on the left. For this design, we had several major considerations. 

• A key consideration of our design was manufacturability and ease of assembly. This was to ensure we could get this made, and to ensure we could swap markers as necessary, in case one got damaged or dried out. This is why there are so few parts, and no fasteners other than those to attach the assembly to the Sawyer. 

• We wanted our marker to have as little free play as possible inside the marker tube to ensure the marker would properly track with the arm. This was achieved by making the tube long and making the tolerance relatively tight. We could not eliminate all free play because the marker itself is not a perfect cylinder, or even a consistent shape along its entire length. Having a tight fit would prevent smooth motion.

• The marker would also need to move freely within the tube so it can always maintain contact the paper surface.

The printed components components were made in the Jacobs Maker Space using Prusa MK3S+ printers and standard PLA filament. Each printed holder required roughly 26 grams of PLA filament. We used standard Crayola fine line markers, and 12mm OD 0.5mm wire diameter 30mm free length springs.