|
| 1 | +#!/usr/bin/env python |
| 2 | +# coding: utf-8 |
| 3 | + |
| 4 | +# In[1]: |
| 5 | +import time |
| 6 | +import sys |
| 7 | + |
| 8 | +import numpy as np |
| 9 | +import roboticstoolbox as rtb |
| 10 | +from spatialmath import * |
| 11 | +from math import pi |
| 12 | +import matplotlib.pyplot as plt |
| 13 | +from matplotlib import cm |
| 14 | +np.set_printoptions(linewidth=100, formatter={'float': lambda x: f"{x:8.4g}" if x > 1e-10 else f"{0:8.4g}"}) |
| 15 | + |
| 16 | + |
| 17 | + |
| 18 | +# We will instantiate a model of the Puma 560 robot which has well known inertial parameters |
| 19 | + |
| 20 | +# In[2]: |
| 21 | + |
| 22 | + |
| 23 | +p560 = rtb.models.DH.Puma560() |
| 24 | + |
| 25 | + |
| 26 | +# and show its configuration in a typical desktop working pose |
| 27 | + |
| 28 | +# In[5]: |
| 29 | + |
| 30 | + |
| 31 | +p560.plot(p560.qn, block=False) |
| 32 | + |
| 33 | + |
| 34 | +# The rigid-body equations of motion for a robot are a set of coupled differential equations |
| 35 | +# $$ |
| 36 | +# \mathbf{M}(\mathit{\ddot{q}}) \mathit{\ddot{q}} + \mathbf{C}(\mathit{q}, \mathit{\dot{q}}) \mathit{\dot{q}} + \mathbf{g}(\mathit{q}) = \mathit{\tau} |
| 37 | +# $$ |
| 38 | +# which relate the motion of the robot $(\mathit{q}, \mathit{\dot{q}}, \mathit{\ddot{q}})$ and the applied torque $\mathit{\tau}$. The coefficients in this equation are: |
| 39 | +# - the inertia or mass matrix $\mathbf{M}(\mathit{\ddot{q}})$ which is a function of joint configuration |
| 40 | +# - the centripetal and Coriolis or velocity term which is a function of joint configuration and rate |
| 41 | +# - the gravity load which is a function of joint configuration |
| 42 | +# |
| 43 | +# If the robot is not moving, that is $\mathit{q} = \mathit{\dot{q}} = 0$ then the equation becomes |
| 44 | +# $$ |
| 45 | +# \mathbf{g}(\mathit{q}) = \mathit{\tau} |
| 46 | +# $$ |
| 47 | +# where $\mathit{\tau}$ is the torque required for this condition $\mathit{q} = \mathit{\dot{q}} = 0$ to be true, that is, the torque required to stop the robot falling under its own weight. The toolbox can compute this |
| 48 | + |
| 49 | +# In[6]: |
| 50 | + |
| 51 | +print('here 0', file=sys.stdout) |
| 52 | +time.sleep(1) |
| 53 | + |
| 54 | +p560.gravload(p560.qn) |
| 55 | + |
| 56 | + |
| 57 | +# and it shows, as expected, that the shoulder is exerting significant torque to hold the arm up and stationary. |
| 58 | +# |
| 59 | +# The inertia matrix relates torque to joint acceleration and is the mass in a multi-dimensional version of Newton's second law $F = m a$. In this configuration the inertia matrix is |
|
E29B
60 | + |
| 61 | +# In[7]: |
| 62 | + |
| 63 | + |
| 64 | +p560.inertia(p560.qn) |
| 65 | + |
| 66 | + |
| 67 | +# The diagonal elements $M_{jj}$ indicate the inertia experienced by the joint $j$, ie. Newton's second law for this joint is $\tau_j = M_{jj} \ddot{q}_j$. |
| 68 | +# |
| 69 | +# The matrix is symmetric and the off-diagonal terms $M_{ij} = M_{ji}$ couple acceleration of one joint into a disturbance torque on another joint, ie. $\tau_j = M_{ij} \ddot{q}_i$. |
| 70 | +# |
| 71 | +# The inertia matrix is a function of joint configuration, that is, the elements of the inertia matrix change as we vary the angles of joints 1 and 2, ie. $q_2$ and $q_3$. It is this configuration varying inertia and coupling between joints that is a fundamental challenge for high-quality joint control. |
| 72 | + |
| 73 | +# In[8]: |
| 74 | + |
| 75 | + |
| 76 | +N = 100 |
| 77 | +(Q2, Q3) = np.meshgrid(np.linspace(-pi, pi, N), np.linspace(-pi, pi, N)) |
| 78 | +M11 = np.zeros((N,N)) |
| 79 | +M12 = np.zeros((N,N)) |
| 80 | +for i in range(N): |
| 81 | + for j in range(N): |
| 82 | + M = p560.inertia(np.r_[0, Q2[i,j], Q3[i,j], 0, 0, 0]) |
| 83 | + M11[i,j] = M[0,0] |
| 84 | + M12[i,j] = M[0,1] |
| 85 | + |
| 86 | + |
| 87 | +# The inertia "seen" by joint 1 varies as a function of $q_2$ and $q_3$ as shown below |
| 88 | + |
| 89 | +# In[12]: |
| 90 | + |
| 91 | + |
| 92 | +fig, ax = plt.subplots(subplot_kw={"projection": "3d"}) |
| 93 | +surf = ax.plot_surface(Q2, Q3, M11, cmap=cm.coolwarm, linewidth=0, antialiased=False) |
| 94 | +fig.colorbar(surf, shrink=0.9, aspect=10, pad=0.12) |
| 95 | +ax.set_xlabel('$q_2$ (rad)') |
| 96 | +ax.set_ylabel('$q_3$ (rad)') |
| 97 | +ax.set_zlabel('$M_{11}$ ($kg.m^2$)') |
| 98 | +plt.show(block=False) |
| 99 | + |
| 100 | + |
| 101 | +# The ratio of maximum to minimum values is |
| 102 | + |
| 103 | +# In[10]: |
| 104 | + |
| 105 | + |
| 106 | +M11.max() / M11.min() |
| 107 | + |
| 108 | + |
| 109 | +# The coupling inertia between joints 1 and 2 also varies with configuration and we can plot that as well |
| 110 | + |
| 111 | +# In[11]: |
| 112 | + |
| 113 | + |
| 114 | +fig, ax = plt.subplots(subplot_kw={"projection": "3d"}) |
| 115 | +surf = ax.plot_surface(Q2, Q3, M12, cmap=cm.coolwarm, linewidth=0, antialiased=False) |
| 116 | +fig.colorbar(surf, shrink=0.9, aspect=10, pad=0.12) |
| 117 | +ax.set_xlabel('$q_2$ (rad)') |
| 118 | +ax.set_ylabel('$q_3$ (rad)') |
| 119 | +ax.set_zlabel('$M_{12}$ ($kg.m^2$)') |
| 120 | +plt.show(block=False) |
| 121 | + |
| 122 | + |
| 123 | +# In[3]: |
| 124 | + |
| 125 | +print('here 1', file=sys.stdout) |
| 126 | +time.sleep(1) |
| 127 | + |
| 128 | + |
| 129 | + |
| 130 | +qd = np.r_[0, 1, 0, 0, 0, 0] |
| 131 | + |
| 132 | +# BUG SEEMS TO BE IN NEXT LINE |
| 133 | + |
| 134 | +p560.coriolis(p560.qn, qd) @ qd |
| 135 | + |
| 136 | + |
| 137 | +# and we see that it exerts a torque on the waist and elbow joints. |
| 138 | +# |
| 139 | +# The algorithms to compute the various terms in the rigid-body equations of motion are based on the recursive Newton-Euler algorithm |
| 140 | + |
| 141 | +# In[4]: |
| 142 | +for i in range(72): |
| 143 | + p560.rne(p560.qn, np.zeros((6,)), np.zeros((6,)), grav=[0,0,0]) |
| 144 | + |
| 145 | +p560.rne(p560.qn, np.zeros((6,)), np.zeros((6,))) |
| 146 | + |
| 147 | +print('here 2', file=sys.stdout) |
| 148 | +time.sleep(1) |
| 149 | + |
| 150 | +# In[5]: |
| 151 | + |
| 152 | +# p560nf = p560.nofriction() |
| 153 | +# p560nf.delete_rne() |
| 154 | + |
| 155 | +# print(p560nf) |
| 156 | + |
| 157 | +tg = p560.fdyn(5, p560.qn, dt=0.05) |
| 158 | + |
| 159 | +print('here 3', file=sys.stdout) |
| 160 | +time.sleep(1) |
| 161 | + |
| 162 | + |
| 163 | +# # The first line needs some explanation. The Toolbox can model two types of joint friction: |
| 164 | +# # - viscous friction which is linearly related to joint velocity |
| 165 | +# # - Coulomb friction which is **non-linearly** related to joint velocity |
| 166 | +# # |
| 167 | +# # Coulomb friction is a _harsh_ non-linearity and it causes the numerical integrator to take very small times steps, so the result will take many minutes to compute. To speed things up, at the expense of some modeling fidelity, we set the Coulomb friction to zero, but retain the viscous friction. The `nofriction()` method returns a clone of the robot with its friction parameters modified. |
| 168 | +# # |
| 169 | +# # The computed joint configuration trajectory is |
| 170 | + |
| 171 | +# # In[ ]: |
| 172 | + |
| 173 | + |
| 174 | +# tg.q |
| 175 | + |
| 176 | + |
| 177 | +# # which we can plot using a Toolbox convenience function |
| 178 | + |
| 179 | +# # In[ ]: |
| 180 | + |
| 181 | + |
| 182 | +rtb.tools.trajectory.qplot(tg.q, tg.t, block=False) |
| 183 | + |
| 184 | + |
| 185 | +# # or we can animate it, showing the robot collapsing under gravity |
| 186 | + |
| 187 | +# # In[ ]: |
| 188 | + |
| 189 | + |
| 190 | +# p560.plot(tg.q.T)p560nf = p560.nofriction() |
| 191 | + |
| 192 | +plt.pause(2) |
| 193 | + |
| 194 | +print('done') |
| 195 | + |
| 196 | + |
| 197 | + |
| 198 | +# The motion of the robot quickly dies out and it hangs downward, this loss of energy is due to the viscous friction in the robot's joints. |
0 commit comments