Can use trajopt with the constraint as the endPose. This requires a 1cm offset in the z direction because the tool frame doesn't exist in openrave. Also, joint command speeds are joint dependent now and have been tuned to acceptable values