URDF Issues

[mouhyemen]

I followed the kuka_lw4.urdf file provided in the repo. After running the ./trajectory.py script (with optional world.urdf model), no feasible solutions were found at the end.
Also, I am interested on my own URDF file for our robotic model and I followed the skeleton format of kuka_lwr4.urdf provided, I get the following error:

mouhyemen@khan-msi:~/desktop/research/FloBaRoID$ ./trajectory.py --filename jim.trajectory.npz --config configs/testing.yaml --model model/krang_from_kuka_v2.urdf --world model/world_kuka.urdf 
loaded model model/krang_from_kuka_v2.urdf
# DOFs: 7
Joints: ['lwr_0_joint', 'lwr_1_joint', 'lwr_2_joint', 'lwr_3_joint', 'lwr_4_joint', 'lwr_5_joint', 'lwr_6_joint']
# regressor outputs: 7
# links: 8 (+ 0 fake)
{0: 'krang_shoulder', 1: 'lwr_1_link', 2: 'lwr_2_link', 3: 'lwr_3_link', 4: 'lwr_4_link', 5: 'lwr_5_link', 6: 'lwr_6_link', 7: 'lwr_7_link'}
# params: 80 (94 will be identified)
loaded random structural regressor from model/krang_from_kuka_v2.urdf.regressor.npz
broken
loaded model model/krang_from_kuka_v2.urdf
# DOFs: 7
Joints: ['lwr_0_joint', 'lwr_1_joint', 'lwr_2_joint', 'lwr_3_joint', 'lwr_4_joint', 'lwr_5_joint', 'lwr_6_joint']
# regressor outputs: 7
# links: 8 (+ 0 fake)
{0: 'krang_shoulder', 1: 'lwr_1_link', 2: 'lwr_2_link', 3: 'lwr_3_link', 4: 'lwr_4_link', 5: 'lwr_5_link', 6: 'lwr_6_link', 7: 'lwr_7_link'}
# params: 80 (94 will be identified)
loaded random structural regressor from model/krang_from_kuka_v2.urdf.regressor.npz
/home/mouhyemen/desktop/research/FloBaRoID/excitation/optimizer.py:200: RuntimeWarning: invalid value encountered in true_divide
  old_com = self.model.xStdModel[i*10+1:i*10+4] / self.model.xStdModel[i*10],
World links: ['ground_link']
Running global optimization with ALPSO
call #1/612
wf 0.5
a [[0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3]]
b [[0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3]]
q [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Traceback (most recent call last):
  File "./trajectory.py", line 94, in <module>
    main()
  File "./trajectory.py", line 71, in main
    trajectory = trajectoryOptimizer.optimizeTrajectory()
  File "/home/mouhyemen/desktop/research/FloBaRoID/excitation/trajectoryOptimizer.py", line 390, in optimizeTrajectory
    sol_vec = self.runOptimizer(opt_prob)
  File "/home/mouhyemen/desktop/research/FloBaRoID/excitation/optimizer.py", line 504, in runOptimizer
    opt(opt_prob, store_hst=False) #, xstart=initial)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyOpt_optimizer.py", line 146, in __call__
    return self.__solve__(opt_problem, *args, **kwargs)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/pyALPSO.py", line 442, in __solve__
    hos_file, seed, scale, nhs, objconfunc)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/alpso.py", line 253, in alpso
    [f[i], g[i, :]] = objfunc(xtmp)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/pyALPSO.py", line 293, in objconfunc
    [ff, gg, fail] = opt_problem.obj_fun(xn, *args, **kwargs)
  File "/home/mouhyemen/desktop/research/FloBaRoID/excitation/trajectoryOptimizer.py", line 257, in objectiveFunc
    if d < g[c_s + g_cnt]:
IndexError: list index out of range

Note: If I do NOT include the world.urdf model, the above error is generated even for kuka_lw4r.urdf.

[kjyv]

Which version of pyOpt did you install? You should install the one mentioned in the readme.

Also, it seems that you have a zero mass somewhere in your model:
RuntimeWarning: invalid value encountered in true_divide
old_com = self.model.xStdModel[i10+1:i10+4] / self.model.xStdModel[i*10],

[mouhyemen]

I installed the one in the readme. Simply cloned it and ran

sudo python setup.py install

The zero mass has been fixed. Not getting any feasible solutions by the way. Even for the kuka URDF provided.

[crush0441]

@mouhyemen @kjyv
I met the same problem with you and I know the cause of this problem.

You can avoid this problem by editing *.yaml config file.

The cause of this problem:

        # amount of collision checks to be done
        eff_links = self.model.num_links - len(self.config['ignoreLinksForCollision']) + len(self.world_links)

        self.num_constraints = self.num_postures * (eff_links * (eff_links-1) // 2)

        #get neighbors
        nb_pairs = []  # type: List[Tuple]
        for link in self.neighbors:
            if link in self.config['ignoreLinksForCollision']:
                continue
            if link not in self.model.linkNames:
                continue
            nb_real = set(self.neighbors[link]['links']).difference(
                self.config['ignoreLinksForCollision']).intersection(self.model.linkNames)
            for l in nb_real:
                if (link, l) not in nb_pairs and (l, link) not in nb_pairs:
                    nb_pairs.append((link, l))
        self.num_constraints -= self.num_postures * (len(nb_pairs) +        # neighbors
                                  len(self.config['ignoreLinkPairsForCollision']))  # custom combination

There is a bug when calculating the amount of collision checks to be done.
The item:

self.config['ignoreLinksForCollision']
self.config['ignoreLinkPairsForCollision']

When the same link name occurs in these two items, the error you describe will happen. This error can be solved by improving the calculation logic of collision amounts.

[kjyv]

About no feasible solutions found: I guess this has to do with newer versions of the libraries and tool stack producing different output than 2 years ago etc. Not sure how to easily find this at the moment but to go through the intermediate steps and look for wrong data. This is not a fire and forget software I guess but if you want to use some of the algorithms, you can use it as a starting point (I'm not actively working on it anymore).

Good find about the wrong calculation, for now it's best if you don't ignore links "twice" I guess :)