4
$\begingroup$

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. Robot configuration

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.

$\endgroup$
1
$\begingroup$

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.

$\endgroup$
  • $\begingroup$ Thanks, I'll have another look at my inertia matrix. In the meantime, I've added my axes to the diagram. $\endgroup$ – robot_overlord Dec 11 '15 at 23:33
  • $\begingroup$ I think you're right about the inertia matrix, I've updated it, it has brought the answer closer but still out by a factor of 2. $\endgroup$ – robot_overlord Dec 11 '15 at 23:43
  • 1
    $\begingroup$ @robot_overlord - The formula for J11 is <bunch of stuff> divided by 2. You have summed the (correctly signed) moments of inertia together but failed to divide by 2. $\endgroup$ – Chuck Dec 12 '15 at 1:38
  • 1
    $\begingroup$ It is interesting that you're looking at link 2 from the end effector and defining a negative moment of inertia. All the work I've done builds a kinematic tree from the fixed frame out, so origin of link 2's frame would coincide with the origin of joint 2. $\endgroup$ – Chuck Dec 12 '15 at 1:42

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.