[Bug]: initial_guesses init

[bestrobotplans]

What happened?

In xs_robot/arm.py line 217 to 219 the code says:

self.initial_guesses = [[0.0] self.group_info.num_joints] 3 self.initial_guesses[1][0] = np.deg2rad(-120) self.initial_guesses[2][0] = np.deg2rad(120)

The line self.initial_guesses[1][0] = np.deg2rad(-120) has no effect.

See code here:

import numpy as np

initial_guesses = [[0.0] 5] 3 initial_guesses[1][0] = np.deg2rad(-120) initial_guesses[2][0] = np.deg2rad(120)

print(initial_guesses)

This prints:

[[2.0943951023931953, 0.0, 0.0, 0.0, 0.0], [2.0943951023931953, 0.0, 0.0, 0.0, 0.0], [2.0943951023931953, 0.0, 0.0, 0.0, 0.0]]

Notice 2.09 is repeated three times, I suspect this was not intended.

A better way is:

import numpy as np

initial_guesses = [[0.0] * 5 for i in range(3)] initial_guesses[1][0] = np.deg2rad(-120) initial_guesses[2][0] = np.deg2rad(120)

print(initial_guesses)

This now prints

[[0.0, 0.0, 0.0, 0.0, 0.0], [-2.0943951023931953, 0.0, 0.0, 0.0, 0.0], [2.0943951023931953, 0.0, 0.0, 0.0, 0.0]]

Now we actually use 3 different angles. Anyway after this fix the inverse kinematics solver converges much more consistently since it's not using the same initialization 3 times in a row.

Robot Model

rx200

Operating System

Ubuntu 22.04

ROS Distro

ROS 2 Humble

Steps To Reproduce

No response

Relevant log output

No response

Additional Info

No response

[lukeschmitt-tr]

Fixed for Humble in 1181762698f2cf53b6bbad24d688447f0aa14dcd. Thanks for bringing this to our attention!