def display(self): if self.state == None: return glEnable(GL_LIGHTING) self.statelock.acquire() for i,d in enumerate(self.state): Twidget = d['widgetTransform'] T = (so3.mul(Twidget[0],so3.inv(deviceToWidgetTransform[0])),Twidget[1]) width = 0.05*d['rotationScale'] length = 0.1*d['positionScale'] gldraw.xform_widget(T,length,width) self.statelock.release() if self.robot: #draw robot qcmd = self.qcmdGetter.get() if qcmd: self.robot.setConfig(qcmd) self.robot.drawGL(True) else: self.robot.drawGL(True) qdest = self.qdestGetter.get() if qdest: 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]) self.robot.setConfig(qdest) self.robot.drawGL(False) glDisable(GL_BLEND) glDisable(GL_LIGHTING)
def move_gripper_upright(self,limb,moveVector,collisionchecker = None): vertical = [0,0,0.1] if self.active_limb == 'left': gripperlink = self.left_gripper_link else: gripperlink = self.right_gripper_link qcur = self.robot.getConfig() vertical_in_gripper_frame = so3.apply(so3.inv(gripperlink.getTransform()[0]),vertical) centerpos = se3.mul(gripperlink.getTransform(),left_gripper_center_xform)[1] move_target = vectorops.add(centerpos,moveVector) movegoal = ik.objective(gripperlink,local=[left_gripper_center_xform[1],vectorops.add(left_gripper_center_xform[1],vertical_in_gripper_frame)],world=[move_target,vectorops.add(move_target,vertical)]) sortedSolutions = self.get_ik_solutions([movegoal],[self.active_limb],qcur,validity_checker=collisionchecker,maxResults=1) if len(sortedSolutions) == 0: print "No upright-movement config found" return False self.robot.setConfig(sortedSolutions[0][0]) return True
def emulate(self, sim): """Given a Simulator instance, emulates the sensor. Result is a CameraColorDetectorOutput structure.""" global objectColors xscale = math.tan(math.radians(self.fov * 0.5)) * self.w / 2 blobs = [] for i in range(sim.world.numRigidObjects()): o = sim.world.rigidObject(i) body = sim.body(o) Tb = body.getTransform() plocal = se3.apply(se3.inv(self.Tsensor), Tb[1]) x, y, z = plocal if z < self.dmin or z > self.dmax: continue c = objectColors[i % len(objectColors)] o.geometry().setCurrentTransform( so3.mul(so3.inv(self.Tsensor[0]), Tb[0]), [0] * 3) bmin, bmax = o.geometry().getBB() err = self.pixelError dscale = xscale / z xim = dscale * x + self.w / 2 yim = dscale * y + self.h / 2 xmin = int(xim - dscale * (bmax[0] - bmin[0]) * 0.5 + random.uniform(-err, err)) ymin = int(yim - dscale * (bmax[1] - bmin[1]) * 0.5 + random.uniform(-err, err)) xmax = int(xim + dscale * (bmax[0] - bmin[0]) * 0.5 + random.uniform(-err, err)) ymax = int(yim + dscale * (bmax[1] - bmin[1]) * 0.5 + random.uniform(-err, err)) if xmin < 0: xmin = 0 if ymin < 0: ymin = 0 if xmax >= self.w: xmax = self.w - 1 if ymax >= self.h: ymax = self.h - 1 if ymax <= ymin: continue if xmax <= xmin: continue #print "Actual x,y,z",x,y,z #print "blob color",c[0:3],"dimensions",xmin,xmax,"x",ymin,ymax blob = CameraBlob(c[0:3], 0.5 * (xmin + xmax), 0.5 * (ymin + ymax), xmax - xmin, ymax - ymin) blobs.append(blob) return CameraColorDetectorOutput(blobs)
def onMessage(self, msg): global deviceState, defaultDeviceState #print "Getting haptic message" #print msg if msg['type'] != 'MultiHapticState': print "Strange message type", msg['type'] return if len(deviceState) == 0: print "Adding", len( msg['devices']) - len(deviceState), "haptic devices" while len(deviceState) < len(msg['devices']): deviceState.append(defaultDeviceState.copy()) if len(msg['devices']) != len(deviceState): print "Incorrect number of devices in message:", len( msg['devices']) return #change overall state depending on button 2 if 'events' in msg: for e in msg['events']: #print e['type'] if e['type'] == 'b2_down': dstate = deviceState[e['device']] toggleMode(dstate) print 'Changing device', e['device'], 'to state', dstate[ 'mode'] for i in range(len(deviceState)): dmsg = msg['devices'][i] dstate = deviceState[i] if dmsg['button1'] == 1: #drag widget if button 1 is down oldButtonDown = dstate['buttonDown'] dstate['buttonDown'] = True #moving if dstate['mode'] == 'normal': if not oldButtonDown: dstate['moveStartDeviceTransform'] = (so3.from_moment( dmsg['rotationMoment']), dmsg['position']) if self.widgetGetters == None: dstate['moveStartWidgetTransform'] = dstate[ 'widgetTransform'] else: Twidget = self.widgetGetters[i]() if Twidget != None: dstate['moveStartWidgetTransform'] = Twidget else: dstate['moveStartWidgetTransform'] = dstate[ 'widgetTransform'] #================================================================================================ # #perform transformation of widget # deviceTransform = (so3.from_moment(dmsg['rotationMoment']),dmsg['position']) # #compute relative motion # Tdev = se3.mul(deviceToWidgetTransform,deviceTransform) # TdevStart = se3.mul(deviceToWidgetTransform,dstate['moveStartDeviceTransform']) # relDeviceTransform = (so3.mul(Tdev[0],so3.inv(TdevStart[0])),vectorops.sub(Tdev[1],TdevStart[1])) # #scale relative motion # Rrel,trel = relDeviceTransform # wrel = so3.moment(Rrel) # wrel = vectorops.mul(wrel,dstate['rotationScale']) # trel = vectorops.mul(trel,dstate['positionScale']) # #print "Moving",trel,"rotating",wrel # relDeviceTransform = (so3.from_moment(wrel),trel) # #perform transform # dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform) #================================================================================================ #- perform transformation of widget deviceTransform = (so3.from_moment(dmsg['rotationMoment']), dmsg['position']) # delta_device = vectorops.sub(deviceTransform[1],dstate['moveStartDeviceTransform'][1]) # print "haptic moves: ", delta_device delta_device_R_matrix = so3.mul( deviceTransform[0], so3.inv(dstate['moveStartDeviceTransform'][0])) # delta_device_R = so3.moment(delta_device_R_matrix) # norm_delta_R = vectorops.norm(delta_device_R) # if norm_delta_R != 0: # vec_delta_R = vectorops.div(delta_device_R, norm_delta_R) # else: # vec_delta_R = [0,0,0] # # print "haptic rotate: norm = ", norm_delta_R, ", vec = ", vec_delta_R #- compute relative motion Tdev = se3.mul(deviceToBaxterBaseTransform, deviceTransform) TdevStart = se3.mul(deviceToBaxterBaseTransform, dstate['moveStartDeviceTransform']) # delta_widget = vectorops.sub(Tdev[1],TdevStart[1]) # print "Baxter moves: ", delta_widget # delta_widget_R_matrix = so3.mul(Tdev[0],so3.inv(TdevStart[0])) # delta_widget_R = so3.moment(delta_widget_R_matrix) # norm_widget_R = vectorops.norm(delta_widget_R) # if norm_widget_R != 0: # vec_widget_R = vectorops.div(delta_widget_R, norm_widget_R) # else: # vec_widget_R = [0,0,0] # print "Baxter rotate: norm = ", norm_widget_R, ", vec = ", vec_widget_R relDeviceTransform = (so3.mul(Tdev[0], so3.inv(TdevStart[0])), vectorops.sub(Tdev[1], TdevStart[1])) #- scale relative motion Rrel, trel = relDeviceTransform wrel = so3.moment(Rrel) wrel = vectorops.mul(wrel, dstate['rotationScale']) trel = vectorops.mul(trel, dstate['positionScale']) #- print "Moving",trel,"rotating",wrel relDeviceTransform = (so3.from_moment(wrel), trel) #- perform transform # dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform) dstate['widgetTransform'] = ( so3.mul(dstate['moveStartWidgetTransform'][0], relDeviceTransform[0]), vectorops.add(dstate['moveStartWidgetTransform'][1], relDeviceTransform[1])) #================================================================================================ elif dstate['mode'] == 'scaling': if not oldButtonDown: dstate['moveStartDeviceTransform'] = (so3.from_moment( dmsg['rotationMoment']), dmsg['position']) #perform scaling deviceTransform = (so3.from_moment(dmsg['rotationMoment']), dmsg['position']) oldDeviceTransform = dstate['moveStartDeviceTransform'] znew = deviceTransform[1][1] zold = oldDeviceTransform[1][1] posscalerange = [1e-2, 20] rotscalerange = [0.5, 2] newposscale = min( max( dstate['positionScale'] * math.exp( (znew - zold) * 10), posscalerange[0]), posscalerange[1]) newrotscale = min( max( dstate['rotationScale'] * math.exp( (znew - zold) * 4), rotscalerange[0]), rotscalerange[1]) if any(newposscale == v for v in posscalerange) or any( newrotscale == v for v in rotscalerange): pass #dont change the scale else: dstate['positionScale'] = newposscale dstate['rotationScale'] = newrotscale print "Setting position scale to", dstate[ 'positionScale'], "rotation scale to", dstate[ 'rotationScale'] #update start device transform dstate['moveStartDeviceTransform'] = deviceTransform elif dstate['mode'] == 'gripper': print "Gripper mode not available" dstate['mode'] = 'normal' dstate['buttonDown'] = False else: dstate['buttonDown'] = False if self.viewer: self.viewer.update(deviceState) else: print "No viewer..."