class MyGLViewer(GLWidgetPlugin): def __init__(self, world): GLWidgetPlugin.__init__(self) self.world = world self.robot = world.robot(0) self.subrobots = [] for i in range(6): self.subrobots.append( SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6)))) (res, val) = editors.run( editors.SelectionEditor("subrobot", self.subrobots[0]._links, description="sub robot links", world=world, robot=self.robot)) if res: print("Return value", val) self.subrobots[0]._links = val self.startConfig = self.robot.getConfig() self.robotWidget = RobotPoser(world.robot(0)) self.robotWidget.setActiveDofs(self.subrobots[0]._links) self.addWidget(self.robotWidget) def keyboardfunc(self, c, x, y): #Put your keyboard handler here #the current example prints the config when [space] is pressed if c == ' ': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) print("Config:", subconfig) self.subrobots[0].setConfig(subconfig) elif c == 'r': self.subrobots[0].randomizeConfig() self.robotWidget.set(self.robot.getConfig()) elif c == 'p': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) self.subrobots[0].setConfig(self.subrobots[0].fromfull( self.startConfig)) settings = { 'type': "sbl", 'perturbationRadius': 0.5, 'bidirectional': 1, 'shortcut': 1, 'restart': 1, 'restartTermCond': "{foundSolution:1,maxIters:1000}" } plan = robotplanning.planToConfig(self.world, self.subrobots[0], subconfig, movingSubset='all', **settings) plan.planMore(1000) print(plan.getPath()) else: GLWidgetPlugin.keyboardfunc(self, c, x, y)
class MyGLViewer(GLWidgetPlugin): def __init__(self, world): GLWidgetPlugin.__init__(self) self.world = world self.robot = world.robot(0) self.subrobots = [] for i in range(6): self.subrobots.append( SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6)))) (res, val) = editors.run( editors.SelectionEditor("subrobot", self.subrobots[0]._links, description="sub robot links", world=world, robot=self.robot)) if res: print("Return value", val) self.subrobots[0]._links = val self.startConfig = self.robot.getConfig() self.robotWidget = RobotPoser(world.robot(0)) self.robotWidget.setActiveDofs(self.subrobots[0]._links) self.addWidget(self.robotWidget) def keyboardfunc(self, c, x, y): #Put your keyboard handler here #the current example prints the config when [space] is pressed if c == 'h' or c == '?': print("Controls:") print("- [space]: print the widget's sub-robot configuration") print("- r: randomize the sub-robot configuration") print("- p: plan to the widget's sub-robot configuration") print("- i: test the IK functions") elif c == ' ': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) print("Config:", subconfig) self.subrobots[0].setConfig(subconfig) elif c == 'r': self.subrobots[0].randomizeConfig() self.robotWidget.set(self.robot.getConfig()) elif c == 'p': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) self.subrobots[0].setConfig(self.subrobots[0].fromfull( self.startConfig)) settings = { 'type': "sbl", 'perturbationRadius': 0.5, 'bidirectional': 1, 'shortcut': 1, 'restart': 1, 'restartTermCond': "{foundSolution:1,maxIters:1000}" } plan = robotplanning.planToConfig(self.world, self.subrobots[0], subconfig, movingSubset='all', **settings) plan.planMore(1000) print(plan.getPath()) elif c == 'i': link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1) print("IK solve for ee to go 10cm upward...") obj = ik.objective(link, local=[0, 0, 0], world=vectorops.add( link.getWorldPosition([0, 0, 0]), [0, 0, 0.1])) solver = ik.solver(obj) res = solver.solve() print(" result", res) print(" residual", solver.getResidual()) print(self.robotWidget.set(self.robot.getConfig())) else: GLWidgetPlugin.keyboardfunc(self, c, x, y)
class KeyboardAutoTrackTaskGenerator(TaskGenerator): """Allows the user to interact with the model by right clicking using the mouse and pressing space bar to send the milestone. Also demonstrates how to write a Python plugin task with an OpenGL widget. """ def __init__(self): self.plugin = None self.q = None def callback(self, data): self.marker = data self.marker_pos[0] = self.marker.pose.position.x self.marker_pos[1] = self.marker.pose.position.y self.marker_pos[2] = self.marker.pose.position.z #=== transform to world coordinate marker = se3.apply(kinect_frame_to_pedestal, tuple(self.marker_pos)) self.marker_pos_tf = list(marker) self.marker_pos_tf[2] = self.marker_pos_tf[2] + self.base_height # print "marker_pos_tf = ", self.marker_pos_tf def name(self): return "Keyboard" def init(self,world): self.world = world self.robotPoser = RobotPoser(world.robot(0)) self.plugin = MyWidgetPlugin(self) self.plugin.addWidget(self.robotPoser) self.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'] base_link_names = ['base_x','base_y','base_yaw'] self.left_arm_link_indices = [self.robot.link(l).index for l in left_arm_link_names] self.right_arm_link_indices = [self.robot.link(l).index for l in right_arm_link_names] self.base_link_indices = [self.robot.link(l).index for l in base_link_names] if any(v < 0 for v in self.left_arm_link_indices+self.right_arm_link_indices): raise ValueError("Robot in world does not have Baxter's torso?") # == subscribe marker position self.marker = Marker() self.base_height = 1.1 self.marker_pos = [0.0, 0.0, 0.0] self.marker_pos_tf = [0.0, 0.0, 0.0] rospy.init_node('marker_listener', anonymous=True) rospy.Subscriber("/Marker_glove", Marker, self.callback, None) #== auto tracking parameter self.tracking_speed_ratio = 1.0 self.tracking_vel = [0.0, 0.0, 0.0] self.left_ee_t = [0.0, 0.0, 0.0] self.right_ee_t = [0.0, 0.0, 0.0] self.reactive_vel_left_ee = [0.0, 0.0, 0.0] self.reactive_vel_right_ee = [0.0, 0.0, 0.0] return True def start(self): self._status = 'ok' self.robotPoser.set(self.world.robot(0).getConfig()) self.serviceThread = ServiceThread() self.serviceThread.start() return True def stop(self): if self.serviceThread: self.serviceThread.kill() print "Waiting for thread join..." self.serviceThread.join() print "Done" self.serviceThread = None def getEE(self): R1,t1 = self.serviceThread.eeGetter_left.get() R2,t2 = self.serviceThread.eeGetter_right.get() self.left_ee_t = list(t1) self.right_ee_t = list(t2) print "left hand EE = ", self.left_ee_t print "right hand EE = ", self.right_ee_t def cal_reactive_vel(self, pos_hand, pos_target, vel_ratio): reactive_vel = [0.0, 0.0, 0.0] pos_diff = vectorops.sub(tuple(pos_target), tuple(pos_hand)) pos_diff_norm = vectorops.norm(pos_diff) if pos_diff_norm >= 0.02: vel = vectorops.mul(pos_diff, vel_ratio) reactive_vel = list(vel) return reactive_vel def get(self): res = self.do_logic() return res def do_logic(self): rospy.Subscriber("/Marker_glove", Marker, self.callback, None) ''' retract arm to initial position ''' if self.plugin.initialPose: self.plugin.initialPose = False self.q = self.robotPoser.get() if self.plugin.selectedLimb == "both": for i in range(len(self.left_arm_link_indices)): idx = self.left_arm_link_indices[i] self.q[idx] = q_init_left[i] for i in range(len(self.right_arm_link_indices)): idx = self.right_arm_link_indices[i] self.q[idx] = q_init_right[i] print "set both arms to initial pose" if self.plugin.selectedLimb == "left": for i in range(len(self.left_arm_link_indices)): idx = self.left_arm_link_indices[i] self.q[idx] = q_init_left[i] print "set left arms to initial pose" if self.plugin.selectedLimb == "right": for i in range(len(self.right_arm_link_indices)): idx = self.right_arm_link_indices[i] self.q[idx] = q_init_right[i] print "set right arms to initial pose" self.robotPoser.set(self.q) if self.plugin.tuckArmPose: self.plugin.tuckArmPose = False self.q = self.robotPoser.get() if self.plugin.selectedLimb == "both": for i in range(len(self.left_arm_link_indices)): idx = self.left_arm_link_indices[i] self.q[idx] = q_tuckarm_left[i] for i in range(len(self.right_arm_link_indices)): idx = self.right_arm_link_indices[i] self.q[idx] = q_tuckarm_right[i] print "Tuck both arms" if self.plugin.selectedLimb == "left": for i in range(len(self.left_arm_link_indices)): idx = self.left_arm_link_indices[i] self.q[idx] = q_tuckarm_left[i] print "Tuck left arm" if self.plugin.selectedLimb == "right": for i in range(len(self.right_arm_link_indices)): idx = self.right_arm_link_indices[i] self.q[idx] = q_tuckarm_right[i] print "Tuck right arm" self.robotPoser.set(self.q) ''' Control - Robot arm and mobile base''' if self.plugin.sendMilestone: self.plugin.sendMilestone = False self.q = self.robotPoser.get() qcmd = self.world.robot(0).getConfig() baseMoved = any(qcmd[i] != self.q[i] for i in self.base_link_indices) leftArmMoved = any(qcmd[i] != self.q[i] for i in self.left_arm_link_indices) rightArmMoved = any(qcmd[i] != self.q[i] for i in self.right_arm_link_indices) moved = [] if baseMoved: moved.append('base') if leftArmMoved: moved.append('left') if rightArmMoved: moved.append('right') targets = [] if baseMoved: targets.append([self.q[i] for i in self.base_link_indices]) if leftArmMoved: targets.append([self.q[i] for i in self.left_arm_link_indices]) if rightArmMoved: targets.append([self.q[i] for i in self.right_arm_link_indices]) print "Sending milestone",self.q,"parts",",".join(moved) #CONSTRUCT THE TASK HERE msg = {} msg['type'] = 'JointPose' msg['parts'] = moved msg['positions'] = targets msg['speed'] = 1 msg['safe'] = 0 return msg ''' Control - gripper ''' if self.plugin.gripperControl == True: self.plugin.gripperControl = False if self.plugin.gripperState == "open": p = [1.0, 1.0, 1.0, 1.0] if self.plugin.selectedLimb == "both": p = p + p elif self.plugin.gripperState == "close": p = [0.3, 0.3, 0.3, 1.0] if self.plugin.selectedLimb == "both": p = p + p msg = {} msg['type'] = 'Gripper' msg['limb'] = self.plugin.selectedLimb msg['position'] = p msg['force'] = 0.2 msg['speed'] = 0.5 return msg ''' Auto tracking ''' if self.plugin.trackPosition == True: self.plugin.trackPosition_lastState = True self.plugin.trackPosition = False print "move to target: ", self.marker_pos_tf self.getEE() if self.plugin.selectedLimb == "both": self.reactive_vel_left_ee = self.cal_reactive_vel(self.left_ee_t, self.marker_pos_tf, self.tracking_speed_ratio) self.reactive_vel_right_ee = self.cal_reactive_vel(self.right_ee_t, self.marker_pos_tf, self.tracking_speed_ratio) print "Use both left or right arm to track" linear_vel = self.reactive_vel_left_ee + self.reactive_vel_right_ee angular_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0] if self.plugin.selectedLimb == "left": self.reactive_vel_left_ee = self.cal_reactive_vel(self.left_ee_t, self.marker_pos_tf, self.tracking_speed_ratio) print "Move left arm to target at velocity: ", self.reactive_vel_left_ee linear_vel = self.reactive_vel_left_ee angular_vel = [0.0, 0.0, 0.0] if self.plugin.selectedLimb == "right": self.reactive_vel_right_ee = self.cal_reactive_vel(self.right_ee_t, self.marker_pos_tf, self.tracking_speed_ratio) print "Move right arm to target" linear_vel = self.reactive_vel_right_ee angular_vel = [0.0, 0.0, 0.0] msg = {} msg['type'] = 'CartesianVelocity' msg['limb'] = self.plugin.selectedLimb msg['linear'] = linear_vel msg['angular'] = angular_vel return msg if self.plugin.trackPosition_lastState == True: self.plugin.trackPosition_lastState = False print "last command is cartesian velocity ... " if self.plugin.selectedLimb == "both": linear_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0] angular_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0] if self.plugin.selectedLimb == "left": linear_vel = [0.0, 0.0, 0.0] angular_vel = [0.0, 0.0, 0.0] if self.plugin.selectedLimb == "right": linear_vel = [0.0, 0.0, 0.0] angular_vel = [0.0, 0.0, 0.0] msg = {} msg['type'] = 'CartesianVelocity' msg['limb'] = self.plugin.selectedLimb msg['linear'] = linear_vel msg['angular'] = angular_vel return msg return None def glPlugin(self): return self.plugin
class CustomGLViewer(GLRealtimeProgram): def __init__(self, simworld=None, planworld=None, controller=None, sim=None, helper=None): if simworld == None and planworld == None: # Dummy GLViewer print "simworld and planworld are None" return GLRealtimeProgram.__init__(self, "My GL program") self.simworld = simworld self.planworld = planworld self.sim = sim self.widgetMaster = WidgetSet() self.widgetButton = 2 # right-clicks self.draggingWidget = False self.poseWidget = PointPoser() self.widgetMaster.add(self.poseWidget) self.robotWidget = RobotPoser(self.simworld.robot(0)) self.widgetMaster.add(self.robotWidget) self.simulate = True if self.sim == None: self.simulate = False self.controller = controller self.command_queue = Queue() if helper != None: print "helper is not none" helper.run() self.points = None self.extraControllers = {} def idle(self): if self.extraControllers != {}: remove_indices = [] for controller in self.extraControllers: self.dt = 0.01 controller.sim.simulate(self.dt) glutPostRedisplay() if controller.remainingTime() <= 0: remove_indices.append(controller) for controller in remove_indices: del self.extraControllers[controller] print "controller removed" if self.simulate: self.dt = 0.01 self.sim.simulate(self.dt) glutPostRedisplay() self.widgetMaster.idle() if self.widgetMaster.wantsRedraw(): print "widgetMaster wants redraw" self.refresh() def printStuff(self): print "shelf xform:", knowledge.shelf_xform print self.simworld.terrain(0).geometry() # print "terrain xform:", self.simworld.terrain(0).geometry().getTransform(), "\n\n" def updatePoints(self, points): self.points = points def addController(self, new_controller, rgb): self.extraControllers[new_controller] = rgb def glShowPointCloud(self, pc, downsample_rate=5, pt_size=None): # print glGetFloatv(GL_CURRENT_COLOR) pc = np.array(pc) # print 'Rendering %d points'%pc.shape[0] try: d = pc.shape[1] assert d == 3 or d == 4, "Unrecognized point cloud shape: " + str(pc.shape) pc = pc[::downsample_rate, :] pc = pc.tolist() glDisable(GL_LIGHTING) glColor3f(0, 1, 0) if pt_size is not None: glPointSize(pt_size) glBegin(GL_POINTS) for p in pc: if d == 4: r, g, b = pcl_float_to_rgb(p[3]) r /= 255.0 g /= 255.0 b /= 255.0 glColor3f(r, g, b) glVertex3f(p[0], p[1], p[2]) glEnd() glEnable(GL_LIGHTING) glColor3f(1, 0, 0) except: # print 'Insufficient data' pass def updateBox(self, index, vertices): BOX_COORDS[index] = vertices def display(self): global BOX_COORDS glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [1, 0, 0, 0.5]) # only 1 robot in this case, but still use for-loop for generality for i in xrange(self.simworld.numRobots()): r = self.controller.robotModel q = self.controller.getCommandedConfig() if self.widgetMaster.wantsRedraw(): # restart sim - start it with the new robot config # if simulating if self.sim != None: # put more checks on mouse function here # might accidentally reset if you mouse over q = self.robotWidget.get() # print 'self.sim is not None' self.controller.robotModel.setConfig(q) self.simworld.robot(0).setConfig(q) self.controller.setLinear(q, 0.01) else: self.robotWidget.set(q) # q = self.controller.getSensedConfig() r.setConfig(q) r.drawGL(False) glDisable(GL_BLEND) for controller in self.extraControllers: glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, self.extraControllers[controller]) r = controller.robotModel q = controller.getCommandedConfig() r.setConfig(q) r.drawGL(False) glDisable(GL_BLEND) for i in xrange(self.simworld.numTerrains()): self.simworld.terrain(i).drawGL() if self.points != None: self.glShowPointCloud(self.points) for i in BOX_COORDS: self.draw_wire_box(*BOX_COORDS[i]) self.widgetMaster.drawGL(self.viewport()) def draw_wire_box(self, bmin, bmax): """Helper: draws a wireframe box""" glBegin(GL_LINE_LOOP) glVertex3f(bmin[0], bmin[1], bmin[2]) glVertex3f(bmin[0], bmin[1], bmax[2]) glVertex3f(bmin[0], bmax[1], bmax[2]) glVertex3f(bmin[0], bmax[1], bmin[2]) glEnd() glBegin(GL_LINE_LOOP) glVertex3f(bmax[0], bmin[1], bmin[2]) glVertex3f(bmax[0], bmin[1], bmax[2]) glVertex3f(bmax[0], bmax[1], bmax[2]) glVertex3f(bmax[0], bmax[1], bmin[2]) glEnd() glBegin(GL_LINES) glVertex3f(bmin[0], bmin[1], bmin[2]) glVertex3f(bmax[0], bmin[1], bmin[2]) glVertex3f(bmin[0], bmin[1], bmax[2]) glVertex3f(bmax[0], bmin[1], bmax[2]) glVertex3f(bmin[0], bmax[1], bmax[2]) glVertex3f(bmax[0], bmax[1], bmax[2]) glVertex3f(bmin[0], bmax[1], bmin[2]) glVertex3f(bmax[0], bmax[1], bmin[2]) glEnd() def keyboardfunc(self, c, x, y): # print c,"pressed" # print 'int value =', ord(c) self.widgetMaster.keypress(c) self.refresh() if c == "`": self.simulate = not self.simulate print "Simulating:", self.simulate else: self.command_queue.put(c) # print int(c) if c == chr(27): # c == esc # self.picking_thread.join() exit(0) glutPostRedisplay() def mousefunc(self, button, state, x, y): # print "mouse",button,state,x,y if button == self.widgetButton: if state == 0: # print 'state was zero' if self.widgetMaster.beginDrag(x, self.height - y, self.viewport()): # print 'beginning drag' self.draggingWidget = True else: # print 'state of button not zero' if self.draggingWidget: # print 'dragging widget true - ending drag' self.widgetMaster.endDrag() self.draggingWidget = False if self.widgetMaster.wantsRedraw(): self.refresh() self.lastx, self.lasty = x, y return GLRealtimeProgram.mousefunc(self, button, state, x, y) def motionfunc(self, x, y, dx, dy): if self.draggingWidget: self.widgetMaster.drag(dx, -dy, self.viewport()) if self.widgetMaster.wantsRedraw(): self.refresh() else: res = self.widgetMaster.hover(x, self.height - y, self.viewport()) if self.widgetMaster.wantsRedraw(): self.refresh() GLRealtimeProgram.motionfunc(self, x, y, dx, dy) def specialfunc(self, c, x, y): # Put your keyboard special character handler here # print c,"pressed" self.widgetMaster.keypress(c) self.refresh()
class MousePoserTaskGenerator(TaskGenerator): """Allows the user to interact with the model by right clicking using the mouse and pressing space bar to send the milestone. Also demonstrates how to write a Python plugin task with an OpenGL widget. """ def __init__(self): self.plugin = None def name(self): return "MousePoser" def init(self, world): self.world = world self.robotPoser = RobotPoser(world.robot(0)) self.plugin = MyWidgetPlugin(self) self.plugin.addWidget(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' ] base_link_names = ['base_x', 'base_y', 'base_yaw'] self.left_arm_link_indices = [ robot.link(l).index for l in left_arm_link_names ] self.right_arm_link_indices = [ robot.link(l).index for l in right_arm_link_names ] self.base_link_indices = [robot.link(l).index for l in base_link_names] if any(v < 0 for v in self.left_arm_link_indices + self.right_arm_link_indices): raise ValueError("Robot in world does not have Baxter's torso?") return True def start(self): self._status = 'ok' self.robotPoser.set(self.world.robot(0).getConfig()) return True def stop(self): pass def get(self): if self.plugin.initialPose: self.plugin.initialPose = False q = self.robotPoser.get() for i in range(len(self.left_arm_link_indices)): idx = self.left_arm_link_indices[i] q[idx] = q_init_left[i] for i in range(len(self.right_arm_link_indices)): idx = self.right_arm_link_indices[i] q[idx] = q_init_right[i] self.robotPoser.set(q) print "set up to initial pose" if self.plugin.sendMilestone: self.plugin.sendMilestone = False q = self.robotPoser.get() qcmd = self.world.robot(0).getConfig() baseMoved = any(qcmd[i] != q[i] for i in self.base_link_indices) leftArmMoved = any(qcmd[i] != q[i] for i in self.left_arm_link_indices) rightArmMoved = any(qcmd[i] != q[i] for i in self.right_arm_link_indices) moved = [] if baseMoved: moved.append('base') if leftArmMoved: moved.append('left') if rightArmMoved: moved.append('right') targets = [] if baseMoved: targets.append([q[i] for i in self.base_link_indices]) if leftArmMoved: targets.append([q[i] for i in self.left_arm_link_indices]) if rightArmMoved: targets.append([q[i] for i in self.right_arm_link_indices]) print "Sending milestone", q, ", moving parts:", ",".join(moved) #CONSTRUCT THE TASK HERE msg = {} msg['type'] = 'JointPose' msg['parts'] = moved msg['positions'] = targets msg['speed'] = 1 #TEST: only allow safe configurations (self collision checking) msg['safe'] = 1 return msg return None def glPlugin(self): return self.plugin