GPMP2 fails to solve custom WAM problems

Hey,

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

```python
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.