Fix case where parent and child belong to the CFG
Fix case where parent and child belong to the CFG

We weren't checking whether the parent belonged to the collision filter group
until too late. The result was that if both parent and child had collision
geometry, the welded link would never belong to the collision filter group -
even if both parent and child did. By checking wether the parent belongs to the
group first, we can avoid this problem.

sheim/drake,crogers3/drake,tkoolen/drake,crogers3/drake,manuelli/drake,scottviteri/drake,manuelli/drake,scottviteri/drake,tkoolen/drake,psiorx/drake,manuelli/drake,hanssusilo/drake,billhoffman/drake,crogers3/drake,crogers3/drake,sheim/drake,hanssusilo/drake,astrorog/drake,billhoffman/drake,psiorx/drake,sheim/drake,edowson/drake,hanssusilo/drake,thientu/drake,sheim/drake,astrorog/drake,thientu/drake,psiorx/drake,tkoolen/drake,hanssusilo/drake,crogers3/drake,sheim/drake,scottviteri/drake,scottviteri/drake,astrorog/drake,sheim/drake,scottviteri/drake,billhoffman/drake,psiorx/drake,thientu/drake,scottviteri/drake,astrorog/drake,manuelli/drake,thientu/drake,crogers3/drake,thientu/drake,astrorog/drake,edowson/drake,edowson/drake,astrorog/drake,thientu/drake,billhoffman/drake,edowson/drake,edowson/drake,tkoolen/drake,billhoffman/drake,hanssusilo/drake,hanssusilo/drake,tkoolen/drake,astrorog/drake,billhoffman/drake,crogers3/drake,sheim/drake,billhoffman/drake,hanssusilo/drake,manuelli/drake,psiorx/drake,thientu/drake,sheim/drake,tkoolen/drake,edowson/drake,edowson/drake,psiorx/drake,manuelli/drake,manuelli/drake,psiorx/drake,billhoffman/drake,psiorx/drake,edowson/drake,hanssusilo/drake,tkoolen/drake,crogers3/drake,scottviteri/drake