I have a 2DOF robot with 2 revolute joints, as shown in the diagram below. I'm trying to calculate (using MATLAB) the torque required to move it but my answers don't match up with what I'm expecting.
Denavit-Hartenberg parameters: $$ \begin{array}{c|cccc} joint & a & \alpha & d & \theta \\ \hline 1 & 0 & \pi/2 & 0 & \theta_1 \\ 2 & 1 & 0 & 0 & \theta_2 \\ \end{array} $$
I'm trying to calculate the torques required to produce a given acceleration, using the Euler-Lagrange techniques as described on pages 5/6 in this paper. Particularly, $$ T_i(inertial) = \sum_{j=0}^nD_{ij}\ddot q_i$$ where $$ D_{ij} = \sum_{p=max(i,j)}^n Trace(U_{pj}J_pU_{pi}^T) $$ and $$ J_i = \begin{bmatrix} {(-I_{xx}+I_{yy}+I_{zz}) \over 2} & I_{xy} & I_{xz} & m_i\bar x_i \\ I_{xy} & {(I_{xx}-I_{yy}+I_{zz}) \over 2} & I_{yz} & m_i\bar y_i \\ I_{xz} & I_{yz} & {(I_{xx}+I_{yy}-I_{zz}) \over 2} & m_i\bar z_i \\ m_i\bar x_i & m_i\bar y_i & m_i\bar z_i & m_i \end{bmatrix} $$
As I was having trouble I've tried to create the simplest example that I'm still getting wrong. For this I'm attempting to calculate the inertial torque required to accelerate $\theta_1$ at a constant 1 ${rad\over s^2}$. As $\theta_2$ is constant at 0, I believe this should remove any gyroscopic/Coriolis forces. I've made link 1 weightless so its pseudo-inertia matrix is 0. I've calculated my pseudo-inertia matrix for link 2: $$ I_{xx} = {mr^2 \over 2} = 0.0025\\ I_{yy} = I_{zz} = {ml^2 \over 3} = 2/3 $$ $$ J_2 =\begin{bmatrix} 1.3308 & 0 & 0 & -1 \\ 0 & 0.0025 & 0 & 0 \\ 0 & 0 & 0.0025 & 0 \\ -1 & 0 & 0 & 2 \\ \end{bmatrix} $$
My expected torque for joint 1: $$ T_1 = I\ddot \omega \\ T_1 = {ml^2 \over 3} \times \ddot \omega \\ T_1 = {2\times1\over3}\times1 \\ T_1= {2\over3}Nm $$
The torque calculated by my code for joint 1:
q = [0 0];
qdd = [1 0];
T = calcT(q);
calc_inertial_torque(1, T, J, qdd)
$$ T_1={4\over3}Nm $$
So this is my problem, my code $T_1$ doesn't match up with my simple mechanics $T_1$.
The key functions called are shown below.
function inertial_torque_n = calc_inertial_torque(n, T, J, qdd)
inertial_torque_n = 0;
for j = 1:2
Mnj = 0;
joint_accel = qdd(j);
for i = 1:2
Uij = calcUij(T, i, j);
Ji = J(:,:,i);
Uin = calcUij(T, i, n);
Mnj = Mnj + trace(Uin*Ji*transpose(Uij));
end
inertial_torque_n = inertial_torque_n + Mnj * joint_accel;
end
end
function U=calcUij(T,i,j)
T(:,:,j) = derivative(T(:,:,j));
U = eye(4,4);
for x = 1:i
U = U*T(:,:,x);
end
end
function T = derivative(T)
dt_by_dtheta = [0 -1 0 0
1 0 0 0
0 0 0 0
0 0 0 0];
T = dt_by_dtheta*T;
end
I realise this is a fairly simple robot, and a complicated process - but I'm hoping to scale it up to more DOF once I'm happy it works.