Hey im not absolutely sure but i think you forgot to multply the input variable in K matrice
you can find it on exercise 4.1.1
input=KP@(sp_q(t)-q)-KD@(dq)
but it should be = K@(input=KP@(sp_q(t)-q)-KD@(dq))
when K is diagonal matrice of K1 and K2
As far as we tested, it seems correct, the input_torque is an array of two compnents, one for each joint.
Have a look at this code with comments that might clear things up a bit:
#!/usr/bin/env python3
import numpy as np
from matplotlib import pyplot as plt
# Function to calculate the D matrix representing the inertia of the robot
def D_robot(m1, m2, l1, l2, lc1, lc2, Izz1, Izz2, th):
D = np.zeros((2, 2))
t1 = th[0, 0] # Angle of the first joint
t2 = th[1, 0] # Angle of the second joint
# Calculation of elements of the D matrix based on robot parameters and joint angles
D[0, 0] = Izz1 + Izz2 + lc1**2 * m1 + l1**2 * m2 + 2 * np.cos(t2) * l1 * lc2 * m2 + lc2**2 * m2
D[0, 1] = Izz2 + np.cos(t2) * l1 * lc2 * m2 + lc2**2 * m2
D[1, 0] = Izz2 + np.cos(t2) * l1 * lc2 * m2 + lc2**2 * m2
D[1, 1] = Izz2 + lc2**2 * m2
return D
# Function to calculate the C matrix representing the Coriolis and centrifugal effects
def C_robot(m1, m2, l1, l2, lc1, lc2, Izz1, Izz2, th, dq):
C = np.zeros((2, 2))
t2 = th[1, 0] # Angle of the second joint
# Calculation of elements of the C matrix based on robot parameters, joint angles, and velocities
C[0, 0] = -2 * np.sin(t2) * l1 * lc2 * m2 * dq[1, 0]
C[0, 1] = -np.sin(t2) * l1 * lc2 * m2 * dq[1, 0]
C[1, 0] = np.sin(t2) * l1 * lc2 * m2 * dq[0, 0]
C[1, 1] = 0.0
return C
# Function to calculate the gravity effects
def g_robot(m1, m2, l1, l2, lc1, lc2, Izz1, Izz2, th, grav):
gravity = np.zeros((2, 1))
return gravity
# Time step for simulation
dt = 0.01
# Initial joint angles in radians
q = np.array([[20.00], [-20.00]]) * np.pi / 180
# Initial joint velocities
dq = np.array([[0.0], [0.0]])
# Frequencies of the sinusoidal set points
f1 = 1.0
f2 = 0.5
# Function defining the set points for joint angles over time
sp_q = lambda t: np.array([[0.5 * np.sin(f1 * t)], [0.75 * np.cos(f2 * t)]])
# Initial control torque
input_torque = np.array([[0.0], [0.0]])
# Robot parameters
m1 = 1.0
m2 = 1.0
l1 = 1.0
l2 = 1.0
lc1 = 0.5
lc2 = 0.5
Izz1 = (1.0 / 12.0) * (0.1 * 0.01 + l1**2)
Izz2 = (1.0 / 12.0) * (0.1 * 0.01 + l2**2)
# Gravity constant
grav = 9.81
# Effective inertias of the motors
Jeff1 = Izz1 + m1 * lc1**2
Jeff2 = Izz2 + m2 * lc2**2
# Voltage-torque conversion factors for the motors
K1 = 0.1
K2 = 0.1
# Friction gains for the motors
Beff1 = 0.001
Beff2 = 0.001
# Reduction ratios between motors and links
red1 = 1
red2 = 1
# PD control gains
etta = 0.5
omega = 2
KP = np.array([[omega**2 * Jeff1 / K1, 0], [0, omega**2 * Jeff2 / K2]])
KD = np.array([[(2 * etta * omega * Jeff1 - Beff1) / K1, 0], [0, (2 * etta * omega * Jeff2 - Beff2) / K2]])
# Lists to store joint angles and set points for plotting
th1 = []
th2 = []
th1_sp = []
th2_sp = []
# Simulation time parameters
t0 = 0
tf = 50
time_list = np.arange(t0, tf, dt)
# Simulation loop
for t in time_list:
# Appending current joint angles and set points to the lists
th1.append(q[0, 0] * 180 / np.pi)
th2.append(q[1, 0] * 180 / np.pi)
th1_sp.append(sp_q(t)[0, 0] * 180 / np.pi)
th2_sp.append(sp_q(t)[1, 0] * 180 / np.pi)
# Calculating the matrices D, C, g
D = D_robot(m1, m2, l1, l2, lc1, lc2, Izz1, Izz2, q)
C = C_robot(m1, m2, l1, l2, lc1, lc2, Izz1, Izz2, q, dq)
g = g_robot(m1, m2, l1, l2, lc1, lc2, Izz1, Izz2, q, grav)
# Diagonal matrices for the motor inertias and frictions
JM = np.diagflat(np.array([(Jeff1) / (red1**2), (Jeff2) / (red2**2)]))
BM = np.diagflat(np.array([(Beff1) / (red1**2), (Beff2) / (red2**2)]))
# Calculating joint accelerations
ddq = np.linalg.inv(D + JM) @ (input_torque - C @ dq - BM @ q - g)
# Updating joint velocities and angles
dq = dq + dt * ddq
q = q + dt * dq
# Updating control torque using PD control
input_torque = KP @ (sp_q(t) - q) - KD @ dq
print(input_torque)
# Plotting results
plt.figure(figsize=(12, 4), dpi=200)
plt.plot(time_list, th1, 'g')
plt.plot(time_list, th2, 'b')
plt.plot(time_list, th1_sp, 'g--')
plt.plot(time_list, th2_sp, 'b--')
plt.legend(["$\Theta_1$", "$\Theta_2$", "set point-$\Theta_1$", "set point-$\Theta_2$"])
plt.grid()
plt.title("PD control revisited")
plt.xlabel("Time [s]")
plt.ylabel("Angular position [deg]")
plt.show()