def __init__(self,world):
GLWidgetProgram.__init__(self,world,"Manual poser")
self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition())
#motion.robot.left_limb.enableSelfCollisionAvoidance(False)
#motion.robot.right_limb.enableSelfCollisionAvoidance(False)
self.robotPoser = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
self.outputFileLeft = open("sag_calibration_test_left.csv","w")
self.outputFileRight = open("sag_calibration_test_right.csv","w")
#write headers
self.outputFileLeft.write("time,")
self.outputFileLeft.write(",".join("qsns"+str(i) for i in range(1,8)))
self.outputFileLeft.write(",")
self.outputFileLeft.write(",".join("qcmd"+str(i) for i in range(1,8)))
self.outputFileLeft.write("\n")
self.outputFileLeft.write("time,")
self.outputFileRight.write(",".join("qsns"+str(i) for i in range(1,8)))
self.outputFileRight.write(",")
self.outputFileRight.write(",".join("qcmd"+str(i) for i in range(1,8)))
self.outputFileRight.write("\n")
self.t0 = time.time()
self.lastLeftMove = -100
self.lastRightMove = -100
def __init__(self,world):
GLWidgetProgram.__init__(self,world,"Manual poser")
self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition())
#motion.robot.left_limb.enableSelfCollisionAvoidance(False)
#motion.robot.right_limb.enableSelfCollisionAvoidance(False)
self.robotPoser = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
