GPMP2 fails to solve custom WAM problems

[riordan45]

Hey,

I tried running the WAM example in python with the following parameters for some benchmarks:

k=0
base_pose = Pose3(Rot3(np.identity(3)), np.asarray([0.0, 0, 0.346]))

arm = generateArm("WAMArm", base_pose)
n_states = 14
states = [list() for _ in range(n_states)]
states[0] = [0.04295548, 0.95584516, -0.96807816, 0.97116162, 0.9778903, 0.65763463, -2.2554430167948967]
states[1] = [0.16082985, 1.11182696, -0.92183762, 0.3794195, 1.23, 0.47523424, -1.8449310467948965]
states[2] = [0.09952304, 1.09863569, -0.88496722, 0.38292964, 1.23, 0.41536308, -1.9511107067948965]
states[3] = [0.10052545, 1.06389854, -1.09858978, 0.48121717, 0.76275836, 1.38780074, -0.7735178867948965]
states[4] = [-0.45014853, 1.59318377, 0.4554682, 0.6065858, -0.38585459, 0.53452102, -1.5629486467948965]
states[5] = [-0.34010213, 1.6881081, 0.98402557, 0.51367941, -2.39890266, -0.58455747, -0.5586590567948966]
states[6] = [-0.22101804, 1.66367157, 1.09508804, 0.56299024, -2.89040372, -0.59143963, -0.25602298679489666]
states[7] = [-0.67729868, 1.64146044, 1.12373694, 0.91912803, -3.17152523, -0.89928808, -0.1827793267948965]
states[8] = [-1.36399638, 1.91753362, 1.32779556, 2.07333031, 0.8333524, 0.08067977, 2.3950357303846896]
states[9] = [-0.87877812, 1.64645585, 1.34329545, 1.62880413, 0.84055928, -0.0062247, 2.42199736038469]
states[10] = [1.38153424, 1.78324208, 0.18278696, 0.43210283, -1.62168076, 1.01491547, 0.6125925832051036]
states[11] = [1.60174351, 1.74358664, 0.12658995, 0.20548551, -1.48280243, 0.92108951, 0.8164594632051037]
states[12] = [1.9937845, 1.52197993, 0.44538624, 1.10392873, -1.28498349, 1.32703383, 0.9266569532051037]
states[13] = [-1.29228216, -1.90587936, 1.65480383, 0.20854488, 0.6896924, 0.52053023, 2.2240916803846895]

indices = [list() for _ in range(20)]
indices[0] = [0, 7]
indices[1] = [7, 1]
indices[2] = [1, 8]
indices[3] = [8, 10]
indices[4] = [10, 9]
indices[5] = [9, 2]
indices[6] = [2, 5]
indices[7] = [5, 13]
indices[8] = [13, 6]
indices[9] = [6, 2]
indices[10] = [2, 12]
indices[11] = [12, 9]
indices[12] = [9, 3]
indices[13] = [3, 4]
indices[14] = [4, 11]
indices[15] = [11, 0]
indices[16] = [0, 6]
indices[17] = [6, 1]
indices[18] = [1, 10]
indices[19] = [10, 5]


q1, q2 = indices[k]
start_conf = np.asarray(states[q1]).reshape(-1, 1)
end_conf = np.asarray(states[q2]).reshape(-1, 1)

And the rest of the code is the same. You can range k from 0 to 19. I'm not sure why but GPMP2 seems to not solve any of the problems and if I change the parameters I can get at most 3/20 problems solved. I know how the hyperparameters affect the algorithm but I find the behavior strange as I was expecting close to 100% with the slight elevation added.

The indices list shows the pairs to be taken from the states to define the start and end configuration.
Here's how the first pair should look, the top config is the start and bottom is the end.

When running with the parameters in the paper the trajectory just goes straight through the shelf.

[mattking-smith]

@riordan45 Have you evaluated the error associate with the obstacle factor in the graph when you give the success and failure cases? Also it is important to remember that this is unconstrained optimization approach, so there are no guarantees that this optimization will find a collision-free trajectory... although it should...

[riordan45]

@mattking-smith To report success and failure cases I took the joint angles GPMP2 gives and tried them in my simulation in PyBullet which is a 1 to 1 reconstruction of your environment. The error the script reports have magnitude between 1e+3 to 1e+4 although GPMP2 reports convergence.

Think you could try running the code I gave here? I have a feeling that the SDF does not take into account the robot position correctly for whatever reason but I'm not sure.

[mattking-smith]

@mattking-smith To report success and failure cases I took the joint angles GPMP2 gives and tried them in my simulation in PyBullet which is a 1 to 1 reconstruction of your environment. The error the script reports have magnitude between 1e+3 to 1e+4 although GPMP2 reports convergence.

Think you could try running the code I gave here? I have a feeling that the SDF does not take into account the robot position correctly for whatever reason but I'm not sure.

Did you know you can specify to look at the error of a particular factor in the graph given some optimized result? I would start by looking at the trajectories which are see as a success and the errors associated with the obstacle factor and the trajectories which fail.