Adding torque saturation

c22fc73a · Tassos (nakano) Natsakis · e66c5d99 · c22fc73a
Commit c22fc73a authored 4 months ago by Tassos (nakano) Natsakis
--- a/assignments/10_Independent_Joint_Control.ipynb
+++ b/assignments/10_Independent_Joint_Control.ipynb
@@ -122,6 +122,11 @@
    "\n",
    "    # compute torques from the control law\n",
    "    tau[i,:] = \n",
+    "    \n",
+    "    # Saturate torques based on joint physical limits\n",
+    "    for j in range(2):\n",
+    "        if abs(tau[i,j]) > taulim:\n",
+    "            tau[i,j] = taulim * tau[i,j]/abs(tau[i,j])\n",
    "\n",
    "    # solve for next step\n",
    "    x = odeint(model,x0,tspan,args=(tau[i,:],))\n",
@@ -237,7 +242,7 @@
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
-   "version": "3.12.6"
+   "version": "3.12.7"
  }
 },
 "nbformat": 4,

 %% Cell type:markdown id:6154114a-73b4-418c-a9b7-be5d16145d5d tags:

 # Help dr. Vasilescu

 Even if dr. Vasilescu has the equations the describe the dynamics of the robot, she cannot do anything directly with them. Help her design a controller for her robot, based on the dynamic process that she identified.

 %% Cell type:markdown id:30a46a8f-2459-4f91-a03e-97ef686ae15e tags:

 ## Proposed problems

  Consider the AL5D_mdw robot with known matrices D, C, G given in the code. Implement an Independent Joint Control with PD controllers and Gravity compensation.

  1. Use step of amplitude 0.1 and interpret the plots.
  2. Adapt the natural frequencies and observe their influence on the movement and position of the robot.
  3. Add gravity compensation; interpret the plots.
  4. Use a sine wave of amplitude 0.5 as input joint trajectories
  5. Interpret the plots.

 %% Cell type:code id:dd30b2d1-9afc-4689-978f-a636070dbcf5 tags:skip-execution

 ``` python
 %reset -f
 import numpy as np
 import roboticstoolbox as rtb
 from scipy.integrate import odeint
 from lab_functions import printProgressBar

 # loading robot model

 rob = rtb.models.DH.AL5D_mdw()

 # real system of al5d
 def model(x,t,tau):
    q  = x[:n]  # First n states are the positions
    dq = x[n:]  # Last n states are the velocities

    G = rob.gravload(q)
    D = rob.inertia(q)
    C = rob.coriolis(q, dq)

    dx = np.zeros(n*2)
    dx[:n] = dq
    dx[n:] = np.linalg.inv(D)@(tau - C@dq - G)

    return dx

 # time step
 dt = 0.01

 # final time
 tf = 3

 # number of joints
 n = rob.n

 # nr of samples
 s = int(np.round(tf/dt))

 # Desired time samples for the solution.
 # np.arrange - returns evenly spaced values within a given interval.
 t = np.arange(0, tf, dt)

 # step reference, one column for each joint
 sp = 0.4*np.heaviside(t, 0)

 # initialisation
 q  = np.zeros((s,n))
 dq = np.zeros((s,n))
 tau = np.zeros((s,n))

 Kp = np.zeros((n,n))
 Kd = np.zeros((n,n))

 # x0 is the initial condition of the state space
 x0 = np.zeros(n*2)

 # solve ODE for each step
 for i in range(1,s):
    # progress bar for visualisation of elapsed time
    printProgressBar((i+1)/s, prefix="Progress:", suffix="complete", length=60)

    # span for next time step
    tspan = [t[i-1],t[i]]

    qerror =
    dqerror =

    # compute torques from the control law
    tau[i,:] =

+    # Saturate torques based on joint physical limits
+    for j in range(2):
+        if abs(tau[i,j]) > taulim:
+            tau[i,j] = taulim * tau[i,j]/abs(tau[i,j])
+
    # solve for next step
    x = odeint(model,x0,tspan,args=(tau[i,:],))

    # store solution for plotting
    q[i,:]  = x[1][:n]
    dq[i,:] = x[1][n:]

    # next initial condition
    x0 = x[1]
 ```

 %% Cell type:code id:e2d068fe-595a-4830-a612-6bee7efa16aa tags:skip-execution

 ``` python
 import matplotlib.pyplot as plt

 # plots
 fig = plt.figure()
 fig.set_size_inches(10, 10)

 plt.subplot(3, 1, 1)
 for i in range(n):
    plt.plot(t, q[:,i], label='$q_'+str(i)+'$')
 plt.plot(t, sp, label='ref')
 plt.legend(loc='best')
 plt.ylabel('values')
 plt.xlabel('time')
 plt.grid(True)

 plt.subplot(3, 1, 2)
 for i in range(n):
    plt.plot(t, tau[:,i], label='$\\tau_'+str(i)+'$')
 plt.legend(loc='best')
 plt.ylabel('commands')
 plt.xlabel('time')
 plt.grid(True)

 plt.subplot(3, 1, 3)
 for i in range(n):
    plt.plot(t, sp[i]-q[:,i], label='$err_'+str(i)+'$')
 plt.legend(loc='best')
 plt.ylabel('errors')
 plt.xlabel('time')
 plt.grid(True)

 plt.show()
 ```

 %% Cell type:code id:3f2721bc-8f53-4446-bf50-6a67a7dab65d tags:skip-execution

 ``` python
 # animation
 from lab_functions import *
 import swift

 # # 3D visualisation of the al5d_mdw
 env = swift.Swift()
 env.launch(realtime=True, browser='notebook')

 al5d = rtb.models.URDF.AL5D_mdw()
 al5d.q = q[0,:]

 arrived = False
 env.add(al5d)

 for i in range(len(q)):
    al5d.q = q[i,:]
    if i == 0:
        env.step(t[i])
    else:
        env.step(t[i]-t[i-1])
 ```