Calculating required torques for a given trajectory using Lagrange-Euler

Question

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.

Answer 1

You don't have axes shown, so the math is a little hard to follow, but based on the bottom row of $ J_2$ it looks like y is along the length of link 2. If this is the case, then I would expect rotation and x and z to have the same moment of inertia, because each would be rotating the cylinder about the end instead about its longitudinal axis.

This means that Ixx and Izz should cancel where their signs are different, as in J11 and J33, leaving only Iyy/2, but instead of that I see zeros as the entries there. As this is (a model of) a real link, Iyy must be something; I would assume $ mr^2/2$, or 0.0025. This should be in J11 and J33. I haven't checked your math on J22, but given that Iyy doesn't cancel out there and you end give a very "round" 2/3 as an answer I suspect it is incorrect as well.

Check your math on your J2 matrix and/or provide drawings of link 2 with a set of datums (dati?) (side note: the plural of datum is data, but I think it would be confusing to ask for "drawings with a set of data" when I mean graphical reference points, so I'll leave it as datums) I think this is where your problem lies.