Assignment 4&5

Assgnment -6/q1.py

@@ -0,0 +1,113 @@
+from math import cos, sin, atan, acos, pi
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+r = np.linspace(-5, 5, 100)
+t = np.linspace(0,2*pi,100)
+
+xd = 2.5*np.cos(t)
+yd = 2.5*np.sin(t)
+
+Xd = [xd, yd]
+
+x_dot = - 2.5*np.sin(t)
+y_dot = 2.5*np.cos(t)
+X_dot = [x_dot,y_dot]
+
+X = [[0]*len(t),[0]*len(t)]
+
+
+def psuedo_Jinv (q):
+  dh = [[0, q[0], 1, 0],[0, q[1], 1, 0],[0, q[2], 1, 0]] 
+  H = [0]*4 
+  H[0] = np.identity(4)
+  H0 = np.identity(4) 
+
+  for i in range(3):
+    d = dh[i][0]
+    theta = dh[i][1]
+    r = dh[i][2]
+    alpha = dh[i][3]
+
+    Z = [[cos(theta), -1*sin(theta), 0, 0],
+         [sin(theta), cos(theta), 0, 0],
+         [0, 0, 1, d],
+         [0, 0, 0, 1]]
+
+    X = [[1, 0, 0, r],
+         [0, cos(alpha), -1*sin(alpha), 0],
+         [0, sin(alpha), cos(alpha), 0],
+         [0, 0, 0, 1]]
+
+    H[i+1] = np.matmul(Z,X) 
+    H0 = H0@H[i+1] 
+
+  O_n0 = [H0[0][3],H0[1][3],H0[2][3]] 
+
+  O = np.zeros(3) 
+  Jv = np.zeros((2,3)) 
+  h = np.identity(4) 
+
+  for i in range(3):
+    h = h@H[i]
+    Z = [h[0][2], h[1][2], h[2][2]] 
+    O_i0 = [h[0][3], h[1][3], h[2][3]] 
+
+    O[0] = O_n0[0] - O_i0[0] 
+    O[1] = O_n0[1] - O_i0[1]
+    O[2] = O_n0[2] - O_i0[2]
+
+    Zx_Oi = (Z[1]*O[2]) - (O[1]*Z[2]) 
+    Zx_Oj = (O[0]*Z[2]) - (Z[0]*O[2])
+    Zx_Ok = (Z[0]*O[1]) - (O[0]*Z[1])
+
+    Zx = [Zx_Oi, Zx_Oj, Zx_Ok] 
+
+    Jv[0][i] = Zx[0]
+    Jv[1][i] = Zx[1]
+
+  Jvt = np.transpose(Jv)
+  Jvplus = [email protected](Jv@Jvt)
+
+  return Jvplus
+
+
+x0 = float(input())
+y0 = float(input())
+
+X[0][0] = x0
+X[1][0] = y0
+
+x2 = x0 - 1
+y2 = y0
+
+q2_0 = acos((x2**2 + y2**2 - 2)/2)
+q1_0 = atan(y2/x2) - atan(sin(q2_0)/(1+cos(q2_0)))
+q3_0 = 2*pi - (q1_0 + q2_0)
+q_0 = [q1_0,q2_0,q3_0]
+
+q = [0]*len(t)
+q[0] = q_0
+kp = 4
+
+for k in range(len(t)):
+  X[0][k] = cos(q[k][0]+q[k][1]+q[k][2]) + cos(q[k][0]+q[k][1]) + cos(q[k][0])
+  X[1][k] = sin(q[k][0]+q[k][1]+q[k][2]) + sin(q[k][0]+q[k][1]) + sin(q[k][0])
+
+  error = [Xd[0][k] - X[0][k], Xd[1][k] - X[1][k]]
+  x_dot = [X_dot[0][k],X_dot[1][k]]
+
+  Jvp = psuedo_Jinv(q[k])
+
+  term1 = Jvp@x_dot
+  term2 = Jvp@error
+  delq = term1 + kp*term2
+
+  q[k+1] = q[k]+ delq
+
+plt.plot(X[0],X[1], color = 'red',label = 'actual') #actual path 
+plt.plot(xd,yd, color = 'blue', label = 'desired') #desired path
+plt.xlim([-2,2])
+plt.ylim([-2,2])
+plt.show()

Assignment - 2/Task-10.py

@@ -0,0 +1,21 @@
+import numpy as np
+
+l1 = float(input('Length of link 1: '))
+l2 = float(input('Length of link 2: '))
+l3 = float(input('Length of link 3: '))
+q1 = float(input('Angle of link 1: '))
+q1 = (q1*np.pi)/180
+q2 = float(input('Angle of link 2: '))
+q2 = (q2*np.pi)/180
+q3 = float(input('Angle of link 3: '))
+q3 = (q3*np.pi)/180
+
+J = [[-1*l1*np.sin(q1)-1*l2*np.sin(q1+q2)-1*l3*np.sin(q1+q2+q3), -1*l2*np.sin(q1+q2)-1*l3*np.sin(q1+q2+q3), -1*l3*np.sin(q1+q2+q3)],
+ [l1*np.cos(q1)+l2*np.cos(q1+q2)+l3*np.cos(q1+q2+q3), l2*np.cos(q1+q2)+l3*np.cos(q1+q2+q3), l3*np.cos(q1+q2+q3)],
+ [0, 0, 0],
+ [0, 0, 0],
+ [0, 0, 0],
+ [1, 1, 1]]
+
+for i in J:
+    print(i)

Assignment - 2/Task-3.py

@@ -0,0 +1,22 @@
+import numpy as np
+
+l1 = float(input('Length of link 1: '))
+l2 = float(input('Length of link 2: '))
+q1 = float(input('Angle of link 1: '))
+q1 = (q1*np.pi)/180
+q2 = float(input('Angle of link 2: '))
+q2 = (q2*np.pi)/180
+q3 = float(input('Displacement of link 3: '))
+
+p3 = [[1], [2], [3], [1]]
+
+H = [[np.cos(q1+q2), -1*np.sin(q1+q2), 0, l1*np.cos(q1)+l2*np.cos(q1+q2)], [np.sin(q1+q2), np.cos(q1+q2), 0, l1*np.sin(q1)+l2*np.sin(q1+q2)],[0, 0, 1, q3], [0, 0, 0, 1]]
+
+p0 = [[0], [0], [0], [0]]
+for i in range(len(H)):
+    for j in range(len(p3[0])):
+        for k in range(len(p3)):
+            p0[i][j] += H[i][k]*p3[k][j]
+
+for r in p0:
+    print(r)

Assignment - 2/Task-4.py

@@ -0,0 +1,21 @@
+import numpy as np
+
+l2 = float(input('Length of link 2: '))
+q1 = float(input('Angle of link 1: '))
+q1 = (q1*np.pi)/180
+q2 = float(input('Angle of link 2: '))
+q2 = (q2*np.pi)/180
+q3 = float(input('Displacement of link 3: '))
+
+p3 = [[1], [2], [3], [1]]
+
+H = [[np.cos(q1)*np.cos(q2), -1*np.sin(q1), np.cos(q1)*np.sin(q2), q3*np.cos(q1)*np.sin(q2)-l2*np.sin(q1)], [np.sin(q1)*np.cos(q2), np.cos(q1), np.sin(q1)*np.sin(q2), q3*np.sin(q1)*np.sin(q2)+l2*np.cos(q1)], [-1*np.sin(q2), 0, np.cos(q2), q3*np.cos(q2)], [0, 0, 0, 1]]
+
+p0 = [[0], [0], [0], [0]]
+for i in range(len(H)):
+    for j in range(len(p3[0])):
+        for k in range(len(p3)):
+            p0[i][j] += H[i][k]*p3[k][j]
+
+for r in p0:
+    print(r)

Assignment - 2/Task-8.py

@@ -0,0 +1,14 @@
+import numpy as np
+
+l1 = float(input('Length of link 1: '))
+l2 = float(input('Length of link 2: '))
+q1 = float(input('Angle of link 1: '))
+q1 = (q1*np.pi)/180
+q2 = float(input('Angle of link 2: '))
+q2 = (q2*np.pi)/180
+q3 = float(input('Displacement of link 3: '))
+
+J = [[-1*l1*np.sin(q1)-1*l2*np.sin(q1+q2), -1*l2*np.sin(q1+q2), 0, 0], [l1*np.cos(q1)+l2*np.cos(q1+q2), l2*np.cos(q1+q2), 0, 0], [0, 0, -1, 0], [0, 0, 0, 0], [0, 0, 0, 0], [1, 1, 0, -1]]
+
+for i in J:
+    print(i)

Assignment - 3/Task-3.py

@@ -0,0 +1,53 @@
+import numpy as np
+import sympy as sp
+
+n = int(input("Enter the no. of links: "))
+matr = input("Enter the DH Parameters in Matrix form, eg:-[[alpha1,theta1,d1,a1],[alpha2,theta2,d2,a2]..] :")
+links = matr.removeprefix("[[").removesuffix("]]").split("],[")
+alphas,thetas,offset,length = [],[],[],[]
+
+def DH_cal(theta,alpha,d,a):
+    dh = sp.Matrix([[np.cos(theta), -np.sin(theta)*np.cos(alpha), np.sin(theta)*np.sin(alpha), np.cos(theta)*a],
+    [np.sin(theta), np.cos(theta)*np.cos(alpha), -np.cos(theta)*np.sin(alpha), np.sin(theta)*a],
+    [0, np.sin(alpha), np.cos(alpha),d],
+    [0,0,0,1]])
+    return dh
+
+qi_dot = input("Enter the joint velocities, eg: q1,q2,q3..:")
+qi_dot_array = []
+
+for q in qi_dot.split(','):
+    qi_dot_array.append(float(q))
+
+qi_dot_array = sp.Matrix(qi_dot_array)
+
+O,z0,o0 = [],np.array([0,0,1]),np.array([0,0,0])
+J = []
+
+if len(links) == n:
+    for link in links:
+        params = link.split(",")
+        alphas.append(float(params[0]))
+        thetas.append(float(params[1]))
+        offset.append(float(params[2]))
+        length.append(float(params[3]))
+    t = sp.Matrix([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
+
+    for i in range(0,n):
+        t = t*DH_cal(thetas[i],alphas[i],offset[i],length[i])
+        O.append(np.array(t.col(3)[0:3]))
+    # print(np.cross(z0,O[n-1]-o0))
+    c=0
+    J.append(np.append(np.cross(z0,O[n-1]-o0),[0,0,1]))
+
+    for o in O[0:n-1]:
+        c+=1
+        J.append(np.append(np.cross(z0,O[n-1]-o),[0,0,1]))
+        # print(np.append(np.cross(z0,O[n-1]-o),[0,0,1]))
+    print("End Effector Position = "+ str(np.dot(t,[0,0,0,1])[0:3]))
+    # print(J)
+    J = sp.Matrix(np.transpose(J))
+    print(J)
+    print(J*qi_dot_array)
+else:
+    print("Please provide correct input.")

Assignment 4&5/q1.py

@@ -0,0 +1,14 @@
+from math import pi, sqrt, atan 
+
+a2 = 1
+
+p_x, p_y, p_z = 1, 0.404, 0.404
+
+d1, d2= 1, 1
+
+r = sqrt((p_x)**2 + (p_y)**2)
+s = p_z - d1
+d3 = sqrt((r**2)+(s**2)) - a2
+
+print("Angle1="+str((atan(p_y/p_x)*180)/pi)+" Angle2="+str((atan(s/r)*180)/pi)+" Linear="+str(d3))
+

Assignment 4&5/q2.py

@@ -0,0 +1,16 @@
+from math import pi, sin, cos, sqrt, atan
+
+p_x, p_y, p_z = 1.23, 2.65, 0.45
+
+a1, a2= 2, 1
+
+r = sqrt((p_x**2 + p_y**2 - a1**2 - a2**2)/(2*a1*a2))
+# print(r)
+theta2 = atan(r/sqrt(1 - r**2))
+# print(theta2)
+theta1 = atan(p_y/p_x) - atan((a2*sin(theta2))/(a1 + a2*cos(theta2)))
+# print(theta1)
+d3 = p_z
+
+print("Angle1="+str(theta1*180/pi)+" Angle2="+str(theta2*180/pi)+" Linear="+str(d3))
+

Assignment 4&5/q3.py

@@ -0,0 +1,7 @@
+import numpy as np
+
+jacob = [[1, 3, 5], [2, 4, 6], [0, 0, 0]]
+end_vel = [[1],[1],[1]]
+
+joint_vel = np.dot(jacob, end_vel)
+print(joint_vel)

Assignment 4&5/q6.py

@@ -0,0 +1,22 @@
+from math import sqrt, atan, sin, cos, pi
+p_x, p_y, p_z = 1.23, 2.65, 0.45
+d1, a2, a3 = 1, 2, 1
+D = (p_x**2 + p_y**2 + (p_z - d1)**2 - a2**2 - a3**2)/(2*a2*a3)
+
+q1 = atan(p_y/p_x)
+q3 = atan(sqrt(1 - D**2)/D)
+q2 = atan(p_z - d1/sqrt(p_x**2 + p_y**2)) - atan(a3*sin(q3)/ a2 + a3*cos(q3))
+# print(q1)
+# print(q2)
+# print(q3)
+
+r11, r12, r21, r22, r13, r23, r33 = 0.404, 0, 0, 1, 0.404, 0, 0.404
+
+theta4 = atan((-cos(q1)*sin(q2+q3)*r13 - sin(q1)*sin(q2+q3)*r23 - cos(q2+q3)*r33)/(cos(q1)*cos(q2+q3)*r13 + sin(q1)*cos(q2+q3)*r23 - sin(q2+q3)*r23))
+theta5 = atan(sqrt(1 - (sin(q1)*r13 - cos(q1)*r23)**2)/(sin(q1)*r13 - cos(q1)*r23))
+theta6 = atan((sin(q1)*r12 + cos(q1)*r22)/(sin(q1)*r11 - cos(q1)*r21))
+# print(theta4)
+# print(theta5)
+# print(theta6)
+
+print("Angle4="+str(theta4*180/pi)+" Angle5="+str(theta5*180/pi)+" Angle6="+str(theta6*180/pi))

Mini - Project/trial1.py

@@ -0,0 +1,79 @@
+from cProfile import label
+import numpy as np
+import math
+import matplotlib.pyplot as plt
+import matplotlib.animation as anim
+import cv2
+import scipy
+import sympy as sp
+import os
+import moviepy.video.io.ImageSequenceClip
+
+# Task - 1: End Effoctor following a given trajectory
+l1 = 15
+l2 = 10
+L = l1 + l2
+
+t = 0
+
+while t < 360:
+	x = 10*math.cos((t*math.pi)/(180))
+	y = 10*math.sin((t*math.pi)/(180))
+	filename = str(t) + '.png'
+	t += 1
+
+	if ((x**2)+(y**2)) <= (L**2):
+		theta = math.acos(((x**2) + (y**2) - (l1**2) - (l2**2))/(2*l1*l2))
+		q1 = math.atan(y/x) - math.atan((l2*math.sin(theta))/(l1 + l2*math.cos(theta)))
+		if 90 < t < 180 and q1 < 0:
+			q1 = math.pi + q1
+		q2 = q1 + theta
+
+		x1 , y1 = [0, l1*math.cos(q1)], [0, l1*math.sin(q1)]
+		x2 , y2 = [l1*math.cos(q1), l1*math.cos(q1)+l2*math.cos(q2)], [l1*math.sin(q1), l1*math.sin(q1)+l2*math.sin(q2)]
+		plt.plot(x1, y1, x2, y2)
+		plt.savefig(filename)
+		# plt.clf()
+		# plt.show()
+
+
+image_folder='images'
+fps=15
+
+image_files = [os.path.join(image_folder,img)
+               for img in os.listdir(image_folder)
+               if img.endswith(".png")]
+clip = moviepy.video.io.ImageSequenceClip.ImageSequenceClip(image_files, fps=fps)
+clip.write_videofile('trialsim2.mp4')
+
+# Task - 2: End effoctor arranges itself to apply the required force on the wall
+
+x0 = float(input('x-coordinate of the point of application: '))
+y0 = float(input('y-coordinate of the point of application: '))
+
+theta = math.acos(((x0**2) + (y0**2) - (l1**2) - (l2**2))/(2*l1*l2))
+q1 = math.atan(y0/x0) - math.atan((l2*math.sin(theta))/(l1 + l2*math.cos(theta)))
+q2 = q1 + theta
+
+Fx = float(input('X-component of Force: '))
+Fy = float(input('Y-component of Force: '))
+Tau_1 = -1*(Fx)*l1*math.sin(q1) + (Fy)*l1*math.cos(q1)
+Tau_2 = -1*(Fx)*l2*math.sin(q2) + (Fy)*l2*math.cos(q2)
+
+x1 , y1 = [0, l1*math.cos(q1)], [0, l1*math.sin(q1)]
+x2 , y2 = [l1*math.cos(q1), l1*math.cos(q1)+l2*math.cos(q2)], [l1*math.sin(q1), l1*math.sin(q1)+l2*math.sin(q2)]
+plt.text(L, L+1, 'Tau_1 = ' + str(Tau_1))
+plt.text(L, L, 'Tau_2 = ' + str(Tau_2))
+plt.plot(x1, y1, x2, y2)
+plt.show()
+
+
+# Task - 3: End Effector's virtual spring effect
+
+x0 = 10
+y0 = 10
+k = 50
+
+Fx = k*(x - x0)
+Fy = k*(y - y0)
+