Basics of kinematics

Robotics in Python

The five bar mechanism shown in the figure 1 is a most commonly used lower DoF planar manipulator. In this

Simulation video

#Code- Python

# kinematics.py

import numpy as np

from math import acos,asin,atan,atan2,sin,cos

# 'r' for lengths

# 'a' for angles

class five_bar():

def __init__(self,link1,link2,link3,link4,link5):

self.r1 = link1

self.r2 = link2

self.r3 = link3

self.r4 = link4

self.r5 = link5

def foward(self,a1,a4):

r1 = self.r1

r2 = self.r2

r3 = self.r3

r4 = self.r4

r5 = self.r5

A_1 = r1**2-r2**2

B_1 = -2*r1*cos(a1)

C_1 = -2*r1*sin(a4)

A_2 = r5**2+r4**2-r3**2

B_2 = -2*r5 -2*r4*cos(a1)

C_2 = -2*r4*sin(a4)

D = (C_1-C_2)/(B_1-B_2)

E = (A_2-A_1)/(B_1-B_2)

F = 2*D*E + B_1*D + C_1

G = A_1 +B_1*E+ E**2

D_1 = D**2

self.y1 = (-F-np.sqrt(F**2-4*G*D_1))/(2*D_1)

self.y2 = (-F+np.sqrt(F**2-4*G*D_1))/(2*D_1)

self.x1 = D*self.y1 + E

self.x2 = D*self.y2 + E

def inverse(self,p_x,p_y):

r1 = self.r1

r2 = self.r2

r3 = self.r3

r4 = self.r4

r5 = self.r5

#### First Loop #####

A1 = p_x**2 + p_y**2 + r1**2 + 2*r1*p_x - r2**2

B1 = -4*r1*p_y

C1 = r1**2 - 2*r1*p_x + p_x**2 + p_y**2 - r2**2

t_11 = (-B1+np.sqrt(B1**2-4*A1*C1))/(2*A1)

t_12 = (-B1-np.sqrt(B1**2-4*A1*C1))/(2*A1)

self.a11 = atan2((2*t_11),(1-t_11**2))

self.a12 = atan2((2*t_12),(1-t_12**2))

#### Second Loop ####

A2 = p_x**2 + p_y**2 + r5**2 + r4**2 - 2*p_x*r5 - r3**2 + 2*p_x*r4 - 2*r4*r5

B2 = -4*p_y*r4

C2 = p_x**2 + p_y**2 + r5**2 + r4**2 - 2*p_x*r5 - r3**2 + 2*r4*r5 - 2*p_x*r4

t_21 = (-B2+np.sqrt(B2**2-4*A2*C2))/(2*A2)

t_22 = (-B2-np.sqrt(B2**2-4*A2*C2))/(2*A2)

self.a41 = atan2((2*t_21),(1-t_21**2))

self.a42 = atan2((2*t_22),(1-t_22**2))

def get_a11(self):

return self.a11

def get_a12(self):

return self.a12

def get_a2(self,a1,p_x,p_y):

a2 = atan2((p_y - self.r1*sin(a1)),(p_x-self.r1*cos(a1)))

return a2

def get_a3(self,a4,p_x,p_y):

a3 = atan2((p_y-self.r4*sin(a4)),(p_x-self.r5-self.r4*cos(a4)))

return a3

def get_a41(self):

return self.a41

def get_a42(self):

return self.a42

def get_x1(self):

return self.x1

def get_x2(self):

return self.x2

def get_y1(self):

return self.y1

def get_y2(self):

return self.y2

# graph.py

import numpy as np

import matplotlib.pyplot as plt

import matplotlib.animation as animation

import matplotlib.lines as lines

import kinematics

def onclick(event):

global cid_m

cid_m = fig.canvas.mpl_connect('motion_notify_event',pull)

def check_nan():

# to control, is the point in workspace of robot

return not np.isnan(a[0]) and not np.isnan(a[1]) and not np.isnan(a[2]) and not np.isnan(a[3])

def pull(event):

global a

x = event.xdata-origin[0]

y = event.ydata-origin[1]

#print(x,y)

robot.inverse(x,y)

a[0] = robot.get_a11()

a[1] = robot.get_a2(a[0],x,y)

a[3] = robot.get_a42()

a[2] = robot.get_a3(a[3],x,y)

#print(a)

if check_nan():

x_end.append(event.xdata)

y_end.append(event.ydata)

else:

pass

def disconnect(event):

fig.canvas.mpl_disconnect(cid_m)

def update(s):

if check_nan():

## line 1

x = [origin[0],origin[0]+r1*np.cos(a[0])]

y = [origin[1],origin[1]+r1*np.sin(a[0])]

line1.set_data(x,y)

## line 2

x = [x[1],x[1]+r2*np.cos(a[1])]

y = [y[1],y[1]+r2*np.sin(a[1])]

line2.set_data(x,y)

## line 4

x = [origin[0]+r5,origin[0]+r5+r4*np.cos(a[3])]

y = [origin[1],origin[1]+r4*np.sin(a[3])]

line4.set_data(x,y)

## line 3

x = [x[1],x[1]+r3*np.cos(a[2])]

y = [y[1],y[1]+r3*np.sin(a[2])]

line3.set_data(x,y)

## line 5

x = [origin[0],origin[0]+r5]

y = [origin[1],pad]

line5.set_data(x,y)

line6.set_data(x_end,y_end)

else:

pass

return line6,line1,line2,line3,line4,line5

# pyhsical constrait of five bar mechanism, to get further information check kinetics.py

# Please change r[1:5] values, if you want to try another configuration

r1 = 60

r2 = 100

r3 = 100

r4 = 60

r5 = 80

robot = kinematics.five_bar(r1,r2,r3,r4,r5)

x_in = 40 # initial end efector y position

y_in = 150 # initial end efector y position

# initial declarations of graph

pad = 30 # do not make me zero, unless you do not want everything will mess up

if r2>r3:

big_l = r2

else:

big_l = r3

a = [0,0,0,0]

xmax = r1+big_l+r5/2+pad

ymax = r1+big_l+pad*2

fig, ax = plt.subplots()

ax.axis([0,xmax,0,ymax])

origin = [xmax/2-r5/2,pad]

line1, = ax.plot([], [],lw=2,animated=True,c='b')

line2, = ax.plot([], [],lw=2,animated=True,c='b')

line3, = ax.plot([], [],lw=2,animated=True,c='b')

line4, = ax.plot([], [],lw=2,animated=True,c='b')

line5, = ax.plot([], [],lw=2,animated=True,c='b')

line6, = ax.plot([], [],lw=1,animated=True,c='g')

robot.inverse(x_in,y_in)

a[0] = robot.get_a11()

a[1] = robot.get_a2(a[0],x_in,y_in)

a[3] = robot.get_a42()

a[2] = robot.get_a3(a[3],x_in,y_in)

x_end = []

y_end = []

# graph control functions

fig.canvas.mpl_connect('button_press_event', onclick)

fig.canvas.mpl_connect('button_release_event',disconnect)

ani = animation.FuncAnimation(fig, update,frames=None,blit=True,interval=25)

plt.show()