Slight change local position in ContactContraintGeneration
Slight change local position in ContactContraintGeneration

Need to go through the element pose instead of the global pose when
transforming the local collision Location to the local physics Location.
This is because of the difference between the collision representation
using the temperary but current element pose needed for collision
detection, while the phyics getPose uses the old, but final element pose from
the last iteration. This just short circuits these and goes through the
local poses only. This doesn't work if the collision representation and
physics representation are in different elements, but we already assume
this in other places.

simquest/opensurgsim,simquest/opensurgsim,simquest/opensurgsim,simquest/opensurgsim