Joint angles wrong

[karl-exwzd]

I have successfully connected to my TM5-900 using this package and following the instructions in the README.md, but it looks like the reported joint angles are wrong, I don't know about the velocities and torques...

This is what the robot looks like:

And this is what RViz reports:

At first, I thought the joint order was wrong, as when I tried to manually move the J1 joint (shoulder_1_joint) it moved the J2 (shoulder_2_joint).

I tried to swap the lines in here, but then I saw combined movements of multiple joints in RViz...

When checked with rostopic echo /joint_states I get the following:

name: [shoulder_1_joint, shoulder_2_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
position: [6.171496059487189, 0.5187536200480904, 10.582116397800602, -2.958383948507209, 0.11735999982528693, 1.5004451981749274]
velocity: [-0.0, -6.220975319068639e-06, -0.0, 0.0, 9.696273856980042e-06, 9.696273856980042e-06]
effort: [6.43369921875, -11.68569921875, -33.8753984375, -3.3955200195312503, -0.9072000122070313, -0.9331199951171876]

These are in radians, the robot reports completely different angles and none of them above 90 degrees.

Has this package been tested with TM5-900? Am I missing something?

[karl-exwzd]

Upon further investigation I noticed that the TCP coordinates are the same as what RViz reports for joint angles.

This is what I see in TMflow:

And this is what is reported in RViz:

Essentially the Cartesian TCP is interpreted as joint angles in ROS... Is there an option somewhere to make TMFlow report joint angles instead of TCP coordinates? I couldn't find it so far.

[Yuki-cpp]

That's probably your Ethernet slave that is wrongfully configured. Did you enable all the settings displayed in the Data Table Setting that the README mentions (and only them?) Enabling too many or too few causes this kind of mix up

[karl-exwzd]

@Yuki-cpp Very good point! I just figured this out as well... it felt like an offset issue. I double checked my configuration and it was correct, however, there was a line permutation in my settings compared to the documentation:

My configuration looked like this:

As you can see the Coord_Robot_Flange option comes before Joint_Angle in the robot, whereas in the documentation is the opposite...

So I dug further found the frame unpacking function. I commented the line 223 and uncommented 214 and now it works as expected.

Now I would like to know what is the reason my robot has these options switched... is it a robot firmware thing? I have TMFlow 1.80.5300.

I checked the documentation (p. 265) regarding this and the order is the same as in the robot:

It feels like code has an issue, can anybody confirm this with a different robot?

[jvdtoorn]

@exwzd-kk I also had this exact issue! That was one of the reasons for me to create my own driver.

[karl-exwzd]

@julespalles I guess you're referring to this repository. The good thing about this official package is that it attempts to be ROS-Industrial compliant and that requires significant effort on top of a standard functional ROS package.

Also, gathering OSS development effort in one place allows for higher quality over time. In the case of this issue we must first understand the reason for why the author (@kentsai0319 ) decided to swap the lines, perhaps it's because of compatibility reasons between the TM5-700, TM5-900 and TM12?

In #10 you mentioned that this driver worked well for you, what robot did you use it with?

[jvdtoorn]

@exwzd-kk you should not see my driver as a replacement for this repository. It implements easy-to-use serialisation and deserialisation of the TCP communication protocol, acting as a solid foundation for any subsequent custom control software.

[karl-exwzd]

@jvdtoorn Got it :)