Spurious readings from wheel position on Turtlebot3 via Dynamixel SDK

[nigelgardiner]

I have tested this on several different Turtlebots. One I have updated to the latest firmware, the others may or may not be the latest, but the problem is consistent between them.

After travelling anywhere from 5-20 metres, both wheel positions suddenly jump to large values, and then jump back. This is ruinous for tracking odometry.

I've tracked the issue upstream to the Dynamixel SDK, which is outputting the large values, so the issue is either in the SDK or in the OpenCR board. I'm not sure how to pursue it further, but happy to investigate in more depth if anyone can give me pointers.

I've uploaded an image from plotjuggler showing the issue.

Interestingly, the wheel velocity is also jumping. Whenever the velocity jumps, the position jumps, but sometimes the position jumps without the velocity jumping.

From doing some driving around to try to reproduce it consistently, the problem does not appear to occur when the robot is travelling at high speeds, but appears when it is travelling slowly.

Here is an example of two /joint_state messages, one after the other:

header:
  stamp:
    sec: 1668580973
    nanosec: 494311220
  frame_id: base_link
name:
- wheel_left_joint
- wheel_right_joint
position:
- 56.493452268
- 57.979879857
velocity:
- 0.024532382872617338
- -0.005539570326074882
effort: []
---
header:
  stamp:
    sec: 1668580973
    nanosec: 544304594
  frame_id: base_link
name:
- wheel_left_joint
- wheel_right_joint
position:
- -51673.713111639
- -51673.713111639
velocity:
- 0.024532382872617338
- -0.004748203136635614
effort: []

Sometimes the velocity jumps as well, usually just before a position jump.

---
header:
  stamp:
    sec: 1669016434
    nanosec: 847821724
  frame_id: base_link
name:
- wheel_left_joint
- wheel_right_joint
position:
- 2065.3980378300002
- 2254.35381741
velocity:
- -0.0015827343788785378
- -26658.010179427813
effort: []
---
header:
  stamp:
    sec: 1669016434
    nanosec: 897817650
  frame_id: base_link
name:
- wheel_left_joint
- wheel_right_joint
position:
- -51673.713111639
- -51673.713111639
velocity:
- 0.0
- 0.0
effort: []

The bot is moving very slowly at this time.

Interesting that the position jumped to is exactly the same (-51673.713111639) on both, and the velocity jumped to is -26658.010179427813 ... but not always on both wheels, as you can see in the velocity field above.

So far, the problem seems to occur when one or both wheels has a negative velocity, due to turning.

[nigelgardiner]

I figure there are 3 places the problem could be...

1. OpenCR code
2. Dynamixel motor firmware
3. Dynamixel SDK

Is that logical?

[kgallowa]

Seems like this may be related to the issue discussed in some of the turtlebot3 issues, such as ROBOTIS-GIT/turtlebot3#899. It appears that the root issue is described in ROBOTIS-GIT/turtlebot3#901. I'd love to see a fix for this, because it's causing a lot of problems with navigation.

[jobst-n]

Is this only happening with ros2 systems ?

[ngl-aws]

I've only tested it with ROS2 Foxy, so can't say if it occurs in ROS, but as kgallowa said, the root issue certainly looks to be the issue behind ROBOTIS-GIT/turtlebot3#901