def stop(self): if self._status == 'ok': robot.stopMotion() self._status = '' robot.enableCollisionChecking(False) return None
def start(self, args): print "Calling JointPose start", args if 'part' not in args and 'parts' not in args: self.onInvalidParameters("invalid argument list") return False if 'position' not in args and 'positions' not in args: self.onInvalidParameters("invalid argument list") return False if 'part' in args: parts = [args['part']] else: parts = args['parts'] if 'position' in args: angles = [args['position']] else: angles = args['positions'] if len(parts) != len(angles): self.onInvalidParameters( "Must have same number of parts as positions") speed = args.get('speed', 1.0) if 'safe' in args: print "Safety flag:", args['safe'] safe = args.get('safe', False) if safe: robot.enableCollisionChecking(True) print "Collision checking enabled on JointPose task" else: robot.enableCollisionChecking(False) print "Collision checking disabled on JointPose task" self.doneTests = [] for part, angle in zip(parts, angles): if part == 'left' or part == 0: if len(angle) != 7: self.onInvalidParameters( "invalid 'position' argument for left arm, not size 7") return False robot.left_mq.setRamp(angle, speed) self.doneTests.append(lambda: not robot.left_mq.moving()) elif part == 'right' or part == 1: if len(angle) != 7: self.onInvalidParameters( "invalid 'position' argument for right arm, not size 7" ) return False robot.right_mq.setRamp(angle) self.doneTests.append(lambda: not robot.right_mq.moving()) elif part == 'base' or part == 2: if len(angle) != 3: self.onInvalidParameters( "invalid 'position' argument for base, not size 3") return False robot.base.moveOdometryPosition(angle[0], angle[1], angle[2]) self.doneTests.append(lambda: not robot.base.moving()) else: self.onInvalidParameters("invalid 'part' setting: %s" % (str(part), )) return False if safe: robot.enableCollisionChecking(False) self._status = 'ok' return True
def start(self,args): print "Calling CartesianPose start",args if 'limb' not in args: self.onInvalidParameters("invalid argument list") return False if 'position' not in args: self.onInvalidParameters("invalid argument list") return False limb = args['limb'] position = args['position'] rotation = args.get('rotation',[]) toolCenterPoint = args.get('toolCenterPoint',None) maxJointDeviation = args.get('maxJointDeviation',0) speed = args.get('speed',1.0) safe = args.get('safe',False) if safe: print 'safe' robot.enableCollisionChecking(True) robot.left_limb.enableSelfCollisionAvoidance(False) robot.right_limb.enableSelfCollisionAvoidance(False) else: robot.enableCollisionChecking(False) robot.left_limb.enableSelfCollisionAvoidance(True) robot.right_limb.enableSelfCollisionAvoidance(True) if limb == 'left' or limb == 0: if len(position) !=3: self.onInvalidParameters("invalid 'position' argument, not size 3") return False if len(rotation) > 0 and len(rotation)!=9: self.onInvalidParameters("invalid 'rotation' argument, not size 3") return False if len(rotation) == 0: rotation = robot.left_ee.commandedTransform()[0] if toolCenterPoint is not None: robot.left_ee.setOffset(toolCenterPoint) robot.left_ee.moveTo(rotation, position, maxJointDeviation=maxJointDeviation) self.mqs = [robot.left_mq] elif limb == 'right' or limb == 1: if len(position) !=3: self.onInvalidParameters("invalid 'position' argument, not size 3") return False if len(rotation) > 0 and len(rotation)!=9: self.onInvalidParameters("invalid 'rotation' argument, not size 9") return False if len(rotation) == 0: rotation = robot.right_ee.commandedTransform()[0] if toolCenterPoint is not None: robot.right_ee.setOffset(toolCenterPoint) robot.right_ee.moveTo(rotation, position, maxJointDeviation=maxJointDeviation) self.mqs = [robot.right_mq] elif limb == 'both' or limb == 2: if len(position) != 6: self.onInvalidParameters("invalid 'position' argument, not size 6") return False if len(rotation) > 0 and len(rotation)!=18: self.onInvalidParameters("invalid 'rotation' argument, not size 18") return False if len(rotation) == 0: rotation = robot.left_ee.commandedTransform()[0] + robot.right_ee.commandedTransform()[0] if toolCenterPoint is not None: robot.right_ee.setOffset(toolCenterPoint) robot.left_ee.setOffset(toolCenterPoint) resl = robot.left_ee.moveTo(rotation[0:9], position[0:3], maxJointDeviation=maxJointDeviation) resr = robot.right_ee.moveTo(rotation[9:18], position[3:6], maxJointDeviation=maxJointDeviation) print "Left result",resl print "Right result",resr self.mqs = [robot.left_mq,robot.right_mq] else: self.onInvalidParameters("invalid 'limb' setting: %s"%(limb,)) return False if safe: robot.enableCollisionChecking(False) self._status = 'ok' return True
def start(self, args): # Error checking the input arguments from the dictionary print "Calling PhilipPoseVel start", args """if 'limb' not in args: self.onInvalidParameters("invalid argument list") return False""" if 'linear' not in args and 'angular' not in args: self.onInvalidParameters("invalid argument list") return False if 'angular' not in args: self.onInvalidParameters( "invalid argument list, must have velocity specified") return False if 'position' not in args: self.onInvalidParameters( "invalid argument list, must have joint position specified") return False if len(args['linear']) != 3: self.onInvalidParameters( "invalid argument list, velocity command is not length 3") return False if len(args['position']) != 7: self.onInvalidParameters( "invalid argument list, joint position command is not length 7" ) return False # Now I extract the arguments from the pose/vel command #--- Safe Control --- safe = args.get('safe', False) if safe: print 'safe' robot.enableCollisionChecking(True) robot.left_limb.enableSelfCollisionAvoidance(False) #robot.right_limb.enableSelfCollisionAvoidance(False) else: robot.enableCollisionChecking(False) robot.left_limb.enableSelfCollisionAvoidance(True) #robot.right_limb.enableSelfCollisionAvoidance(True) #--- End Safe Control --- # We do not actually care about which limb -- the left is pose control and the right one is cartesian control #limb = args['limb'] # velocity dlinear = args['linear'] dangular = args['angular'] toolCenterPoint = args.get('toolCenterPoint', None) # position angles = args['position'] # Time maxTime = args.get('maxTime', 1.0) if maxTime < 0: self.onInvalidParameters("maxTime argument is negative") return False # Now I set the Zero configuration for velocity control if len(dlinear) > 0 and len(dlinear) != 3: self.onInvalidParameters( "invalid 'linear' argument for linear velocity, not size 3") return False if len(dangular) > 0 and len(dangular) != 3: self.onInvalidParameters( "invalid 'angular' argument for angular velocity, not size 3") return False if len(dlinear) == 3 and len(dangular) == 0: dangular = [0.0, 0.0, 0.0] if len(dangular) == 3 and len(dlinear) == 0: dlinear = [0.0, 0.0, 0.0] #deadband for i, v in enumerate(dlinear): if abs(v) < 1e-4: dlinear[i] = 0 for i, v in enumerate(dangular): if abs(v) < 1e-4: dangular[i] = 0 if safe: robot.enableCollisionChecking(True) else: robot.enableCollisionChecking(False) # Command the left arm to a cartesian velocity #robot.left_ee.moveTo(rotation, position, maxJointDeviation=maxJointDeviation) robot.left_ee.driveCommand(dangular, dlinear) # Command the right arm to a joint pose #robot.right_ee.driveCommand(dangular, dlinear) robot.right_mq.setRamp(angles) self.mqs = [robot.left_mq, robot.right_mq] self.endTime = time.time() + maxTime if max(abs(v) for v in dangular + dlinear) == 0: self._status = 'done' else: self._status = 'ok' return True
def start(self, args): if 'limb' not in args: self.onInvalidParameters("invalid argument list") return False if 'linear' not in args and 'angular' not in args: self.onInvalidParameters("invalid argument list") return False limb = args['limb'] dlinear = args['linear'] dangular = args['angular'] toolCenterPoint = args.get('toolCenterPoint', None) maxTime = args.get('maxTime', 1.0) safe = args.get('safe', False) #deadband for i, v in enumerate(dlinear): if abs(v) < 1e-4: dlinear[i] = 0 for i, v in enumerate(dangular): if abs(v) < 1e-4: dangular[i] = 0 if safe: robot.enableCollisionChecking(True) else: robot.enableCollisionChecking(False) if limb == 'left' or limb == 0: if len(dlinear) > 0 and len(dlinear) != 3: self.onInvalidParameters( "invalid 'linear' argument for linear velocity, not size 3" ) return False if len(dangular) > 0 and len(dangular) != 3: self.onInvalidParameters( "invalid 'angular' argument for angular velocity, not size 3" ) return False if len(dlinear) == 3 and len(dangular) == 0: dangular = [0.0, 0.0, 0.0] if len(dangular) == 3 and len(dlinear) == 0: dlinear = [0.0, 0.0, 0.0] if toolCenterPoint is not None: robot.left_ee.setOffset(toolCenterPoint) robot.left_ee.driveCommand(dangular, dlinear) elif limb == 'right' or limb == 1: if len(dlinear) > 0 and len(dlinear) != 3: self.onInvalidParameters( "invalid 'linear' argument for linear velocity, not size 3" ) return False if len(dangular) > 0 and len(dangular) != 3: self.onInvalidParameters( "invalid 'angular' argument for angular velocity, not size 3" ) return False if len(dlinear) == 3 and len(dangular) == 0: dangular = [0.0, 0.0, 0.0] if len(dangular) == 3 and len(dlinear) == 0: dlinear = [0.0, 0.0, 0.0] if toolCenterPoint is not None: robot.right_ee.setOffset(toolCenterPoint) robot.right_ee.driveCommand(dangular, dlinear) elif limb == 'both' or limb == 2: if len(dlinear) > 0 and len(dlinear) != 6: self.onInvalidParameters( "invalid 'linear' argument for linear velocity, not size 6" ) return False if len(dangular) > 0 and len(dangular) != 6: self.onInvalidParameters( "invalid 'angular' argument for angular velocity, not size 6" ) return False if len(dlinear) == 6 and len(dangular) == 0: dangular = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] if len(dangular) == 6 and len(dlinear) == 0: dlinear = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] if toolCenterPoint is not None: robot.right_ee.setOffset(toolCenterPoint) robot.left_ee.setOffset(toolCenterPoint) robot.right_ee.driveCommand(dangular[0:3], dlinear[0:3]) robot.left_ee.driveCommand(dangular[3:6], dlinear[3:6]) else: self.onInvalidParameters("invalid 'limb' setting: %s" % (limb, )) return False if max(abs(v) for v in dangular + dlinear) == 0: self._status = 'done' else: self._status = 'ok' return True
def stop(self): self._status = '' robot.enableCollisionChecking(False) return None