fix fcl_collision_detection tests

This fixes #108 #109 #110 also stops segfault in test. Mostly a bunch of calls to robotstate.update(), but also fixed bad pose being passed in TestChangingShapeSize and missing link in gripper that was in collision.