anthonysimeonov/baxter_mpnet_experiments

Tied to reproduce results

Closed this issue · 1 comments

Hello guys,

I am trying to run project with pretrained model and given test example. I did an installation and downloaded and unzipped tests samples and model. But I always get resul like this

GOAL IN COLLISION --- BREAKING step: i=9 j=92 fp: 0 tp: 0 388 START: [-0.07975602 0.32780212 0.03883335 -0.00221989 0.23852038 -0.001236 0.1863429 ] GOAL: [-0.07325188 -0.29086515 -0.17204982 -0.34340358 0.44761458 -0.14867507 0.7157227 ] GOAL IN COLLISION --- BREAKING

for each sample. And as final result a got

`
feasible paths:
0
nan
nan
env: trainEnv_4
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_5
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_6
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_7
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_0
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_1
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_2
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_3
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_8
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

env: trainEnv_9
accuracy: UNKNOWN data[tp_env] == 0 (data[fp_env] == 0); formula 100*data[fp_env]/data[tp_env]
Mean min and max time are UNKNOWN: data['et_env] has 0 length

0.0
0.0
`

Can you please give any advice ?

I've found the reason of the problem! It was in baxter_moveit configuration.

The collision was between gripper's fingers and gripper base. I've added

`
<disable_collisions link1="r_gripper_r_finger" link2="r_gripper_r_finger_tip" reason="Adjacent" />
<disable_collisions link1="l_gripper_r_finger" link2="l_gripper_r_finger_tip" reason="Adjacent" />

<disable_collisions link1="r_gripper_l_finger" link2="r_gripper_l_finger_tip" reason="Adjacent" />        
<disable_collisions link1="l_gripper_l_finger" link2="l_gripper_l_finger_tip" reason="Adjacent" />        
                                                                                                          
<disable_collisions link1="r_gripper_r_finger" link2="right_gripper_base" reason="Adjacent" />            
<disable_collisions link1="l_gripper_r_finger" link2="left_gripper_base" reason="Adjacent" />             
                                                                                                          
<disable_collisions link1="r_gripper_l_finger" link2="right_gripper_base" reason="Adjacent" />            
<disable_collisions link1="l_gripper_l_finger" link2="left_gripper_base" reason="Adjacent" />             
                                                                                                          
<disable_collisions link1="r_gripper_r_finger" link2="r_gripper_l_finger" reason="Adjacent" />            
<disable_collisions link1="l_gripper_r_finger" link2="l_gripper_l_finger" reason="Adjacent" />

inmoveit_robots/baxter/baxter_moveit_config/config/baxter_base.srdf.xacro` file and it works now!