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 keyboardfunc(self,c,x,y): #Put your keyboard handler here #the current example toggles simulation / movie mode if c == 'h': print '[space]: send the current posed milestone' print 'q: clean quit' elif c == ' ': q = self.robotPoser.get() robot = motion.robot send_debounced_motion_command(motion,'left',robot.left_limb.configFromKlampt(q)) send_debounced_motion_command(motion,'right',robot.right_limb.configFromKlampt(q)) #robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q)) #robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q)) qlg = robot.left_gripper.configFromKlampt(q) qrg = robot.right_gripper.configFromKlampt(q) if qlg: robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg)) if qrg: robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg)) #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices]) #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices]) elif c == 'q': motion.robot.shutdown() exit(0) else: GLWidgetProgram.keyboardfunc(self,c,x,y) self.refresh()
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]
def display(self): #Put your display handler here #the current example draws the sensed robot in grey and the #commanded configurations in transparent green #this line with draw the world robot = motion.robot q = robot.getKlamptSensedPosition() self.world.robot(0).setConfig(q) self.world.robot(0).drawGL() GLWidgetProgram.display(self) #draw commanded configuration glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA) glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5]) q = robot.getKlamptCommandedPosition() self.world.robot(0).setConfig(q) self.world.robot(0).drawGL(False) glDisable(GL_BLEND)
def idle(self): GLWidgetProgram.idle(self) t = time.time()-self.t0 if motion.robot.left_mq.moving(): self.lastLeftMove = t if motion.robot.right_mq.moving(): self.lastRightMove = t if t < self.lastLeftMove + 3: #print "Saving left",t qsensed = motion.robot.left_limb.sensedPosition() qcmd = motion.robot.left_limb.commandedPosition() self.outputFileLeft.write(str(t)+",") self.outputFileLeft.write(','.join(str(v) for v in qsensed+qcmd)) self.outputFileLeft.write('\n') if t < self.lastRightMove + 3: #print "Saving right",t qsensed = motion.robot.right_limb.sensedPosition() qcmd = motion.robot.right_limb.commandedPosition() self.outputFileRight.write(str(t)+",") self.outputFileRight.write(','.join(str(v) for v in qsensed+qcmd)) self.outputFileRight.write('\n')
def idle(self): GLWidgetProgram.idle(self)