Adding PlanToEndEffectorPose to ompl