def set_torque(joint, goal, t=3.0):
c.set_time(t)
time.sleep(0.1)
c.set_torque(goal)
time.sleep(t)
def set_position(joint, goal, t=3.0):
c.set_time(t)
time.sleep(0.1)
c.set_position(goal)
time.sleep(t)