I'm trying to figure out how to get a 6DOF robot to work with Codesys using its kinematics FBs. I'm not sure if my implementation is wrong or an actual bug with the FBs.
To start I used ABB 1200 parameters, this way I can compare the results of Codesys with proven results of an Offline Robot programming software(RoboDK). I'm using virtual drives to map each Axis.
The results don't match, and the TRAFO 6DOF FB outputs values don't make sense. For example, SMC_Trafo_ArticulatedRobot_6DOF is always at error(Invalid_Config), which is strange because its config comes from the Axis group parameters. However, I can Jog the Axis group using MC_GroupJog2. I know these two FBS are not related, but I wanted to compare the results between them.
Also, when I jog the axis group in any X, Y or Z plane, with MCS as Coordinate System, the output angles direction of Axis 2 and Axis 3 move in opposite directions as I would expect from the simulation, plus the TCP XYZ does not match with what I see on the simulation. Even with all Axis at 0 degree, the X,Y,Z TCP does not match.
Also what is interesting is that the result of the SMC_TrafoF_ArticulatedRobot_6DOF, Z coordinate, does not match with the Z value that I get on MC-GroupJOG2.CurrentPosition.c, again, Config is the same.
To sum up I have 3 different results: from RoboDK, which I know is the correct one, the results from MC-GroupJOG2 and the results from SMC_Trafo(TRafoF)Articulated_6DOF. The last two should at least be consistent, but they are not.
I have attached some images and I hope it helps to understand what I am explaining.
I would appreciate it if someone could shed some light on what I found.
I have to do a correction, I was using the DH parameters of a different variation of the IRB 1200, now with the correct DH parameters the TCP values match(reading from MC_GroupJOG2.Currentposition.c). But still something is wrong with the inverse kinematics calculation.
I changed my approach, instead of using MC_GroupJog2, I used MC_MoveLinearAbsolute. I moved the robot, from home(all joints at 0 deg, except J5 at 90 deg) in the simulation software(Robodk) to an arbitrary position, in linear mode along the Y axis, then using the Linear Absolute FB in codesys I moved to the same position:
I attached the final joints position in angles, of both in Robodk and the Codesys values. A nice feature of RoboDK is that i gives you all possible configurations solutions as well.
You will see that the results from Codesys do not match with any possible configuration solution, presented in RoboDK(compare last two images).
The absolute values of J2 and J3 do match but its signals are inverted, and J6 angle value in Codesys would only match if J4 was at -180 and J5 at -93(you can see all of this in the images).
Question to Codesys: Is it possible to access the source code of the 6DOF inverse kinematics FB?
Due to the lack of response from 3S-Codesys, I decided to write my own FBs to perform this task. It took me some time to learn FK and IK, but it was worth it. I had to write some auxiliary Functions as well. Some are redundant with what is included in the Robotics library. The matrix operations are two dimensional arrays instead of a single array.
The FBs are not optimized, do not cover all the elbow configurations, and probably contain some errors, but they are a good starting point. I was using RoboDk to compare my results, and they seem to be consistent.
Because 3S Robotics library is not open source I can't change the parameters of the Homogeneous Transformation matrix and this was the problem(I think). I described in my previous post.
Attached my project archive. Go ahead and test it. Constructive comments are welcome.
Hi
Actually I am also facing codesys Kinematic issues , and upon searching online I found your Post
Thanks for sharing this here
I tested your code Fk is matching with RoboDk, in IK the values are sometimes matching with RoboDK & sometimes its not maybe due to elbow positions
I think euler angles are not matching correctly, Could you please provide the reference for the code you have developed, it would be great help
Thanks
Venkat
If you would like to refer to this comment somewhere else in this project, copy and paste the following link:
Hello,
I'm trying to figure out how to get a 6DOF robot to work with Codesys using its kinematics FBs. I'm not sure if my implementation is wrong or an actual bug with the FBs.
To start I used ABB 1200 parameters, this way I can compare the results of Codesys with proven results of an Offline Robot programming software(RoboDK). I'm using virtual drives to map each Axis.
The results don't match, and the TRAFO 6DOF FB outputs values don't make sense. For example, SMC_Trafo_ArticulatedRobot_6DOF is always at error(Invalid_Config), which is strange because its config comes from the Axis group parameters. However, I can Jog the Axis group using MC_GroupJog2. I know these two FBS are not related, but I wanted to compare the results between them.
Also, when I jog the axis group in any X, Y or Z plane, with MCS as Coordinate System, the output angles direction of Axis 2 and Axis 3 move in opposite directions as I would expect from the simulation, plus the TCP XYZ does not match with what I see on the simulation. Even with all Axis at 0 degree, the X,Y,Z TCP does not match.
Also what is interesting is that the result of the SMC_TrafoF_ArticulatedRobot_6DOF, Z coordinate, does not match with the Z value that I get on MC-GroupJOG2.CurrentPosition.c, again, Config is the same.
To sum up I have 3 different results: from RoboDK, which I know is the correct one, the results from MC-GroupJOG2 and the results from SMC_Trafo(TRafoF)Articulated_6DOF. The last two should at least be consistent, but they are not.
I have attached some images and I hope it helps to understand what I am explaining.
I would appreciate it if someone could shed some light on what I found.
Thank you.
Last edit: rett84 2020-04-20
more posts ...
I have to do a correction, I was using the DH parameters of a different variation of the IRB 1200, now with the correct DH parameters the TCP values match(reading from MC_GroupJOG2.Currentposition.c). But still something is wrong with the inverse kinematics calculation.
I changed my approach, instead of using MC_GroupJog2, I used MC_MoveLinearAbsolute. I moved the robot, from home(all joints at 0 deg, except J5 at 90 deg) in the simulation software(Robodk) to an arbitrary position, in linear mode along the Y axis, then using the Linear Absolute FB in codesys I moved to the same position:
X: 451mm
Y: 385mm
Z: 807mm
Rot:
Z: 0 deg
Y': 180 deg
Z'': 0 deg
I attached the final joints position in angles, of both in Robodk and the Codesys values. A nice feature of RoboDK is that i gives you all possible configurations solutions as well.
You will see that the results from Codesys do not match with any possible configuration solution, presented in RoboDK(compare last two images).
The absolute values of J2 and J3 do match but its signals are inverted, and J6 angle value in Codesys would only match if J4 was at -180 and J5 at -93(you can see all of this in the images).
Question to Codesys: Is it possible to access the source code of the 6DOF inverse kinematics FB?
Note: Raspberry Pi SL 3.5.15.0 + Softmotion 4.6.3
Last edit: rett84 2020-04-26
Due to the lack of response from 3S-Codesys, I decided to write my own FBs to perform this task. It took me some time to learn FK and IK, but it was worth it. I had to write some auxiliary Functions as well. Some are redundant with what is included in the Robotics library. The matrix operations are two dimensional arrays instead of a single array.
The FBs are not optimized, do not cover all the elbow configurations, and probably contain some errors, but they are a good starting point. I was using RoboDk to compare my results, and they seem to be consistent.
Because 3S Robotics library is not open source I can't change the parameters of the Homogeneous Transformation matrix and this was the problem(I think). I described in my previous post.
Attached my project archive. Go ahead and test it. Constructive comments are welcome.
Next will be velocity control.
Hi
Actually I am also facing codesys Kinematic issues , and upon searching online I found your Post
Thanks for sharing this here
I tested your code Fk is matching with RoboDk, in IK the values are sometimes matching with RoboDK & sometimes its not maybe due to elbow positions
I think euler angles are not matching correctly, Could you please provide the reference for the code you have developed, it would be great help
Thanks
Venkat