# MeArm (V): 2 link robot arm simulation with VPython

After a lot of time spent to solve it, at last I can write this post about how to simulate a 2 link robotic arm or, in other words, a robotic arm similar to a MeArm. I had this idea after researching on the theory behind controlling this kind of robot based normally on inverse kinematics. The problem I’ve encounter is that, if you just applied the theory, usually something breaks or if you are lucky, it is not as accurate as I would like. The problem usually is that inverse trigonometric functions (arcsine, arccosine and arctangent) are multi-evaluated, that is, more than one angles gives the correct answer. That makes the code slightly more difficult because you need to get the correct answer to make the robot going to the correct target position. If you are not careful, the only problem is not lack of accuracy: if the angle is misinterpreted, you robot may attempt to go far form the limits of the servo motors, possibly damaging them.

So in this post I’m going to cover step by step how you can simulate the arm and then I will use what we have learnt here in a future post to program an Arduino to move the servos, all controlled by Python via the serial port.

**Requirements:**

1. Python 2.7 installed with VPython and the following libraries: numpy.

**Theoretical Background:**

So the goal of this project is to make a 2-link robotic arm reach a target position that is controlled by the keyboard. The first thing we need is to get an idea about all the parameters we need. The actual robot is capable of moving in 3 dimensions but at the beginning we are going to assume only movement in a plane, that is, the machine will not rotate in the z direction and the movement will only happen in the x-y plane. In the following sketch all the variables needed are shown:

The only parameters we know are x and y which are the position of the target in a the x-y plane and the length of both arms l1 and l2. We need to make use of some triangle trigonometry:

- the distance from the origin to the target T is calculated by
- we have different ways to get using the arcsine, arccosine or the arctangent. I will use the last one to be able to use the trigonometric function atan2.
- the idea now is to use the sine rule to get all the angles, for that we know that the sine rule is:
- we know all the sides of the triangle and there’s a formula that allows us to relate the perimeter with the area of a triangle called Heron’s formula, which is: where s is the semiperimeter
- using the Area, the sine rule now is:
- so to calculate all the angles of the triangle, we need to solve: , and

And that’s all that we need to calculate the angles for each arm to reach the target.

**Python code:**

In this time I’m going to divide the code in different chunks because each one has a different function in the program and I think that improves the understanding:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
#import required libraries from visual import * import numpy as np #define dimensions for the MeArm l1 = 8.5 #length of link 1 in cm l2 = 8.5 #length of link 2 in cm #Create virtual environment: scene = display(title='Robot movements', width=600, height=600, center=(8,4,0)) #set up the scene #To improve clarity, create a set of x, y and z axis x_axis= arrow(pos=(0,0,0), axis=(15,0,0), shaftwidth=0.1, headwidth=0.3) y_axis= arrow(pos=(0,0,0), axis=(0,15,0), shaftwidth=0.1, headwidth=0.3) pos_z_axis= arrow(pos=(0,0,0), axis=(0,0,-15), shaftwidth=0.1, headwidth=0.3) neg_z_axis= arrow(pos=(0,0,0), axis=(0,0,15), shaftwidth=0.1, headwidth=0.3) |

We start importing required libraries vpython and numpy at the beginning and we define l1 and l2 as the dimensions of each link. The next step is made just to improve the visualization of the robot movements: we make the center of the scene on the 3 axis and we make three white arrows to indicate the x, y and z directions.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
#Indicators for the target, link 1 and link 2 respectively indicator = arrow(pos=(0,0,0), axis=(10,10,0), shaftwidth=0.2, headwidth=0.3, color=color.yellow) l1_ind = arrow(pos=(0,0,0), axis=(2.65,7.64,0), shaftwidth=0.2, headwidth=0.3, color=color.red) l2_ind = arrow(pos=(2.65,7.64,0), axis=(7.3,2.3,0), shaftwidth=0.2, headwidth=0.3, color=color.green) #Labels to show how to move the robot label_1=label(pos=(8,18,0), text='Use arrows to move in plane. Use a and d to rotate. Use w and s to open/close the clamp') label_2=label(pos=(8,-8,0), text='Clamp status = Close') #Labels to improve the visualization of the position of the arm in_x_plane=arrow(pos=(0,0,0), axis=(10,0,0), shaftwidth=0.1, headwidth=0.1, color=color.orange, opacity=0.3) in_y_plane=arrow(pos=(0,0,0), axis=(0,10,0), shaftwidth=0.1, headwidth=0.1, color=color.orange, opacity=0.3) in_z_plane=arrow(pos=(0,0,0), axis=(0,0,10), shaftwidth=0.1, headwidth=0.1, color=color.orange, opacity=0.3) #Initial position x=10 y=10 phi=0 #angle for base rotation clamp = 'Close' #Clamp is close |

Now we create a yellow arrow which indicates the target: this arrows goes from the origin to the x and y coordinates we define at each step using the arrows in the keyboard. For each link l1 and l2 we also create a red and green indicator respectively. Here I’ve added an optional label to remember how to control the simulation. The second label just indicates if the clamp is open or close. To improve the visualization of the robot when we rotate it, I’ve added three lines which shows the position of the target in all planes, this is optional but it really helps to see where the robot is when you rotate it. Finally we define son initial parameters for the simulation, in this case I set x and y equal to 10, the angle of rotation of the base equals 0 and the clamp is closed.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 |
#now we made an infinite while loop to keep the program running while (1==1): rate(20) #refresh rate required for VPython ev = scene.waitfor('keydown') if ev.key == 'up': y = y+0.25 elif ev.key == 'down': y = y-0.25 elif ev.key == 'right': x = x+0.25 elif ev.key == 'left': x = x-0.25 elif ev.key == 'a': phi = phi-5 if phi <= -90: print 'Minimum angle reached' phi = -90 elif ev.key == 'd': phi = phi+5 if phi >= 90: print 'Maximum angle reached' phi = 90 elif ev.key == 'q': print 'Going to initial position...' x=10 y=10 phi=0 elif ev.key == 'w': print 'Opening clamp...' clamp = 'Open' elif ev.key == 's': print 'Closing clamp...' clamp = 'Close' |

We start an infinite loop with *while (1==1)*, which always returns true and we set the refresh rate of VPython to 20. The next if loop establishes how we control the simulation. At the event of a key pressed, if the key is the up arrow, the program will add 0.25 to the y variable, that is, the target will go up. On the other hand, if we press the down arrow, the program will subtract 0.25 to y variable making the target go down. Similarly, the x variable moves the target to the right and to the left with the right and left arrows respectively. If we pressed a or d the angle to rotate the base will go to one side or to the other, I selected a for the negative values and d for the positive values. To make things more accurate, there’s an if loop to prevent the robot to exceed the limits of the servo motor. This is not required for the simulation but it will help in further steps. I’ve also added an event that resets everything and makes the robot to go to the original position. And finally with w and s we just print if the clamp is opening or closing, this last step is also made to use this code in the future to move the physical robot.

1 2 3 4 5 6 7 8 |
#Calculate the distance to the target and the angle to the x axis T = np.sqrt(x*x+y*y) #Distance to target if l1+l2<T+0.5: #Loop to prevent targets out of range print 'Position cannot be reached, reseting...' x=10 y=10 T = np.sqrt(x*x+y*y) theta = np.arctan2(y,x) |

Now we can start to calculate things: the first thing is to calculate the distance to the target. The if loop added here prevents the arm to try to reach a position out of the range of the links. I’ve added 0.5 cm to the target distance and this prevents to things: In the simulation, when l1+l2 is close to T the angle of is very high and we have problems with the multi-evaluation of arcsine (more of this later). If we think about what will be happening in the actual robot, if we push it so far we will set the servo motors close to their limits and that could damage them, so better to err on the safe side.

In this case if the target goes out of range the robot just goes to the initial position although you can choose the behavior you want. Finally, once the target is checked to be in range, the angle is calculated.

1 2 3 4 5 6 7 8 9 10 11 |
#Calculate the Area of the triangle using Heron's formula s=(l1+l2+T)/2 #Calculate the semiperimeter A= np.sqrt(s*(s-l1)*(s-l2)*(s-T)) #Area of the triangle 2-link arm #Now we calculate the angles alpha = np.arcsin((2*A)/(l1*T)) gamma = np.arcsin((2*A)/(T*l2)) beta = np.arcsin((2*A)/(l1*l2)) if beta>0.5: beta = np.pi-alpha-gamma ang=3.141592+alpha+theta+beta #Correct angle from the l1 indicator |

As explained before, we use the Heron’s formula to calculate the area and with that we can calculate all the angles of the triangle. And here is where problems appear and I haven’t found a good correction anywhere without making the code very complex: when x and y are small, the values of the three angles are lower than 90 degrees, so the functions give the correct angle. When x and y get bigger, the angle gets bigger than 90 degrees. As you can see in the reference about the arcsine function in numpy, this function is defined between -90 and 90 degrees (or between and radians). So the arcsine function will not give us angles higher than 90 degrees which are required for . There are two solutions for this: we can calculate which will be smaller than 90 degrees in this case because the sum of all angles in a triangle has to equal 180 degrees (or radians). We can then calculate in radians as .

Yet, this approach doesn’t work well at very small angles where is close to 90 degrees for the same previous reason. To prevent this I’ve added an if loop that only uses the previous definition of for angles higher than 25 degrees, at very small x and y we don’t need to calculate gamma and the value of beta is correct to reach the target.

I set the value of smaller than 0.5 radians (around 28 degrees) after doing a couple of trials and checking that everything works. So if you use different dimensions or make the robot able to reach further points, you might have to change this parameter. For this robot however, this value works well.

Last, I need to define an angle called ang for the indicator of l2. This is because VPython will plot the indicator in the positive axis of the x-y plane by default; however, our angle goes from l1 to l2. Adding 180 degrees plus the angles from the x axis to l1 to beta will plot l2 correctly.

1 2 3 4 5 6 7 8 9 10 11 12 |
#Update the indicators indicator.axis=(x*np.cos(phi*0.01745),y,x*np.sin(phi*0.01745)) #calculate the new axis of the indicator l1_ind.axis=(l1*np.cos(alpha+theta)*np.cos(phi*0.01745),l1*np.sin(alpha+theta),l1*np.cos(alpha+theta)*np.sin(phi*0.01745)) #calculate the new axis of l1 l2_ind.pos=(l1*np.cos(alpha+theta)*np.cos(phi*0.01745),l1*np.sin(alpha+theta),l1*np.cos(alpha+theta)*np.sin(phi*0.01745)) #calculate new origin for l2 l2_ind.axis=(l2*np.cos(ang)*np.cos(phi*0.01745),l2*np.sin(ang),l2*np.cos(ang)*np.sin(phi*0.01745)) #Calculate new axis for l2 in_x_plane.pos=(0,0,x*np.sin(phi*0.01745)) in_y_plane.pos=(x*np.cos(phi*0.01745),0,x*np.sin(phi*0.01745)) in_z_plane.pos=(x*np.cos(phi*0.01745),0,0) in_x_plane.axis=(x*np.cos(phi*0.01745),0,0) in_y_plane.axis=(0,y,0) in_z_plane.axis=(0,0,x*np.sin(phi*0.01745)) label_2.text='Clamp status = '+clamp |

The last chunk of code just updates all the indicators: The first one plots the target indicator using all the angles we have measured including as the angle of rotation for the base. Next we update the axis of the link l1 and we use the same parameters to update the position of the link l2. Remember that the position argument here just sets where the arrow has its origin. Then we update the axis of link l2 which should end in the same point as the target if everything is correct.

Last optional steps improve the lines that help to visualize the position of the robot in space and finally the label that gives information about the clamp is updated with the actual status. To show how the program works in real time, here is a video capture of the program working in my computer:

You can copy and paste all the code from the post or find it in my Github account clicking here.