Robotics Toolbox

Answer from Corrado Guarino Lo Bianco -- July 22, 2022, 03:50:18 PM    

Hi Eric,
concerning the orientation mismatching it can depend on one (or both) of these resons:
1) As you know, the RPY notation admits 2 equivalent solutions. It is possible that a
library proposes the first of them, while another the second one. Both of them are
correct.
If you evaluate the rotation matrix starting from the RPY notation it should be the same
for both solutions;
2) The RPY notation used in our Robotics Toolbox comes from the J.J.Craig book. Other
authors use alternative definitions, which produce alternative notations: please,
accurately compare the documentation of the libraries.

Conversely, the Y difference in the first test is very strange.
How did you implement -q1, q3 - 90°, and q6 + 180° in the Robotics Toolbos? Can you send
me the initialization code?

Best regards,
Corrado

> Hello,
> I'm new to Scilab and the Robotic Toolkit.
> I tested the equivalent on SicosLab and Python.
> 
> I compared the results of the direct kinematics with RobotDk and RoKiSim based on a
> Robot 
> known to these 2 software: 'KR270 R2700 Ultra'.
> 
> │ aⱼ₋₁ │ ⍺ⱼ₋₁ │ θⱼ │ dⱼ │ q⁻ │ q⁺ │
> │ 0.0 │ 0.0° │ -q1 │ 0.675 │ -185.0° │ 185.0° │
> │ 0.35 │ -90.0° │ q2 │ 0.0 │ -140.0° │ -5.0° │
> │ 1.15 │ 0.0° │ q3 - 90° │ 0.0 │ -120.0° │ 155.0° │
> │-0.041 │ -90.0° │ q4 │ 1.2 │ -350.0° │ 350.0° │
> │ 0.0 │ 90.0° │ q5 │ 0.0 │ -122.5° │ 122.5° │
> │ 0.0 │ -90.0° │ q6 + 180° │ 0.24 │ -350.0° │ 350.0° │
> 
> The result of fkline from Scilab, SicosLab and Python is different from the RobotDk
and
> 
> RoKiSim tools:
> 
> Tb=fkine(KUKA,[45 -90 90 0 0 0]*%pi/180) gives:
> -> Scilab, SicosLab and Python:
>   -5.024D-33 -0.7071068 0.7071068 1.2657211
>   -8.660D-17 0.7071068 0.7071068 1.2657211
>   -1. -6.123D-17 -6.123D-17 1.784
>    0.0.0.1.
> 
>    With tr2rpy(Tb)/%pi*180 = 0. 90. -45.
> 
> -> RobotDk and RoKiSim:
> X=1.2657211, Y=-1.2657211, Z=1.784, Yaw=45, Pitch=90, Roll=0
> 
> So I have in this case a problem on Y and Yaw.
> 
> Tb=fkine(KUKA,[0 -90 90 15 0 0]*%pi/180) gives:
> -> Scilab, SicosLab and Python:
>   -7.499D-17 -4.330D-17 1. 1.79
>    0.258819 0.9659258 6.123D-17 2.027D-17
>   -0.9659258 0.258819 -6.123D-17 1.784
>    0.0.0.1.
> 
>     With tr2rpy(Tb)/%pi*180 = 90. 75. 90.
> 
> -> RobotDk and RoKiSim:
> X = 1.79, Y = 0, Z= 1.784, Yaw = -90, Pitch = 75, Roll=-90
> 
> So I have in this case a problem on Yaw and Pitch
> 
> Tb=fkine(KUKA,[0 -90 90 0 15 0]*%pi/180) gives:
> -> Scilab, SicosLab and Python:
>   -0.258819 -9.084D-17 0.9659258 1.7818222
>   -6.332D-17 1.7.708D-17 2.407D-17
>   -0.9659258 -4.121D-17 -0.258819 1.7218834
>    0.0.0.1.
> 
>  With tr2rpy(Tb)/%pi*180 = -180. 75. -180.
> 
> -> RobotDk and RoKiSim:
> X=1.781822, Y=0, Z=1.721883, Yaw=0, Pitch=105, Roll=0
> 
> So I have in this case a problem on Yaw, Pitch and Roll
> 
> Tb=fkine(KUKA,[0 -90 90 0 0 15]*%pi/180) gives:
> -> Scilab, SicosLab and Python:
>   -7.499D-17 -4.330D-17 1. 1.79
>    0.258819 0.9659258 6.123D-17 2.027D-17
>   -0.9659258 0.258819 -6.123D-17 1.784
>    0.0.0.1.
> 
>  With tr2rpy(Tb)/%pi*180 = 90. 75. 90.
> 
> -> RobotDk and RoKiSim:
> X=1.79, Y=0, Z=1.78399, Yaw=-90, Pitch=75, Roll=-90
> 
> So I have in this case a problem on Yaw and Roll
> 
> I don't understand where my mistake can come from. Can you please help me.
> 
> Thank you in advance for your feedback,

Answer from Corrado Guarino Lo Bianco -- July 22, 2022, 03:48:26 PM    

This answer has been deleted.

Answer from Eric Josse -- July 22, 2022, 04:03:23 PM    

This answer has been deleted.

Answer from Eric Josse -- July 22, 2022, 04:05:56 PM    

This answer has been deleted.

Answer from Eric Josse -- July 22, 2022, 04:06:39 PM    

> > > Hello Carrado,
> > > Many thanks for your return.
> > > 
> > > -q1 was a mistake from me (I play with the "Flip" attribut in
> Python) the goood MDH parameters are :
> > > │ aⱼ₋₁  │  ⍺ⱼ₋₁  │     θⱼ     │  dⱼ   │   q⁻ 
 
> │ 
> > 
> > > q⁺   │
> > >
> >
>
├───────┼────────┼────────────┼───────┼─────────┼────────┤
> > > │   0.0 │   0.0° │         q1 │ 0.675 │ -185.0° │ 185.0° │
> > > │  0.35 │ -90.0° │         q2 │   0.0 │ -140.0° │  -5.0° │
> > > │  1.15 │   0.0° │   q3 - 90° │   0.0 │ -120.0° │ 155.0°
│
> > > │-0.041 │ -90.0° │         q4 │   1.2 │ -350.0° │ 350.0° │
> > > │   0.0 │  90.0° │         q5 │   0.0 │ -122.5° │ 122.5° │
> > > │   0.0 │ -90.0° │  q6 + 180° │  0.24 │ -350.0° │ 350.0°
│
> > > 
> > > In scilab I use offset on J3 et J6
> > > 
> > > Below you will find my code : 
> > > 
> > > //Conncetion Type
> > R=0
> > P=1
> > 
> > //Theta 
> > Theta1=0
> > Theta2=0
> > Theta3=0
> > Theta4=0
> > Theta5=0
> > Theta6=0
> > 
> > d1=675/1000
> > d2=0/1000
> > d3=0/1000
> > d4=1200/1000
> > d5=0/1000
> > d6=240/1000
> > 
> > a1=0/1000
> > a2=350/1000
> > a3=1150/1000
> > a4=-41/1000
> > a5=0/1000
> > a6=0/1000
> > 
> > Alpha1=0
> > Alpha2=-%pi/2
> > Alpha3=0
> > Alpha4=-%pi/2
> > Alpha5=%pi/2
> > Alpha6=-%pi/2
> > 
> > //Offset
> > Sigma1 = R
> > Sigma2 = R
> > Sigma3 = R
> > Sigma4 = R
> > Sigma5 = R
> > Sigma6 = R
> > 
> > //Offset
> > Offset1=0 
> > Offset2=0
> > Offset3=-%pi/2
> > Offset4=0 
> > Offset5=0
> > Offset6=%pi
> > 
> > Qlim1 = [-185 185]*%pi/180
> > Qlim2 = [-140 -5]*%pi/180
> > Qlim3 = [-120 155]*%pi/180
> > Qlim4 = [-350 350]*%pi/180
> > Qlim5 = [-122.5 122.5]*%pi/180
> > Qlim6 = [-350 350]*%pi/180
> > 
> > Lk = list();
> > //Old syntax: [THETA D A ALPHA SIGMA OFFSET]
> > Lk(1) = Link([Theta1 d1 a1 Alpha1 Sigma1 Offset1],'modified')
> > Lk(2) = Link([Theta2 d2 a2 Alpha2 Sigma2 Offset2],'modified')
> > Lk(3) = Link([Theta3 d3 a3 Alpha3 Sigma3 Offset3],'modified')
> > Lk(4) = Link([Theta4 d4 a4 Alpha4 Sigma4 Offset4],'modified')
> > Lk(5) = Link([Theta5 d5 a5 Alpha5 Sigma5 Offset5],'modified')
> > Lk(6) = Link([Theta6 d6 a6 Alpha6 Sigma6 Offset6],'modified')
> > 
> > Lk(1).qlim=Qlim1
> > Lk(2).qlim=Qlim2
> > Lk(3).qlim=Qlim3
> > Lk(4).qlim=Qlim4
> > Lk(5).qlim=Qlim5
> > Lk(6).qlim=Qlim6
> > 
> > KUKA = SerialLink(Lk, 'name', 'KUKA');
> > 
> > //Target
> > Q=[0 -90 90 0 15 0]*%pi/180;
> > plot_robot(KUKA,Q);
> > Tb = fkine(KUKA,Q);
> > unix_w("echo " + "Tb=" + strcat(string(Tb), " "));

Answer from Corrado Guarino Lo Bianco -- July 22, 2022, 04:19:53 PM    

Hi Eric,
just to be sure: did you try using [-45 -90 90 0 0 0]?

Best regards,
Corrado

> > > > Hello Carrado,
> > > > Many thanks for your return.
> > > > 
> > > > -q1 was a mistake from me (I play with the "Flip" attribut
in
> > Python) the goood MDH parameters are :
> > > > │ aⱼ₋₁  │  ⍺ⱼ₋₁  │     θⱼ     │  dⱼ   │  
> q⁻   
> > │ 
> > > 
> > > > q⁺   │
> > > >
> > >
> >
>
├───────┼────────┼────────────┼───────┼─────────┼────────┤
> > > > │   0.0 │   0.0° │         q1 │ 0.675 │ -185.0° │
185.0°
> │
> > > > │  0.35 │ -90.0° │         q2 │   0.0 │ -140.0° │ 
-5.0°
> │
> > > > │  1.15 │   0.0° │   q3 - 90° │   0.0 │ -120.0° │
155.0°
> │
> > > > │-0.041 │ -90.0° │         q4 │   1.2 │ -350.0° │
350.0°
> │
> > > > │   0.0 │  90.0° │         q5 │   0.0 │ -122.5° │
122.5°
> │
> > > > │   0.0 │ -90.0° │  q6 + 180° │  0.24 │ -350.0° │
350.0°
> │
> > > > 
> > > > In scilab I use offset on J3 et J6
> > > > 
> > > > Below you will find my code : 
> > > > 
> > > > //Conncetion Type
> > > R=0
> > > P=1
> > > 
> > > //Theta 
> > > Theta1=0
> > > Theta2=0
> > > Theta3=0
> > > Theta4=0
> > > Theta5=0
> > > Theta6=0
> > > 
> > > d1=675/1000
> > > d2=0/1000
> > > d3=0/1000
> > > d4=1200/1000
> > > d5=0/1000
> > > d6=240/1000
> > > 
> > > a1=0/1000
> > > a2=350/1000
> > > a3=1150/1000
> > > a4=-41/1000
> > > a5=0/1000
> > > a6=0/1000
> > > 
> > > Alpha1=0
> > > Alpha2=-%pi/2
> > > Alpha3=0
> > > Alpha4=-%pi/2
> > > Alpha5=%pi/2
> > > Alpha6=-%pi/2
> > > 
> > > //Offset
> > > Sigma1 = R
> > > Sigma2 = R
> > > Sigma3 = R
> > > Sigma4 = R
> > > Sigma5 = R
> > > Sigma6 = R
> > > 
> > > //Offset
> > > Offset1=0 
> > > Offset2=0
> > > Offset3=-%pi/2
> > > Offset4=0 
> > > Offset5=0
> > > Offset6=%pi
> > > 
> > > Qlim1 = [-185 185]*%pi/180
> > > Qlim2 = [-140 -5]*%pi/180
> > > Qlim3 = [-120 155]*%pi/180
> > > Qlim4 = [-350 350]*%pi/180
> > > Qlim5 = [-122.5 122.5]*%pi/180
> > > Qlim6 = [-350 350]*%pi/180
> > > 
> > > Lk = list();
> > > //Old syntax: [THETA D A ALPHA SIGMA OFFSET]
> > > Lk(1) = Link([Theta1 d1 a1 Alpha1 Sigma1 Offset1],'modified')
> > > Lk(2) = Link([Theta2 d2 a2 Alpha2 Sigma2 Offset2],'modified')
> > > Lk(3) = Link([Theta3 d3 a3 Alpha3 Sigma3 Offset3],'modified')
> > > Lk(4) = Link([Theta4 d4 a4 Alpha4 Sigma4 Offset4],'modified')
> > > Lk(5) = Link([Theta5 d5 a5 Alpha5 Sigma5 Offset5],'modified')
> > > Lk(6) = Link([Theta6 d6 a6 Alpha6 Sigma6 Offset6],'modified')
> > > 
> > > Lk(1).qlim=Qlim1
> > > Lk(2).qlim=Qlim2
> > > Lk(3).qlim=Qlim3
> > > Lk(4).qlim=Qlim4
> > > Lk(5).qlim=Qlim5
> > > Lk(6).qlim=Qlim6
> > > 
> > > KUKA = SerialLink(Lk, 'name', 'KUKA');
> > > 
> > > //Target
> > > Q=[0 -90 90 0 15 0]*%pi/180;
> > > plot_robot(KUKA,Q);
> > > Tb = fkine(KUKA,Q);
> > > unix_w("echo " + "Tb=" + strcat(string(Tb), "
> "));

Answer from Eric Josse -- July 22, 2022, 04:47:22 PM    

Corrado,
OK thnaks

With my first exemple, 
- In python, if I set the flip parameter (joint moves in opposite direction) of q1 to 
"True" then Y is correcte. 
- In Scilab or Sciscolab if i inverse the sing of J1 target (compared to RoboDk and 
RoKiSim) then Y is correcte.

But in this to case Yaw is in the opposite direction
Again, maybe it's like you say an RPY notation problem
I will work on that.


Another track: Could this difference come from a problem of orientation of the base of the

robot in relation to the world?

PI, Under RobotDk and RokiSim no offset is added to the database.
Below you will find the definition of the same robot for RoKiSim:

<robot_definition>
<robot_dk name=KUKA_KR_270_R2700_ultra>
<axis id="Base" maxvalue="0" minvalue="0"
alpha="0" a="0" theta="0" d="0"/>
<axis id="Joint1" limsup="185.000" liminf="-185.000"
alpha="0.000" a="0.000" theta="0.000" 
d="675.000"/>
<axis id="Joint2" limsup="-5.000" liminf="-140.000"
alpha="-90.000" a="350.000" 
theta="0.000" d="0.000"/>
<axis id="Joint3" limsup="155.000" liminf="-120.000"
alpha="0.000" a="1150.000" 
theta="-90.000" d="0.000"/>
<axis id="Joint4" limsup="350.000" liminf="-350.000"
alpha="-90.000" a="-41.000" 
theta="0.000" d="1200.000"/>
<axis id="Joint5" limsup="122.500" liminf="-122.500"
alpha="90.000" a="0.000" 
theta="0.000" d="0.000"/>
<axis id="Joint6" limsup="350.000" liminf="-350.000"
alpha="-90.000" a="0.000" 
theta="180.000" d="240.000"/>
</robot_dk>
<robot_geometry>
<geometry geo="KUKA_KR_270_R2700_ultra-0" name="Base"/>
<geometry geo="KUKA_KR_270_R2700_ultra-1" name="Joint1"/>
<geometry geo="KUKA_KR_270_R2700_ultra-2" name="Joint2"/>
<geometry geo="KUKA_KR_270_R2700_ultra-3" name="Joint3"/>
<geometry geo="KUKA_KR_270_R2700_ultra-4" name="Joint4"/>
<geometry geo="KUKA_KR_270_R2700_ultra-5" name="Joint5"/>
<geometry geo="KUKA_KR_270_R2700_ultra-6" name="Joint6"/>
</robot_geometry>
<CAD_base x="0.000" y="0.000" z="0.000"
rx="0.000" ry="0.000" rz="0.000"/>
<CAD_scale FACTOR="1.000000000"/>
<CAD_offsets cad_theta1="0.000" cad_theta2="0.000"
cad_theta3="0.000" cad_theta4="0.000" 
cad_theta5="0.000" cad_theta6="0.000"/>
<Angle_Type id="5"/>
</robot_definition>

Best regards,
Eric

Answer from Corrado Guarino Lo Bianco -- July 22, 2022, 05:00:27 PM    

Eric,
ok, it seems that the problem on Y depends on the sign of q1.

Concerning the orientation, I still think that differences depend on the two points I
lighted. Unfortunately, I do not know the sintax of RoKiSim, so that I cannot help you.

Best regards,
Corrado

> Corrado,
> OK thnaks
> 
> With my first exemple, 
> - In python, if I set the flip parameter (joint moves in opposite direction) of q1 to

> "True" then Y is correcte. 
> - In Scilab or Sciscolab if i inverse the sing of J1 target (compared to RoboDk and 
> RoKiSim) then Y is correcte.
> 
> But in this to case Yaw is in the opposite direction
> Again, maybe it's like you say an RPY notation problem
> I will work on that.
> 
> 
> Another track: Could this difference come from a problem of orientation of the base
of
> the 
> robot in relation to the world?
> 
> PI, Under RobotDk and RokiSim no offset is added to the database.
> Below you will find the definition of the same robot for RoKiSim:
> 
> <robot_definition>
> <robot_dk name=KUKA_KR_270_R2700_ultra>
> <axis id="Base" maxvalue="0" minvalue="0"
> alpha="0" a="0" theta="0" d="0"/>
> <axis id="Joint1" limsup="185.000" liminf="-185.000"
> alpha="0.000" a="0.000" theta="0.000" 
> d="675.000"/>
> <axis id="Joint2" limsup="-5.000" liminf="-140.000"
> alpha="-90.000" a="350.000" 
> theta="0.000" d="0.000"/>
> <axis id="Joint3" limsup="155.000" liminf="-120.000"
> alpha="0.000" a="1150.000" 
> theta="-90.000" d="0.000"/>
> <axis id="Joint4" limsup="350.000" liminf="-350.000"
> alpha="-90.000" a="-41.000" 
> theta="0.000" d="1200.000"/>
> <axis id="Joint5" limsup="122.500" liminf="-122.500"
> alpha="90.000" a="0.000" 
> theta="0.000" d="0.000"/>
> <axis id="Joint6" limsup="350.000" liminf="-350.000"
> alpha="-90.000" a="0.000" 
> theta="180.000" d="240.000"/>
> </robot_dk>
> <robot_geometry>
> <geometry geo="KUKA_KR_270_R2700_ultra-0" name="Base"/>
> <geometry geo="KUKA_KR_270_R2700_ultra-1" name="Joint1"/>
> <geometry geo="KUKA_KR_270_R2700_ultra-2" name="Joint2"/>
> <geometry geo="KUKA_KR_270_R2700_ultra-3" name="Joint3"/>
> <geometry geo="KUKA_KR_270_R2700_ultra-4" name="Joint4"/>
> <geometry geo="KUKA_KR_270_R2700_ultra-5" name="Joint5"/>
> <geometry geo="KUKA_KR_270_R2700_ultra-6" name="Joint6"/>
> </robot_geometry>
> <CAD_base x="0.000" y="0.000" z="0.000"
> rx="0.000" ry="0.000" rz="0.000"/>
> <CAD_scale FACTOR="1.000000000"/>
> <CAD_offsets cad_theta1="0.000" cad_theta2="0.000"
> cad_theta3="0.000" cad_theta4="0.000" 
> cad_theta5="0.000" cad_theta6="0.000"/>
> <Angle_Type id="5"/>
> </robot_definition>
> 
> Best regards,
> Eric

Answer from Corrado Guarino Lo Bianco -- July 25, 2022, 09:28:52 AM    

Hi Eric,
the orientation of the world frame is the same of frame 0 (base frame), i.e., it coincides
with frame 1 when q1 = 0. If you need to assume another orientation (or another position)
for the world frame, you can initialize your manipulator by adding a base transform matrix
between the world frame and the base frame (to this purpose, please, read the SerialLink
help). In the same way, you can add a tool frame transform matrix after the last joint.

Best regards,
Corrado

> Other exemple : 
> 
> q = [-15*pi/180,-15*pi/180,-15*pi/180,-15*pi/180,-15*pi/180,-15*pi/180]
> q = [-0.2617993877991494, -0.2617993877991494, -0.2617993877991494,
-0.2617993877991494,
> 
> -0.2617993877991494, -0.2617993877991494]
> 
> With the toolkit : 
> T =    0.4848    0.5181    0.7046    2.604   
>   -0.6387    0.7601   -0.1194   -0.681   
>   -0.5975   -0.3921    0.6995    1.705   
>    0         0         0         1       
> 
> T.rpy = [-29.27619346  36.68874483 -52.79959892]
> 
> With Robotic Tools: 
> X=2.603765 , Y= 0.681033 , Z=1.705007 , Yaw=29.276, Pitch=36.68, Roll=52.8
> 
> Conclusion :
> I still have a problem on Y (sign), Yaw and Roll
> 
> If I inverse the sign of q1 : 
> q = [15*pi/180,-15*pi/180,-15*pi/180,-15*pi/180,-15*pi/180,-15*pi/180]
> 
> q = [0.2617993877991494, -0.2617993877991494, -0.2617993877991494,
-0.2617993877991494,
> 
> -0.2617993877991494, -0.2617993877991494]
> T =    0.7392    0.06867   0.6699    2.595   
>   -0.3107    0.9173    0.2489    0.7121  
>   -0.5975   -0.3921    0.6995    1.705   
>    0         0         0         1       
> 
> T.rpy = [-29.27619346  36.68874483 -22.79959892]
> 
> Conclusion :
> Value of X and Y are not correct
> 
> 
> This is why I will suggest a problem with the orientation of the robot's base in
> relation 
> to the world!
> What do you think about?
> 
> How is the word oriented in Scilab?
> 
> Best regards,
> Eric