Using "rne_python" in "MDH SerialLink" to Symbolic calculation, the result is not same with it in Matlab #326
Description
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)