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)
