2
$\begingroup$

Assume two uniform cylindrical links robot has 3 DOF as shown in the figure. Where link one has 1 dof that is rotation on its main axis. The second link has 2 DOF yaw and pitch movements. I can calculate forward kinematic, Jacobian, kinetic and potential energy of it using below code. However, I am not sure how can I verify that the computed kinetic and potential energy equations are correct? Moreover, the computed equations seem not right, for example if we check PE for q2 joint (which is m2g((r2_0(1,1))) becomes zero with this code. However, we know it should be equal to somewhat like m2*g(L2/2)*sinq2.

It would be great if someone can help me in this. Thank you in advance.

enter image description here

DH parameters of 3 DOF robot

% DH parameters
% theta, d, a, alpha
dh_params = [q1,       0,  0,     0; 
             q2,      L1,  0,  pi/2;
             q3+pi/2,  0,  0,  pi/2; 
             0,        0, L2,    0];

homogeneous transformation matrices

% Define homogeneous transformation matrices
T = sym(zeros(4,4,4));     %T = sym(zeros(4,4,2));
for i=1:4
    T(:,:,i) = [cos(dh_params(i,1)), -sin(dh_params(i,1))*cos(dh_params(i,4)), sin(dh_params(i,1))*sin(dh_params(i,4)), dh_params(i,3)*cos(dh_params(i,1));
                sin(dh_params(i,1)), cos(dh_params(i,1))*cos(dh_params(i,4)), -cos(dh_params(i,1))*sin(dh_params(i,4)), dh_params(i,3)*sin(dh_params(i,1));
                0, sin(dh_params(i,4)), cos(dh_params(i,4)), dh_params(i,2);
                0, 0, 0, 1];
end

% Calculate forward kinematics
T01 = simplify(T(:,:,1));
T12 = simplify(T(:,:,2));
T02 = simplify(T01*T(:,:,2));
T23 = simplify(T(:,:,3));
T03 = simplify(T02*T(:,:,3));
T34 = simplify(T(:,:,4));
T04 = simplify(T03*T(:,:,4));

Then

px = T04(1,4);
py = T04(2,4);
pz = T04(3,4);

%ri_i represents the position of the mass center of link i relative to its own coordinate frame
r1_1 = [0; 0; L1/2];
r2_2 = [0; L2/2; 0];   %r2_2 = [0; -L2/2; 0]; may be also
r3_3 = [L2/2; 0; 0];

%ri_0 is the distance of ith link mass center to base coordinates frame
r1_0 = simplify(T01 * [r1_1; 1]);
r2_0 = simplify(T02 * [r2_2; 1]);
r3_0 = simplify(T03 * [r3_3; 1]);

Calculate Jacobian matrix

J = simplify([diff(px, q1), diff(px, q2), diff(px, q3), diff(px, q4); diff(py, q1), diff(py, q2), diff(py, q3), diff(py, q4); diff(pz, q1), diff(pz, q2), diff(pz, q3), diff(pz, q4)]);

Output:

J = 
[ L2*sin(q1 + q2)*sin(q3),  L2*sin(q1 + q2)*sin(q3), -L2*cos(q1 + q2)*cos(q3), 0]
[-L2*cos(q1 + q2)*sin(q3), -L2*cos(q1 + q2)*sin(q3), -L2*sin(q1 + q2)*cos(q3), 0]
[0,                                               0,              -L2*sin(q3), 0]

Calculate Kinetic energy

% Calculate kinetic energy
KE = simplify(0.5*(m1*(diff(px, q1)^2 + diff(py, q1)^2) + m2*(diff(px, q1)^2 + diff(py, q1)^2 + diff(px, q2)^2 + diff(py, q2)^2) ...
    + m2*(diff(px, q1)^2 + diff(py, q1)^2 + diff(px, q2)^2 + diff(py, q2)^2) + diff(px, q3)^2 + diff(py, q3)^2)...
    + I1*dq1^2 + I2*(dq1 + dq2)^2 + I3*(dq1 + dq2 +dq3)^2);

Calculate potential energy

% Calculate potential energy
PE = simplify(m1*g*(r1_0(1,1)) + m2*g*((r2_0(1,1))+m2*g*((r3_0(1,1)))));

PE output:

PE =-(L2*g^2*m2^2*cos(q1 + q2)*sin(q3))/2

At q2=-45 the figure drawn above would look like enter image description here

Improve this question
$\endgroup$

2 Answers 2

Reset to default
1
$\begingroup$

Welcome to Robotics, DANAISH! I'm sorry I don't have access to Matlab anymore, so I'm not able to follow along with the symbolic math you've posted here. I'm assuming you've double-checked your math for the DH parameters and creating the 4x4 transforms from the DH parameters, so I'm looking for things that could be wrong elsewhere in your work.

In looking at it from that perspective, I see you making transforms like:

T01 = simplify(T(:,:,1));

Which looks like the transform from frame 0 to frame 1. Then, when I see:

T02 = simplify(T01*T(:,:,2));

I think you're using the transform from frame 0 to frame 1 times the transform from frame 1 to frame 2 to get the transform from frame 0 to frame 2.

In looking at this though, later you have:

%ri_i represents the position of the mass center of link i relative to its own coordinate frame
r1_1 = [0; 0; L1/2];

so r1_1 is the center of mass of L1 in the F1 frame. That's fine, but then if you have:

%ri_0 is the distance of ith link mass center to base coordinates frame
r1_0 = simplify(T01 * [r1_1; 1]);

I think this is incorrect because now it looks to me like you're using T01, the transform FROM frame 0 TO frame 1 to convert r1_1, which is the center of mass as defined in frame 1.

I don't think you're applying the transforms correctly here, I think you should be converting r1_1 from 1 to 0, meaning I think you should be doing:

r1_0 = simplify(T10 * [r1_1; 1]);

with the transform T10 being the inverse of T01:

T10 = T01^(-1);

Another problem you're going to have though is that you've got a degenerate system, in that there's no link for Frame 2. You have Frame 2 go directly into Frame 3, and then there's a definition for the center of mass for L2 with respect to Frame 3. You seem to be guessing about what the definition is for Frame 2's link mass:

r2_2 = [0; L2/2; 0];   %r2_2 = [0; -L2/2; 0]; may be also

The way it's defined, for a right-handed convention, the position would be L2/2, but it's also dependent on the position of q3, so (as drawn, if it's drawn for all the joints in the zero position) more like (L2/2)*cos(q3), because at q3=0 you have the offset at L2/2, but if q3 rotates 90 degrees then you have the center of mass for L2 along Frame 2's z-axis.

Finally I'll caution you that your rotation conventions aren't all right-handed, but you don't reflect the polarity difference in your DH table. If you point your right thumb along the z-axis (or any rotation axis) then a positive rotation is the direction your fingers curl to make a fist. This means that your q1 rotation is left-handed, q2 rotation is right-handed, and q3 rotation is left-handed. If you want to get the motions as indicated then you can throw some negative signs in the DH table to account for the left-handed q1 and q3 rotations.

(removed a section about q3 not being able to affect L2's potential energy, but it can because I'm an idiot and misread the diagram. q3's z-axis points up, not along L2!)

Improve this answer
$\endgroup$
5
  • $\begingroup$ Thank you for detailed description and correcting me, it really helped. For the potential energy, you are right q3 can not move the COM of L2 when the q2=0. However if q2 is greater than 0, in that case q3 would has impact on the potential energy as the L2 has 2 DOF and both joints can move it. Foreaxmple taking only L2 when q2=45, and q3=0 we can imagine the potential energy for L2 would be 'PE=m2*g*(L2/2)*sin(q2)'. But now the movement of q3 can influence the potential energy as it can change the height of the centre of mass of L2. Please correct me if I am not right. again thank you $\endgroup$
    – DANAISH
    Jun 4 at 8:23
  • 1
    $\begingroup$ @DANAISH (using italics because we don't have strikethrough in comments) even if q2 sets the joint to 45 degrees, q3 is still aligned with the center of mass of L2, so there is no rotation about q3 that's going to move the L2 center of mass. I think the kind of motion you're describing would be if q ONE were rotating, then that does move the center of mass of L2, but there are no scenarios (as drawn) where q3 can move the center of mass of L2 $\endgroup$
    – Chuck ♦
    Jun 5 at 7:08
  • $\begingroup$ I am confuse here; as both joints are attached at the start of the link (L2) and the both joints q2 and q3 can rotate the L2 and, therefore both should influence each others rotation frame. If I see here (i.stack.imgur.com/yPpco.png) when q2=45 now the joint q3 can have some impact on PE by rotating it. Thank you again. $\endgroup$
    – DANAISH
    Jun 7 at 4:07
  • 1
    $\begingroup$ @DANAISH - Yeah, you're right, I'm so sorry! Somehow I totally misread the diagram and though q3's z-axis was pointing through the center of gravity. You're totally right, I'll edit the answer. $\endgroup$
    – Chuck ♦
    Jun 7 at 10:22
  • $\begingroup$ No problem! Thank you again for considering it. $\endgroup$
    – DANAISH
    Jun 9 at 5:30
0
$\begingroup$

Kinetic Energies

In general, the kinetic energy of a link will be:

$$K = \frac{1}{2}mv^2 + \frac{1}{2}I\omega^2$$

For the link of length $l_1$:

$$K_1 = \frac{1}{2}m_1{v_1}^2 + \frac{1}{2}I_1{\omega_1}^2$$

with

$$v_1 = \left| \frac{1}{2}{\bf r_1}{\dot{\theta}}_1 \right|$$

$${\bf r_1} = l_1\left(\hat{x}\cos\theta_1+\hat{y}\sin\theta_1\right)$$

$$I_1=\frac{1}{12}m_1{l_1}^2$$

$$\omega_1 = {\dot{\theta}}_1$$

For the link of length $l_2$:

$$K_2 = \frac{1}{2}m_2{v_2}^2 + \frac{1}{2}I_2{\omega_2}^2$$

with

$$v_2 = \left| {\bf r_1}{\dot{\theta}}_1 + \frac{1}{2}{\bf r_2}\left({\dot{\theta}}_1+{\dot{\theta}}_1\right) \right|$$

$${\bf r_2} = l_2\left[\hat{x}\cos\left(\theta_1+\theta_2\right)+ \hat{y}\sin\left(\theta_1+\theta_2\right)\right]$$

$$I_2=\frac{1}{12}m_2{l_2}^2$$

$$\omega_2 = {\dot{\theta}}_2$$

Potential Energies

$$P_1 = \frac{1}{2}m_1l_1g\sin\theta_1$$

$$P_2 = m_2\left( l_1 + \frac{1}{2}l_2 \right) g \sin(\theta_1+\theta_2) $$

Improve this answer
$\endgroup$
1
  • $\begingroup$ How are your frames defined? You give the equation P1 = (1/2)*m*l*g*sin(theta1), but then this means that theta1 has some impact on the potential energy of link 1, but it doesn't in OP's drawing. Your equations are also missing q3/theta3. $\endgroup$
    – Chuck ♦
    Jun 2 at 12:41

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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