def __init__(self, name, host, password, graphplan, actionplan, knowledge): self.communicator = Communicator() print "will init rosnode with name: " + name rospy.init_node(name) # set the speed of each servo device = ArbotiX("/dev/ttyUSB0") for i in range(0, 5): device.setSpeed(i, 20) self.arm = ArmPub(name) self.arm.goToPosition(ArmPub.POSITION_RELAX) behaviours = self.communicator.getBehaviours() behaviours.append(SystemCore()) # does not use the overwritten SpadeAgent # calls directly the constructor of Spade Agent Agent.__init__(self, name, host, password, graphplan, actionplan, behaviours) with open(knowledge) as knowledge: self.setData("knowledge", json.load(knowledge)[self.localName]) self.setData("goals", {"pose": {}}) self.setData("naoAid", spade.AID.aid("nao1@" + host, ["xmpp://nao1@" + host])) other = self.getData("knowledge")["other"] + "@" + host otherAid = spade.AID.aid(other, ["xmpp://" + other]) self.setData("otherTurtleAid", otherAid) self.setData("otherState", Goals.otherStatus["nonReady"]) print self.getData("knowledge") # stores parameters received by waitMessageBehaviour # in case it is a plan self.plan = "" # stores the parameters of an order self.parameters = list() # stores who gave the order self.messageSender = "" self.name = name
class Arm: DEFAULT_SPEED = 20 # predifined positions. More an be added the same way # a list of angles for each servo (first one is the gripper then the motors in order) # from these positions we can define variants: # with gripper closed. First parameter = 2.6 # on the left 90. Second parameter = PI/2 # on the right 90. Second parameter = -PI/2 POSITION_1 = [0,0,0.38,0.28, 0.88, -1.7] POSITION_2 = [0,0,1.07,-0.59, 0.88, -1.7] POSITION_RELAX = [0,0,-1.21,1.5,0.64, 0] POSITION_BRING = [2.6,0,-0.57,0.12,1.5,-1.7] def __init__(self, port="/dev/ttyUSB0"): # creating an ArbotiX device (the arm) on port (normally "/dev/ttyUSB0") self.device = ArbotiX(port) joint_defaults = getJointsFromURDF() self.servos = list() # list of the joints joints = rospy.get_param('/arbotix/joints', dict()) for name in sorted(joints.keys()): # pull data to convert from Angle to ticks n = "/arbotix/joints/"+ name +"/" ticks = rospy.get_param(n+"ticks", 1024) neutral = rospy.get_param(n+"neutral", ticks/2) invert = rospy.get_param(n+"invert", False) ranges = rospy.get_param(n+"range", 300) rad_per_tick = radians(ranges)/ticks # pull min and max angles for each servo min_angle, max_angle = getJointLimits(name, joint_defaults) serv = Servo(name, min_angle, max_angle, ticks, neutral, rad_per_tick, invert) self.servos.append(serv) # moves a single servo at a given speed to a given angle (in radians) # by default the speed is set at 20 def moveServo(self, idserv, angle , speed=DEFAULT_SPEED): serv = self.servos[idserv] self.device.setSpeed(idserv, speed) ticks = serv.angleToTicks(angle) self.device.setPosition(idserv, ticks) def goToPosition(self, position): i = 0 for pos in position: print "moving servo: ", i self.moveServo(i,pos) i = i+1 def closeGripper(self): min_angle = self.servos[0].min_angle print " min angle for gripper = " , min_angle self.moveServo(0,min_angle) def openGripper(self): max_angle = self.servos[0].max_angle print " max angle for gripper = " , max_angle self.moveServo(0,max_angle) def take(self): self.openGripper() time.sleep(2) self.goToPosition(self.POSITION_1) # waits 2 seconds before closing the gripper time.sleep(2) self.closeGripper() # waits 2 seconds before taking the object off the support time.sleep(2) self.goToPosition(Arm.POSITION_BRING) def put(self): self.goToPosition(self.POSITION_1) # waits 2 seconds before closing the gripper time.sleep(2) self.openGripper() # waits 2 seconds before taking the object off the support time.sleep(2) self.goToPosition(Arm.POSITION_BRING)