def publishPose(q, problem, t=0.0):
    problem.getScene().Update(q, t)
    problem.getScene().getSolver().publishFrames()
def publishPose(q, problem, t=0.0):
    problem.getScene().Update(q, t)
    problem.getScene().getSolver().publishFrames()