<jupyter_start><jupyter_text>Cartesian robot coordinate path planner This code will output G code for a cnc cartesian robot to run<jupyter_code>import warnings
warnings.filterwarnings('ignore')
import matplotlib.pyplot as plt
import numpy as np<jupyter_output><empty_output><jupyter_text>For a cartesian 2D robot, the real space coordinates are given by:\begin{equation*}\mathbf{M}\mathbf{S}\vec{s} = \vec{r}\label{stepToReal} \tag{1}\end{equation*}Where $\mathbf{M}$ is the transformation matrix from the machine space to the real space, $\mathbf{S}$ is the transformation matrix from step space into machine space, $\vec{s}$ is the step space coordinates and $\vec{r}$ is the real space coordinates. For this 2D example with a cartesian robot based on stepper motors, let us assume the steppers have a $1.8^{o}$ degree step, and that the stepper is coupled to the axis, being X or Y, by a pulley and belt mechanism, that has $12mm$ diameter.For such a system, the step to machine space transformation matrix is:\begin{equation*}\begin{bmatrix}1.8 & 0\\0 & 1.8\end{bmatrix}\label{stepToMachineMatrix} \tag{2}\end{equation*}So you get the equation bellow for the transformation from step to machine space:\begin{equation*}\begin{bmatrix} 1.8 & 0 \\ 0 & 1.8 \end{bmatrix} . \begin{bmatrix} StepsMotorX \\ StepsMotorY \end{bmatrix} = \begin{bmatrix} StepperAngleX \\ StepperAngleY \end{bmatrix}\label{stepToMachineEquation} \tag{3}\end{equation*} Now we have to solve the transformation from the belt and pulley system to the real space. This system will follow the following equation for each axis:\begin{equation*} = \frac{Angle * \pi * diameter}{360}\label{beltPulleyEquation} \tag{4}\end{equation*}So the transformation matrix $\mathbf{M}$ becomes:\begin{equation*}\begin{bmatrix}0.1047 & 0\\0 & 0.1047\end{bmatrix}\label{machineToRealMatrix} \tag{5}\end{equation*} The equation for the whole system thus becomes:\begin{equation*}\begin{bmatrix} 0.1047 & 0 \\ 0 & 0.1047 \end{bmatrix} . \begin{bmatrix} 1.8 & 0 \\ 0 & 1.8 \end{bmatrix} . \begin{bmatrix} StepsMotorX \\ StepsMotorY \end{bmatrix} = \begin{bmatrix} x \\ y \end{bmatrix}\label{wholeSystem} \tag{6}\end{equation*} The two matrices $M$ and $S$ can be combined for simplicity, running the code bellow:<jupyter_code>M = np.matrix([[0.1047, 0     ],
              [0     , 0.1047]])
S = np.matrix([[1.8, 0  ],
              [0  , 1.8]])
MS = M*S
MS<jupyter_output><empty_output><jupyter_text>So the system becomes:\begin{equation*}\begin{bmatrix} 0.188 & 0 \\ 0 & 0.188 \end{bmatrix} . \begin{bmatrix} StepsMotorX \\ StepsMotorY \end{bmatrix} = \begin{bmatrix} x \\ y \end{bmatrix}\label{wholeSystemCompressed} \tag{7}\end{equation*} Now, for us to create a control system, we need to understand that what we can control here is limited to one step at a time, and we control steps only.We can keep track of the step count per motor, that will begin referenced at zero steps and we assume that this is where the origin of x and y are(which is not the case for more complex robots, but is fine for the simple cartesian case), negative steps will translate do negative coordinates and we will assume the robot does not lose any steps during the operation(infinite torque or whatever) for simplicity.With all this in mind, we now need a way to translate real space coordinates back into step space coordinates, so we can control the robot by simply stepping in the direction of the difference of coordinates between the points given. The problems of complex trajectories and acceleration will be left for later.The way to get from real space into the step space is to invert the matrix MS, by running the code bellow:<jupyter_code>MS.I<jupyter_output><empty_output><jupyter_text>So now we have \begin{equation*}\begin{bmatrix} 5.31 & 0 \\ 0 & 5.31 \end{bmatrix} . \begin{bmatrix} x \\ y \end{bmatrix} = \begin{bmatrix} StepsMotorX \\ StepsMotorY \end{bmatrix}\label{wholeSystemInverse} \tag{8}\end{equation*} Now we have what we need to start planning some trajectories.Let us start trying to move from $A=\begin{bmatrix} 0 \\ 0\end{bmatrix}$ to $B=\begin{bmatrix} 1 \\ 1\end{bmatrix}$<jupyter_code>pointA = np.matrix([0,0]).T
pointA
stepPointA = MS.I*pointA #This is the step space representation for the point A
stepPointA
pointB = np.matrix([1,1]).T
pointB
stepPointB = MS.I*pointB #This is the step space representation for the point B
stepPointB<jupyter_output><empty_output><jupyter_text>Here we get to an important point: The system is discrete, therefore some points cannot be reached with arbitrary precision, there will always be an error between 0 and half a step, positive or negative, because only whole steps can be taken.I know steppers can half-step, whatever-step or micro-step, it changes nothing here, however you divide the step, the system is still discrete.$\textit{"But what if i take the limit when the step is an infinitessimal?"}$Well good luck trying to find such a stepper motor for sale Mr. Newton.Anyway... We need now to make a decision on how to approximate the point and update the position of the robot to a "reachable" position.<jupyter_code>reachableB = np.round(stepPointB)
reachableB<jupyter_output><empty_output><jupyter_text>Now we know we can reach this point in the $\textbf{STEP SPACE}$, we can calculate where it lands on $\textbf{REAL SPACE}$<jupyter_code>realReachableB = MS*reachableB
realReachableB
fig, ax = plt.subplots(1,2,figsize=(12,6))
ax[0].grid()
ax[1].grid()
ax[0].scatter(*np.array(pointA))
ax[0].scatter(*np.array(pointB))
ax[0].scatter(*np.array(realReachableB))
ax[0].set_title("Real Space")
ax[1].scatter(*np.array(stepPointA))
ax[1].scatter(*np.array(stepPointB))
ax[1].scatter(*np.array(reachableB))
ax[1].set_title("Step Space")<jupyter_output><empty_output><jupyter_text>The good news is, this error is absolute,it does not grow with larger values of x and y, given we keep the same motor and mechanisms, so for larger trajectories the relative error gets smaller.Now we got to the point where we have to think about the microcontroller that is going to do the actual stepping on the direction we established. Using a discrete microcontroller means we have a single thread running on the program, so we have to multiplex the output control. This means we control one motor at a time, one step at a time. Ok, so how can we get the smoothest curve possible from this system?This is a real question, at the time of writing this line i have no idea how. But when you don't know what to do, you do what you know. A good way to start is by assembling a vector with how many steps have to be taken in each dimension<jupyter_code>steppingVector = reachableB - stepPointA
steppingVector<jupyter_output><empty_output><jupyter_text>Great, now that we know far to travel in each direction, we can start taking steps in that direction. Simply take a bunch of (1,1) steps and we are done right? NO!And yes...Naive first, always!So lets plot this trajectory from A to B<jupyter_code>stepPointsX = [0,1,1,2,2,3,3,4,4,5,5]
stepPointsY = [0,0,1,1,2,2,3,3,4,4,5]
pointsRealTrajectory = []
for index in range(len(stepPointsX)):
    tmpPointVector = np.matrix([stepPointsX[index],stepPointsY[index]]).T
    pointsRealTrajectory.append(MS*tmpPointVector)
pointsRealTrajectory

fig, ax = plt.subplots(1,2,figsize=(12,6))
ax[0].grid()
ax[1].grid()
ax[0].scatter(*np.array(pointA))
ax[0].scatter(*np.array(pointB))
ax[0].plot(np.array(pointsRealTrajectory)[:,0],np.array(pointsRealTrajectory)[:,1])
ax[0].scatter(*np.array(realReachableB))
ax[0].plot([0,1],[0,1])
ax[0].set_title("Real Space")
ax[1].scatter(*np.array(stepPointA))
ax[1].scatter(*np.array(stepPointB))
ax[1].plot(stepPointsX,stepPointsY)
ax[1].scatter(*np.array(reachableB))
ax[1].set_title("Step Space")<jupyter_output><empty_output><jupyter_text>To be continued... right now ! Now we have done it by hand, let us automate this process.First, divide the stepping vector into smaller steps, we do not want to jump all the x and y values at once, interpolating as close as possible to the line is what we actually need.The way of doing it is by calculating the direction of the line that connects A to B. This is done with the formula:\begin{equation*}\begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} \frac{1}{|\vec{AB}|} & 0 \\ 0 & \frac{1}{|\vec{AB}|} \end{bmatrix} . \begin{bmatrix} x \\ y \end{bmatrix}\label{vectorModule} \tag{9}\end{equation*}Looking at this matrix, you can notice that trying to move from a point to itself will result in a singularity. That has to be treated in the code when implementing into a system to avoid exceptions locking your application. In code that looks like this:<jupyter_code>deltaPoints = pointB - pointA
deltaPoints
moduleDeltaPoints = np.sqrt(deltaPoints[0,0]**2 + deltaPoints[1,0]**2)
moduleDeltaPoints
directionVector = deltaPoints[:,0]/moduleDeltaPoints
directionVector<jupyter_output><empty_output><jupyter_text>It is important to notice that only if the space of steps and real space are a scaled version of one another that this vector will point in the exact same direction in step and real space, this will not be the case for spaces that are transformed in other ways than simple scaling. The transformation matrix will make it point in the right direction, or as i have done bellow, the transformation matrix is actually applied to the points and the vector is being calculated directly in the step space.<jupyter_code>def moveAB(realXYSource,realXYDestination,stepToRealMatrix):
    stepXYSource = stepToRealMatrix.I*np.matrix(realXYSource).T
    stepXYDestination = stepToRealMatrix.I*np.matrix(realXYDestination).T
    deltaStep = stepXYDestination - stepXYSource
    currentStepPoint = stepXYSource
    visitedStepPoints = []
    visitedStepPoints.append(currentStepPoint.copy())
    visitedRealPoints = []
    visitedRealPoints.append(MS*currentStepPoint.copy())
    deltaModule = np.sqrt(deltaStep[0,0]**2 + deltaStep[1,0]**2)
    currentModule = 0
    directionVector = deltaStep[:,0]/deltaModule
    while currentModule<deltaModule:
        currentModule += 1
        currentStepPoint[0,0] = np.round(visitedStepPoints[0][0]+directionVector[0,0]*currentModule)
        visitedStepPoints.append(currentStepPoint.copy())
        visitedRealPoints.append(stepToRealMatrix*currentStepPoint.copy())
        currentStepPoint[1,0] = np.round(visitedStepPoints[0][1]+directionVector[1,0]*currentModule)
        visitedStepPoints.append(currentStepPoint.copy())
        visitedRealPoints.append(stepToRealMatrix*currentStepPoint.copy())
    finalPoint = MS*currentStepPoint
    visitedStepPoints = np.array(visitedStepPoints).T
    visitedRealPoints = np.array(visitedRealPoints).T
    return finalPoint, visitedStepPoints, visitedRealPoints
lineTest=((0,0),(100,100))
moveAB(*lineTest,MS)
lineTestArray = [((0,0),(-2,-2)),\
                 ((0,0),(2,2)),\
                 ((0,0),(2,-2)),\
                 ((0,0),(-2,2)),\
                 ((0,0),(-3,-2)),\
                 ((0,0),(3,2)),\
                 ((0,0),(3,-2)),\
                 ((0,0),(-3,2)),\
                 ((1.5,0),(0,1.5)),\
                 ((0,1.5),(-1.5,0)),\
                 ((-1.5,0),(0,-1.5)),\
                 ((0,-1.5),(1.5,0))]
fig, ax = plt.subplots(1,2,figsize=(12,6))
ax[0].grid()
ax[1].grid()
ax[0].axis('equal')
ax[1].axis('equal')
ax[0].set_title("Real Space")
ax[1].set_title("Step Space")
for line in lineTestArray:
    movements = moveAB(*line,MS)   
    ax[0].scatter(*np.array(line[0]))
    ax[0].scatter(*np.array(line[1]))
    ax[0].plot(movements[2][0,0],movements[2][0,1])
    ax[0].plot(np.array(line)[:,0],np.array(line)[:,1])
    ax[1].scatter(*np.array(MS.I*np.matrix(line[0]).T))
    ax[1].scatter(*np.array(MS.I*np.matrix(line[1]).T))
    ax[1].plot(movements[1][0,0],movements[1][0,1])
    ax[1].plot(MS.I*np.matrix(line)[:,0],MS.I*np.matrix(line)[:,1])<jupyter_output><empty_output><jupyter_text>With all these concepts in mind, we can now start thinking about how to interpolate a trajectory that is not a line, by dividing it into line segments and then stepping through those line segments. More to come...Today! Lets try to follow a parabolic function<jupyter_code>x = np.linspace(-3,3,100)
y = x**2
plt.figure(figsize=(8,8))
plt.grid()
plt.axis('equal')
plt.plot(x,y)<jupyter_output><empty_output><jupyter_text>To divide such an object into segments we need to sample it at certain points and see what are the x and y coordinates we must visit, and at what order. Let us sample this curve at 1mm intervals.<jupyter_code>xSample = np.arange(-3,4,1)
xSample
ySample = xSample**2
ySample<jupyter_output><empty_output><jupyter_text>So the pairs of points that we must visit are:<jupyter_code>pointPairs=np.array([xSample,ySample])
pointPairs<jupyter_output><empty_output><jupyter_text>Now we can plot the trajectories our robot will take between those points<jupyter_code>lineTestArray = []
for i in range(pointPairs.shape[1]-1):
    lineTestArray.append(((pointPairs[0,i],pointPairs[1,i]),(pointPairs[0,i+1],pointPairs[1,i+1])))
fig, ax = plt.subplots(1,2,figsize=(12,6))
ax[0].grid()
ax[1].grid()
ax[0].axis('equal')
ax[1].axis('equal')
ax[0].set_title("Real Space")
ax[1].set_title("Step Space")
for line in lineTestArray:
    movements = moveAB(*line,MS)   
    ax[0].scatter(*np.array(line[0]))
    ax[0].scatter(*np.array(line[1]))
    ax[0].plot(movements[2][0,0],movements[2][0,1])
    ax[0].plot(np.array(line)[:,0],np.array(line)[:,1])
    ax[1].scatter(*np.array(MS.I*np.matrix(line[0]).T))
    ax[1].scatter(*np.array(MS.I*np.matrix(line[1]).T))
    ax[1].plot(movements[1][0,0],movements[1][0,1])
    ax[1].plot(MS.I*np.matrix(line)[:,0],MS.I*np.matrix(line)[:,1])<jupyter_output><empty_output><jupyter_text>Nice, the concept works. Now a set of primitives must be created, that are going to be the ones implemented in the code that will be run on the CNC machine One important thing to note is, this parabole seems very crunchy, but in real life, cnc machinery do not step one axis at a time like this, here we are representing the SIGNAL we must send to it, as we have one thread only to control the motors. However, this thread is orders of magnitude faster than the motor response, so the steps will be taken in a diagonal between the two points, as both motors have not finished moving yet as the next command strikes the drivers, smoothing out the curve. Other smoothing factor is the inertia of the system, that will affect how the shape of those "diagonals", making them look like acceleration curves. Vibrations will arise from all this stepping and inertia interplay, and must be accounted for with delays in activation. Sometimes this oscilations become harmonic and some steps must be taken to mitigate the effects of such vibrations, otherwise they get transfered to the workpiece or worse, breaking something in the machine itself. Let us do a circle, because why not!We need this in order to make circular interpolations, which are a big part of CNC control.<jupyter_code>theta = np.linspace(0,2*np.pi,100)
x = np.sin(theta)*5
y = np.cos(theta)*5
plt.figure(figsize=(8,8))
plt.grid()
plt.axis('equal')
plt.plot(x,y)<jupyter_output><empty_output><jupyter_text>Beauty, now we sample it<jupyter_code>theta = np.linspace(0,2*np.pi,int(np.round(2*np.pi*5))) #<--- this int(blablabla) thing here is dividing the circle into 1mm segments
xSample = np.sin(theta)*5
ySample = np.cos(theta)*5
pointPairs=np.array([xSample,ySample])
pointPairs

lineTestArray = []
for i in range(pointPairs.shape[1]-1):
    lineTestArray.append(((pointPairs[0,i],pointPairs[1,i]),(pointPairs[0,i+1],pointPairs[1,i+1])))
fig, ax = plt.subplots(1,2,figsize=(12,6))
ax[0].grid()
ax[1].grid()
ax[0].axis('equal')
ax[1].axis('equal')
ax[0].set_title("Real Space")
ax[1].set_title("Step Space")
for line in lineTestArray:
    movements = moveAB(*line,MS)   
    ax[0].scatter(*np.array(line[0]))
    ax[0].scatter(*np.array(line[1]))
    ax[0].plot(movements[2][0,0],movements[2][0,1])
    ax[0].plot(np.array(line)[:,0],np.array(line)[:,1])
    ax[1].scatter(*np.array(MS.I*np.matrix(line[0]).T))
    ax[1].scatter(*np.array(MS.I*np.matrix(line[1]).T))
    ax[1].plot(movements[1][0,0],movements[1][0,1])
    ax[1].plot(MS.I*np.matrix(line)[:,0],MS.I*np.matrix(line)[:,1])<jupyter_output><empty_output><jupyter_text>Cruunnnnchy, but hey, it's 5mm in size only, let's do a 10mm one to see<jupyter_code>theta = np.linspace(0,2*np.pi,int(np.round(2*np.pi*10)))
xSample = np.sin(theta)*10
ySample = np.cos(theta)*10
pointPairs=np.array([xSample,ySample])
lineTestArray = []
for i in range(pointPairs.shape[1]-1):
    lineTestArray.append(((pointPairs[0,i],pointPairs[1,i]),(pointPairs[0,i+1],pointPairs[1,i+1])))
fig, ax = plt.subplots(1,2,figsize=(18,9))
ax[0].grid()
ax[1].grid()
ax[0].axis('equal')
ax[1].axis('equal')
ax[0].set_title("Real Space")
ax[1].set_title("Step Space")
for line in lineTestArray:
    movements = moveAB(*line,MS)   
    ax[0].scatter(*np.array(line[0]))
    ax[0].scatter(*np.array(line[1]))
    ax[0].plot(movements[2][0,0],movements[2][0,1])
    ax[0].plot(np.array(line)[:,0],np.array(line)[:,1])
    ax[1].scatter(*np.array(MS.I*np.matrix(line[0]).T))
    ax[1].scatter(*np.array(MS.I*np.matrix(line[1]).T))
    ax[1].plot(movements[1][0,0],movements[1][0,1])
    ax[1].plot(MS.I*np.matrix(line)[:,0],MS.I*np.matrix(line)[:,1])<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1382300580026417, 'fraction_numerical': 0.027852753689772008, 'mean_word_length': 5.118763176387913, 'pattern_counts': {'":': 0, '<': 71, '<?xml version=': 0, '>': 69, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '17', 'dir': 'jupyter-structured-clean-dedup', 'id': '29287', 'license': 'MIT', 'n_tokens_mistral': 5927, 'n_tokens_neox': 5747, 'n_words': 1705, 'path': 'notebooks/Cartesian2D.ipynb', 'repo_name': 'SiliconForge/roboticspoc', 'size': '17413'}