MyCobot 320 M5 makes 0.2m arc collision unless joint 6 starts at -π instead of +π

[fschiro]

I believe there is a problem with the robot move_angles function or the latest firmware.

Robot: MyCobot 320 M5 2024
System Version: 1.5
Basic Version: 2.4
Atom Version: 5.2
Trajectory planning: MoveIt2 trajectory
Controller: send every waypoint in trajectory to pymycobot send_angles method without any delay

Issue:
When I move from starting position to target position, the robot does not move straight, instead it does a huge crazy arc, dipping 0.2 meters crashing into the table to then go up again.
However, I noticed if I simply rotate joint6 360 degrees before the motion then the arm moves straight!

To reproduce move from start to target:

Case 1: causes crazy arc pattern
Start: [-0.3128, 0.2822, 1.5860, 1.4005, 1.2870, 3.0786]
target: [-0.2208, 1.3928, -0.3313, 2.1859, 1.3989, 3.0709]

Case 2: reasonable motion identical joint angles except for 360 degree turn of joint6:
Start: [-0.3128, 0.2822, 1.5860, 1.4005, 1.2870, -3.0786]
target: [-0.2208, 1.3928, -0.3313, 2.1859, 1.3989, 3.0709]

Start and target are vectors are joint-state angles vectors in radians.

Video: https://youtu.be/GsN7CmC3Eys

When watching the video in slow motion, it looks like link4 goes backwards before going forwards. No idea why it seems like a bug!

Note: In order to do this test one needs calibrated offsets.
If I just use send_angle on start or target, then the robot is off by about 5 degrees.
In order to actually reach start and target positions with accuracy I have to add offsets to each joint angle.

Instead of:
target: [-0.2208, 1.3928, -0.3313, 2.1859, 1.3989, 3.0709]
I have to add offset vector like this:
target: [-0.2208, 1.3928, -0.3313, 2.1859, 1.3989, 3.0709] + [0.005046, -0.030167, -0.023075, -0.008153, 0.005604 3.368944]

This will get me to my target position. To get the offset vector just perform the move from start to target and you will see error between target position and actual position. The error is systematic, so you can get rid of the error with offsets: calibrated_target = target + ( target - mc.get_angles() ).

Note: This is how I get my firmware version

from pymycobot.mycobot320 import MyCobot320
port = "/dev/mycobot"
baudrate = 115200
mc = MyCobot320(port, baudrate)

print("System Version:", mc.get_system_version())
System Version: 1.5
print("Basic Version:", mc.get_basic_version())
Basic Version: 2.4
print("Atom Version:", mc.get_atom_version())
Atom Version: 5.2

[fschiro]

send_angles() does not take shortest path - caused hardware damage.

Issue: send_angles() does not calculate the shortest rotation path between angles, causing unexpected large rotations that can result in collisions and hardware damage.
What happened:

Current position: Joint 6 at 175°
Target position: Joint 6 at 161°
Expected behavior: Rotate -14° (shortest path)
Actual behavior: Rotated +180° to 0°, then -194° to 161° (total 346° rotation)

Impact: During the initial +180° rotation, the gripper collided with the table, breaking the left gripper finger.

Root cause: The library appears to handle angle wrapping incorrectly. When given 175° → 161°, it likely converts one angle to a negative representation (e.g., 175° → -185°), then calculates the naive difference without choosing the shortest rotational path.

Expected behavior: send_angles() should automatically calculate and execute the shortest angular path between current and target positions, just like standard robotic motion controllers do.

Request: Please fix send_angles() to use shortest-path angle interpolation, or clearly document this limitation so users can protect their hardware.

# Starting position (radians): [-0.1963, 1.3422, 0.7470, 1.2086, 2.9758, 3.0603]
# Target position (radians):   [0.2714, 1.1795, 0.7500, 1.7303, 2.8716, 2.8194]

start_angles = [-11.25, 76.90, 42.80, 69.24, 170.49, 175.33]  # degrees
target_angles = [15.55, 67.58, 42.97, 99.14, 164.52, 161.53]  # degrees

mc.send_angles(target_angles, speed=50)

Edit: Easy fix with gorilla glue

Why does the robot wobble so much?