Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Using "rne_python" in "MDH SerialLink" to Symbolic calculation, the result is not same with it in Matlab #326

Open
xxel61 opened this issue Jul 19, 2022 · 1 comment

Comments

@xxel61
Copy link

xxel61 commented Jul 19, 2022

I use robotics-toolbox-matlab in Matlab to Symbolic calculation, I got result as followed:
Tau =
[L1^2ddq1m1 + L1^2ddq1m2 + L2^2ddq1m2 + L2^2ddq2m2 + L2gm2cos(q1 + q2) + L1gm1cos(q1) + L1gm2cos(q1) - L1L2dq2^2m2sin(q2) + 2L1L2ddq1m2cos(q2) + L1L2ddq2m2cos(q2) - 2L1L2dq1dq2m2sin(q2),
L2m2(cos(q2)(L1ddq1 + gcos(q1)) + sin(q2)(L1dq1^2 - gsin(q1)) + L2*(ddq1 + ddq2))]

But in rne_python the result is:
[L_1*(m_1*(L_1qdd_0 + g) - 1.0m_2*((L_1qd_0**2 - 1.0L_1qdd_0sin(q_1) + L_2*(1.0qd_0 + qd_1)**2)sin(q_1) - 1.0(L_1qdd_0cos(q_1) + L_2qdd_0 + L_2*qdd_1 + g)*cos(q_1))), 0]

I use different symbols Matlab q1 dq1 dqq1 is python q_0 qd_0 qdd_0 and q2 dq2 dqq2 is python q_1 qd_1 qdd_1.

Obviously, the result tau2 shouldn't be zero.

I put the code at the end. Please check it, is there something wrong with my code? I use it in the wrong way. Or it is a bug in rne_python for MDH SerialLink.

MATLAB CODE:

clear,clc,close all;
format compact
syms q1 q2 dq1 dq2 ddq1 ddq2 m1 m2 L1 L2 Izz1 Izz2 g real
L(1)=RevoluteMDH('d',0,'a',0,'alpha',0); 
L(2)=RevoluteMDH('d',0,'a',L1,'alpha',0,'offset',0); %-pi/2
twolink = SerialLink(L); 
theta=sym('theta',[1 2]);
L(1).m = m1; L(2).m = m2;
L(1).Jm=0;L(2).Jm=0;

L(1).r = [L1 0 0];L(2).r = [L2 0 0];

grav=[0 g 0];
q=[q1 q2];
qd=[dq1 dq2];
qdd=[ddq1 ddq2];
T01=L(1).A(q(1));T12=L(2).A(q(2));
G=twolink.gravload(q,grav);  %Gravity Term
G=simplify(G);
M=twolink.inertia(q);         %Inertia Matrix
M=simplify(M);
C=twolink.coriolis(q, qd);  %Coriolis Matrix
C=simplify(C);
Tau =twolink.rne(q,qd,qdd,grav); %,'gravity'
Tau=simplify(Tau);
Tau

PYTHON CODE:

import roboticstoolbox as rtb
from spatialmath.base import symbol, simplify
from math import pi
import numpy as np

q = symbol('q_:2')
qd = symbol('qd_:2')
qdd = symbol('qdd_:2')

m1 = symbol('m_1')
m2 = symbol('m_2')
lc1 = symbol('l_c_1')
lc2 = symbol('l_c_2')
L1 = symbol('L_1')
L2 = symbol('L_2')
g = symbol('g')
Izz1 = symbol('Izz1')
Izz2 = symbol('Izz2')


Link1 = rtb.RevoluteMDH(alpha=0, a=0, offset=0, d=0, m=m1, r=[L1, 0, 0])
Link2 = rtb.RevoluteMDH(alpha=0, a=L1, offset=0, d=0, m=m2, r=[L2, 0, 0])

twoLink_e2 = rtb.SerialLink([
    Link1,
    Link2
],gravity=[0, -g, 0], name="twoLink-example2", symbolic = True)
print(twoLink_e2)
twoLink_e2.dynamics()
tau = twoLink_e2.rne_python(q, qd, qdd, debug=True)
tau = simplify(tau)
print(tau)
@xxel61
Copy link
Author

xxel61 commented Jul 24, 2022

#266
#267
#268

I read the above article and made changes to DHRobot.py.
Now the result from rne_python is the same as Matlab.
But I don't know why...

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant