"""
Dynamics and Control Assignment 4a: forward kinematics and Jacobian matrix
-------------------------------------------------------------------------------
DESCRIPTION:
2-DOF planar robot arm model with shoulder and elbow joints.
Important variables:
q1 -> shoulder joint configuration
q2 -> elbow joint configuration
l1 -> first segment length
l2 -> second segment length
TASK:
Derive forward kinematics function and Jacobian matrix function with the given
inputs and outputs.
-------------------------------------------------------------------------------
INSTRUCTOR: 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):
self.l = l # link length
self.q = np.array([0.0, 0.0]) # joint position
self.dq = np.array([-1.0, 1.0]) # joint veloctiy
self.tau = np.array([0.0, 0.0]) # joint torque
self.p = np.array([0.0, 0.0])
self.dp = np.array([0.0, 0.0])
# forward kinematics
def FK(self):
'''*********** Student should fill in ***********'''
# q1, q2 -> p (x, y)
self.p[0] = np.add(self.l[0]*np.cos(self.q[0]), self.l[1]*np.cos(self.q[0] + self.q[1]))
self.p[1] = self.l[0]*np.sin(self.q[0]) + self.l[1]*np.sin(self.q[0] + self.q[1])
'''*********** Student should fill in ***********'''
# return p
# arm Jacobian matrix
def Jacobian(self):
'''*********** Student should fill in ***********'''
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])]
])
'''*********** Student should fill in ***********'''
return J
# 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):
self.q = q
self.dq = dq
# 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 = np.array([l1, l2]) # link length
# SIMULATOR
# initialise robot model class
model = human_arm_2dof(l)
# SIMULATION PARAMETERS
dt = 0.001 # integration step time
T = 15 # total simulation time
time_steps = T/dt # number of integration steps
# initial conditions
t = 0.0 # time
# q = model.q # initial joint angles
# dq = model.dq # initial joint velocity
state = [] # state vector
for i in range(int(time_steps)):
# obtain state from joint config
model.FK()
# transform joint velocity to endpoint velocity
model.dp = np.dot(model.Jacobian(), model.dq)
# verify with IK model
q_check = model.IK(model.p)
# save state for plotting
state.append([
t,
model.q[0],
model.q[1],
model.dq[0],
model.dq[1],
model.p[0],
model.p[1],
model.dp[0],
model.dp[1],
q_check[0] ,
q_check[1]])
# update state
model.p = np.add(model.p, model.dp * dt)
model.q = np.add(model.q, model.dq * dt)
t += dt
''' Test your forward kinematics function using the provided inverse kinematics function. '''
import pandas as pd
state = np.array(state)
state = pd.DataFrame(state)
state.columns = ["t", "q0", "q1", "dq0", "dq1", "p0", "p1", "dp0", "dp1","q0_check" , "q1_check"]
# rough first check of all my data for preliminary checking
state.plot(figsize=(15,10))
state
plt.plot(state["t"], state["q0"], label= "joint q0")
plt.plot(state["t"], state["q1"], label= "joint q1")
plt.plot(state["t"], state["q0_check"], label= "joint q0 check w/ IK")
plt.plot(state["t"], state["q1_check"], label= "joint q1 check w/ IK")
plt.xlabel("time")
plt.ylabel("angle (rad)")
plt.legend()
plt.show()
plt.plot(state["t"], state["p0"], label= "endpoint x")
plt.plot(state["t"], state["p1"], label= "endpoint y")
plt.xlabel("time")
plt.ylabel("pos (m)")
plt.legend()
"""
Dynamics and Control Assignment 4b: kinematic 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 classic position-controlled robot:
- Measured joint angle vector q is provided each sample time.
- Calculate the joint velocity dq in each sample time to be sent to the robot.
Important variables:
q1 -> shoulder joint configuration
q2 -> elbow joint configuration
p1 -> endpoint x position
p2 -> endpoint y position
TASK:
Make the robot track a given endpoint reference trajectory by using kinematic
control (i.e., PID controller). Experimentally tune the PID parameters to
achieve a good performance.
-------------------------------------------------------------------------------
INSTURCTOR: Luka Peternel
e-mail: l.peternel@tudelft.nl
"""
import numpy as np
import math
import matplotlib.pyplot as plt
# imports for live plot in Jupyter
import time
from IPython import display
'''ROBOT MODEL'''
class human_arm_2dof:
def __init__(self, l):
self.l = l # link length
self.q = [0.0, 0.0] # joint position
self.dq = [0.0, 0.0] # joint veloctiy
self.tau = [0.0, 0.0] # joint torque
# 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
# 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):
self.q = q
self.dq = dq
'''SIMULATION'''
# SIMULATION PARAMETERS
dt = 0.05 # 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
# 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
'''SIMULATOR'''
def run_sim_Q2(controller_type, gains, show_plot=False, give_output=False):
'''*********** Student should fill in ***********'''
# PID CONTROLLER PARAMETERS
Kp = gains[0] # proportional gain
Ki = gains[1] # integral gain
Kd = gains[2] # derivative gain
'''*********** Student should fill in ***********'''
se = 0.0 # integrated error
pe = 0.0 # previous error
# initialise robot model class
model = human_arm_2dof(l)
# initial conditions
t = 0.0 # time
q = model.IK(pr[:,0]) # initial configuration
dq = [0., 0.] # joint velocity
model.state(q, dq) # update initial state
i = 0 # loop counter
state = [] # state vector
t = 0.0 # time
if show_plot == True:
# initialise real-time plot
plt.close()
plt.figure(1)
plt.xlim(-0.4,0.4)
plt.ylim(-0.1,0.7)
# 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)):
if show_plot == True:
# 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
# TO MAKE THE LIVE PLOT WORK IN JUPYTER
display.clear_output(wait=True)
display.display(plt.gcf())
time.sleep(dts/100)
'''*********** 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
# derivative error
d_error = error - pe
# print(d_error)
# previous error
pe = error
if controller_type == 'P':
dp = Kp*error
# print(controller_type, " -> P")
elif controller_type == 'PI':
dp = Kp*error + Ki*se
# print(controller_type, " -> PI")
elif controller_type == 'PD':
dp = Kp*error + Kd*d_error
# print(controller_type, " -> PD")
elif controller_type == 'PID':
dp = Kp*error + Kd*d_error + Ki*se
# print(controller_type, " -> PID")
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], p[0], p[1], loss_MSE])
# get joint angles by intergration
q += dq*dt
t += dt
# increase loop counter
i = i + 1
'''ANALYSIS'''
state = np.array(state)
if show_plot == True:
plt.figure(3)
plt.subplot(311)
plt.title("ARM BEHAVIOUR")
plt.plot(state[:,0],state[:,5],"b",label="x")
plt.plot(state[:,0],state[:,6],"r",label="y")
plt.legend()
plt.ylabel("p [m]")
plt.subplot(312)
plt.plot(state[:,0],state[:,3],"b",label="shoulder")
plt.plot(state[:,0],state[:,4],"r",label="elbow")
plt.legend()
plt.ylabel("dq [rad/s]")
plt.subplot(313)
plt.plot(state[:,0],state[:,1],"b",label="shoulder")
plt.plot(state[:,0],state[:,2],"r",label="elbow")
plt.legend()
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()
error = state[:,7]
# print('-------error: ', state)
if give_output == True:
return error
run_sim_Q2('PID',[11,3,30], show_plot=True)
Unsupported output type: clearOutput
import pandas as pd
error_df = pd.DataFrame()
gains=[17, 5,2]
error_df["no controller"] = run_sim_Q2('PID',[0,0,0], give_output=True)
error_df["no controller"] = run_sim_Q2('PID',[0,0,0], give_output=True)
error_df["100"] = run_sim_Q2('PID',[1,0,0], give_output=True)
error_df["010"] = run_sim_Q2('PID',[0,1,0], give_output=True)
error_df["001"] = run_sim_Q2('PID',[0,0,1], give_output=True)
error_df["110"] = run_sim_Q2('PID',[1,1,0], give_output=True)
error_df["011"] = run_sim_Q2('PID',[0,1,1], give_output=True)
error_df["101"] = run_sim_Q2('PID',[1,0,1], give_output=True)
error_df["111"] = run_sim_Q2('PID',[1,1,1], give_output=True)
# error_df.plot()
import plotly.express as px
fig = px.line(error_df)
fig.show()
import pandas as pd
error_df = pd.DataFrame()
gains=[17, 5,2]
# error_df["no controller"] = run_sim_Q2('PID',[0,0,0], give_output=True)
error_df["P"] = run_sim_Q2('P',gains, give_output=True)
error_df["PI"] = run_sim_Q2('PI',gains, give_output=True)
error_df["PD"] = run_sim_Q2('PD',gains, give_output=True)
error_df["PID"] = run_sim_Q2('PID',gains, give_output=True)
# error_df.plot()
import plotly.express as px
fig = px.line(error_df)
fig.show()
"""
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()
NameError: name 'ddq' is not defined
"""
Dynamics and Control Assignment 4d: singularity problem solution
-------------------------------------------------------------------------------
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 classic position-controlled robot:
- Measured joint angle vector q is provided each sample time.
- Calculate the joint velocity dq in each sample time to be sent to the robot.
Important variables:
q1 -> shoulder joint configuration
q2 -> elbow joint configuration
p1 -> endpoint x position
p2 -> endpoint y position
TASK:
Copy the kinematic controller designed in Assignment 4b into the main loop.
Note that this time the reference trajectory moves the robot into singular
configurations, therefore singularity problem has to be solved by a modified
controller.
-------------------------------------------------------------------------------
INSTURCTOR: Luka Peternel
e-mail: l.peternel@tudelft.nl
"""
import numpy as np
import math
import matplotlib.pyplot as plt
# imports for live plot in Jupyter
import time
from IPython import display
'''ROBOT MODEL'''
class human_arm_2dof:
def __init__(self, l):
self.l = l # link length
self.q = [0.0, 0.0] # joint position
self.dq = [0.0, 0.0] # joint veloctiy
self.tau = [0.0, 0.0] # joint torque
# 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
# 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):
self.q = q
self.dq = dq
'''SIMULATION'''
# SIMULATION PARAMETERS
dt = 0.1 # 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
# 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 / 5 + 0.0, y / 5 + 0.45)) # reference endpoint trajectory
'''*********** Student should fill in ***********'''
# PID CONTROLLER PARAMETERS
Kp = 10# proportional gain
Ki = 1# integral gain
Kd = 1# derivative gain
'''*********** Student should fill in ***********'''
se = 0.0 # integrated error
pe = 0.0 # previous error
''' SIMULATOR '''
def run_sim(controller_type):
# initialise robot model class
model = human_arm_2dof(l)
# 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]) # initial configuration
dq = [0., 0.] # joint velocity
model.state(q, dq) # update initial state
i = 0 # loop counter
state = [] # state vector
# robot links
def calc_link_positions():
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])
return x1, y1, x2, y2
x1, y1, x2, y2 = calc_link_positions()
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)):
x1, y1, x2, y2 = calc_link_positions()
# 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
# TO MAKE THE LIVE PLOT WORK IN JUPYTER
display.clear_output(wait=True)
display.display(plt.gcf())
time.sleep(dts)
'''*********** Student should fill in ***********'''
# NOT SINGULARITY ROBUST endpoint reference trajectory controller
if controller_type == "P":
p = model.FK(q)
dp = Kp*(pr[:, i] - p)
dq = np.dot(np.linalg.pinv(model.Jacobian()), dp)
# singularity-robust endpoint reference trajectory controller
if controller_type == "robust":
p = model.FK(q)
dp = Kp*(pr[:, i] - p)
J = model.Jacobian()
I = np.identity(2, dtype = float)
damping_coeff = 0.001
J_robust = np.dot(np.linalg.inv(np.dot(J.T, J) + damping_coeff * I), J.T)
dq = np.dot(J_robust, dp)
'''*********** Student should fill in ***********'''
# log states for analysis
state.append([t, q[0], q[1], dq[0], dq[1], p[0], p[1]])
# get joint angles by intergration
q += dq*dt
t += dt
# increase loop counter
i = i + 1
'''ANALYSIS'''
state = np.array(state)
plt.figure(3)
plt.subplot(311)
plt.title("ARM BEHAVIOUR")
plt.plot(state[:,0],state[:,5],"b",label="x")
plt.plot(state[:,0],state[:,6],"r",label="y")
plt.legend()
plt.ylabel("p [m]")
plt.subplot(312)
plt.plot(state[:,0],state[:,3],"b",label="shoulder")
plt.plot(state[:,0],state[:,4],"r",label="elbow")
plt.legend()
plt.ylabel("dq [rad/s]")
plt.subplot(313)
plt.plot(state[:,0],state[:,1],"b",label="shoulder")
plt.plot(state[:,0],state[:,2],"r",label="elbow")
plt.legend()
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()
run_sim("P")
Unsupported output type: clearOutput
run_sim("robust")