""" Dynamics and Control Assignment 4c: dynamic control of endpoint ------------------------------------------------------------------------------- DESCRIPTION: 2-DOF planar robot arm model with shoulder and elbow joints. The code includes simulation environment and visualisation of the robot. The robot is a torque-controlled robot: - Measured joint angle vector q is provided each sample time. - Calculate the joint torque tau in each sample time to be sent to the robot. Important variables: q1/tau -> shoulder joint configuration/torque q2/tau -> elbow joint configuration/torque p1 -> endpoint x position p2 -> endpoint y position TASK: Make the robot track a given endpoint reference trajectory by using dynamic control (i.e., impedance controller). Try impedance control without and with the damping term to analyse the stability. ------------------------------------------------------------------------------- INSTURCTOR: Luka Peternel e-mail: l.peternel@tudelft.nl """ import numpy as np import math import matplotlib.pyplot as plt '''ROBOT MODEL''' class human_arm_2dof: def __init__(self, l, I, m, s): self.l = l # link length self.I = I # link moment of inertia self.m = m # link mass self.s = s # link center of mass location self.q = [0.0, 0.0] # joint position self.dq = [0.0, 0.0] # joint veloctiy self.tau = [0.0, 0.0] # joint torque # arm dynamics parameters self.k = np.array([ self.I[0] + self.I[1] + self.m[1]*(self.l[0]**2), self.m[1]*self.l[0]*self.s[1], self.I[1] ]) # joint friction matrix self.B = np.array([[0.050, 0.025], [0.025, 0.050]]) # external damping matrix (e.g., when endpoint is moving inside a fluid) self.D = np.diag([1.0, 1.0]) # forward kinematics def FK(self, q): '''*********** This should be added from Assignment 4a ***********''' # q1, q2 -> p (x, y) p = np.zeros([2]) p[0] = self.l[0]*np.cos(self.q[0]) + self.l[1]*np.cos(self.q[0] + self.q[1]) p[1] = self.l[0]*np.sin(self.q[0]) + self.l[1]*np.sin(self.q[0] + self.q[1]) '''*********** This should be added from Assignment 4a ***********''' return p # arm Jacobian matrix def Jacobian(self): '''*********** This should be added from Assignment 4a ***********''' l = self.l q = self.q J = np.array([ [-l[0]*np.sin(q[0]) - l[1]*np.sin(q[0] + q[1]), -l[1]*np.sin(q[0] + q[1])], [l[0]*np.cos(q[0]) + l[1]*np.cos(q[0] + q[1]), l[1]*np.cos(q[0] + q[1])] ]) '''*********** This should be added from Assignment 4a ***********''' return J # forward arm dynamics def FD(self): '''*********** This should be added from previous Assignments ***********''' # credits to the dynamics and control whatsapp group for validating these EoM l = self.l # link length I = self.I # link moment of inertia m = self.m # link mass s = self.s # link center of mass location Ia=I Ib=I phi = self.q[0] psi = self.q[1] dphi = self.dq[0] dpsi = self.dq[1] ddq = np.array([ [(L*(L ^ 2*g*mb ^ 2*np.sin(phi) - L ^ 2*g*mb ^ 2*np.sin(phi + 2*psi) + Ib*g*ma*np.sin(phi) + 2*Ib*g*mb*np.sin(phi) + 2*L ^ 3*dphi ^ 2*mb ^ 2*np.sin(psi) + 2*L ^ 3*dpsi ^ 2*mb ^ 2*np.sin(psi) + 2*L ^ 3*dphi ^ 2*mb ^ 2*np.sin(2*psi) + 2*Ib*L*dphi ^ 2*mb * np.sin(psi) + 2*Ib*L*dpsi ^ 2*mb*np.sin(psi) + L ^ 2*g*ma*mb*np.sin(phi) + 4*L ^ 3*dphi*dpsi*mb ^ 2*np.sin(psi) + 4*Ib*L*dphi*dpsi*mb*np.sin(psi)))/(2*L ^ 4*mb ^ 2 + Ia*Ib - 2*L ^ 4*mb ^ 2*np.cos(2*psi) + Ia*L ^ 2*mb + Ib*L ^ 2*ma + 4*Ib*L ^ 2*mb + L ^ 4*ma*mb)], [-(L ^ 3*g*mb ^ 2*np.sin(phi) + 2*L ^ 3*g*mb ^ 2*np.sin(phi - psi) - L ^ 3*g*mb ^ 2*np.sin(phi + 2*psi) + 10*L ^ 4*dphi ^ 2*mb ^ 2*np.sin(psi) + 2*L ^ 4*dpsi ^ 2*mb ^ 2*np.sin(psi) + 4*L ^ 4*dphi ^ 2*mb ^ 2*np.sin(2*psi) + 2*L ^ 4*dpsi ^ 2*mb ^ 2*np.sin(2*psi) - 2*L ^ 3*g*mb ^ 2*np.sin(phi + psi) + L ^ 3*g*ma*mb*np.sin(phi) + L ^ 3*g*ma*mb*np.sin(phi - psi) + 2*Ia*L ^ 2*dphi ^ 2*mb*np.sin(psi) + 2*Ib*L ^ 2 * dphi ^ 2*mb*np.sin(psi) + 2*Ib*L ^ 2*dpsi ^ 2*mb*np.sin(psi) - Ia*L*g*mb*np.sin(phi + psi) + 4*L ^ 4*dphi*dpsi*mb ^ 2*np.sin(psi) + 2*L ^ 4*dphi ^ 2*ma*mb*np.sin(psi) + Ib*L*g*ma*np.sin(phi) + 2*Ib*L*g*mb*np.sin(phi) + 4*L ^ 4*dphi*dpsi*mb ^ 2*np.sin(2*psi) + 4*Ib*L ^ 2*dphi*dpsi*mb*np.sin(psi))/(2*L ^ 4*mb ^ 2 + Ia*Ib - 2*L ^ 4*mb ^ 2*np.cos(2*psi) + Ia*L ^ 2*mb + Ib*L ^ 2*ma + 4*Ib*L ^ 2*mb + L ^ 4*ma*mb)] ]) '''*********** This should be added from previous Assignments ***********''' return ddq # inverse kinematics def IK(self, p): q = np.zeros([2]) r = np.sqrt(p[0]**2+p[1]**2) q[1] = np.pi - math.acos((self.l[0]**2+self.l[1]**2-r**2)/(2*self.l[0]*self.l[1])) q[0] = math.atan2(p[1],p[0]) - math.acos((self.l[0]**2-self.l[1]**2+r**2)/(2*self.l[0]*r)) return q # state change def state(self, q, dq, tau): self.q = q self.dq = dq self.tau = tau '''SIMULATION''' # SIMULATION PARAMETERS dt = 0.01 # intergration step timedt = 0.01 # integration step time dts = dt*1 # desired simulation step time (NOTE: it may not be achieved) T = 3 # total simulation time # ROBOT PARAMETERS x0 = 0.0 # base x position y0 = 0.0 # base y position l1 = 0.3 # link 1 length l2 = 0.35 # link 2 length (includes hand) l = [l1, l2] # link length I = [0.025, 0.045] # link moment of inertia m = [1.4, 1.1] # link mass s = [0.11, 0.16] # link center of mass location # REFERENCE TRAJETORY ts = T/dt # trajectory size xt = np.linspace(-2,2,int(ts)) yt1 = np.sqrt(1-(abs(xt)-1)**2) yt2 = -3*np.sqrt(1-(abs(xt)/2)**0.5) x = np.concatenate((xt, np.flip(xt,0)), axis=0) y = np.concatenate((yt1, np.flip(yt2,0)), axis=0) pr = np.array((x / 10 + 0.0, y / 10 + 0.45)) # reference endpoint trajectory '''*********** Student should fill in ***********''' # IMPEDANCE CONTROLLER PARAMETERS K = 1 # stiffness N/m D = 1 # damping Ns/m '''*********** Student should fill in ***********''' '''SIMULATOR''' # initialise robot model class model = human_arm_2dof(l, I, m, s) # initialise real-time plot plt.close() plt.figure(1) plt.xlim(-0.4,0.4) plt.ylim(-0.1,0.7) # initial conditions t = 0.0 # time q = model.IK(pr[:,0]) # joint position dq = [0., 0.] # joint velocity tau = [0., 0.] # joint torque model.state(q, dq, tau) # update initial state p_prev = pr[:,0] # previous endpoint position i = 0 # loop counter state = [] # state vector # robot links x1 = l1*np.cos(q[0]) y1 = l1*np.sin(q[0]) x2 = x1+l2*np.cos(q[0]+q[1]) y2 = y1+l2*np.sin(q[0]+q[1]) link1, = plt.plot([x0,x1],[y0,y1],color='b',linewidth=3) # draw upper arm link2, = plt.plot([x1,x2],[y1,y2],color='b',linewidth=3) # draw lower arm shoulder, = plt.plot(x0,y0,color='k',marker='o',markersize=8) # draw shoulder / base elbow, = plt.plot(x1,y1,color='k',marker='o',markersize=8) # draw elbow hand, = plt.plot(x2,y2,color='k',marker='*',markersize=15) # draw hand / endpoint for i in range(len(x)): # update individual link position x1 = l1*np.cos(q[0]) y1 = l1*np.sin(q[0]) x2 = x1+l2*np.cos(q[0]+q[1]) y2 = y1+l2*np.sin(q[0]+q[1]) # real-time plotting ref, = plt.plot(pr[0,i],pr[1,i],color='g',marker='+') # draw reference link1.set_xdata([x0,x1]) # update upper arm link1.set_ydata([y0,y1]) # update upper arm link2.set_xdata([x1,x2]) # update lower arm link2.set_ydata([y1,y2]) # update lower arm shoulder.set_xdata(x0) # update shoulder / base shoulder.set_ydata(y0) # update shoulder / base elbow.set_xdata(x1) # update elbow elbow.set_ydata(y1) # update elbow hand.set_xdata(x2) # update hand / endpoint hand.set_ydata(y2) # update hand / endpoint plt.pause(dts) # try to keep it real time with the desired step time '''*********** Student should fill in ***********''' # # endpoint reference trajectory controller # p = model.FK(q) # error = (pr[:, i] - p) # # for comparison # loss_MSE = np.sqrt(error[0]**2 + error[1]**2) # # integrated error # se += error*dt # # derivative error # d_error = error - pe # # previous error # pe = error # if controller_type == 'P': # dp = Kp*error # if controller_type == 'PD': # dp = Kp*error + Kd*d_error # if controller_type == 'PI': # dp = Kp*error + Ki*se # if controller_type == 'PID': # dp = Kp*error + Kd*d_error + Ki*se # dq = np.dot(np.linalg.pinv(model.Jacobian()), dp) '''*********** Student should fill in ***********''' # log states for analysis state.append([t, q[0], q[1], dq[0], dq[1], ddq[0], ddq[1], tau[0], tau[1], p[0], p[1]]) # previous endpoint position for velocity calculation p_prev = p # integration dq += ddq*dt q += dq*dt t += dt # increase loop counter i = i + 1 '''ANALYSIS''' state = np.array(state) plt.figure(3) plt.subplot(411) plt.title("JOINT BEHAVIOUR") plt.plot(state[:,0],state[:,7],"b",label="shoulder") plt.plot(state[:,0],state[:,8],"r",label="elbow") plt.legend() plt.ylabel("tau [Nm]") plt.subplot(412) plt.plot(state[:,0],state[:,5],"b") plt.plot(state[:,0],state[:,6],"r") plt.ylabel("ddq [rad/s2]") plt.subplot(413) plt.plot(state[:,0],state[:,3],"b") plt.plot(state[:,0],state[:,4],"r") plt.ylabel("dq [rad/s]") plt.subplot(414) plt.plot(state[:,0],state[:,1],"b") plt.plot(state[:,0],state[:,2],"r") plt.ylabel("q [rad]") plt.xlabel("t [s]") plt.tight_layout() plt.figure(4) plt.title("ENDPOINT BEHAVIOUR") plt.plot(0,0,"ok",label="shoulder") plt.plot(state[:,5],state[:,6],label="trajectory") plt.plot(state[0,5],state[0,6],"xg",label="start point") plt.plot(state[-1,5],state[-1,6],"+r",label="end point") plt.axis('equal') plt.xlabel("x [m]") plt.ylabel("y [m]") plt.legend() plt.tight_layout()