def setHeadJointPosition(self,joint,position):
self.iPosHead.positionMove(joint,position)
done = self.iPosHead.checkMotionDone()
while not done:
print "reaching head target position..."
yarp.Time_delay(0.1)
done = self.iPosHead.checkMotionDone()
print "head target position reached"
def setHeadPosition(self,encodersList):
for i in range(len(encodersList)):
self.iPosHead.positionMove(i,encodersList[i])
done = self.iPosHead.checkMotionDone()
while not done:
print "reaching head starting position..."
yarp.Time_delay(0.1)
done = self.iPosHead.checkMotionDone()
print "ready"
def main():
yarp.Network.init()
options = yarp.Property()
driver = yarp.PolyDriver()
# set the poly driver options
options.put("robot", "icub")
options.put("device", "remote_controlboard")
options.put("local", "/example_enc/client")
options.put("remote", "/icub/left_arm")
# opening the drivers
print 'Opening the motor driver...'
driver.open(options)
if not driver.isValid():
print 'Cannot open the driver!'
sys.exit()
# opening the drivers
print 'Viewing motor position/encoders...'
ipos = driver.viewIPositionControl()
ienc = driver.viewIEncoders()
if ienc is None or ipos is None:
print 'Cannot view motor positions/encoders!'
sys.exit()
# wait a bit for the interface
yarp.Time_delay(1.0)
encs = yarp.Vector(ipos.getAxes())
for i in range(0, 10):
# read encoders
ret = ienc.getEncoders(encs.data())
if ret is False: continue
print "Current encoders value: "
print encs.toString(-1, -1)
yarp.Time_delay(0.01)
# closing the driver
driver.close()
yarp.Network.fini()
def loadInterfaces(self):
yarp.Network.init()
# load parameters from config file
self.params = dict()
fileDescriptor = open(self.configFileFullName,'r')
self.params['robot'] = self.readString(fileDescriptor,'robot')
self.params['whichHand'] = self.readString(fileDescriptor,'whichHand')
# create, open and connect ports
# tactile port
self.tactDataPort = yarp.BufferedPortBottle()
tactDataPortName = "/gpc/skin/" + self.params['whichHand'] + "_hand_comp:i"
self.tactDataPort.open(tactDataPortName)
yarp.Network.connect("/icub/skin/" + self.params['whichHand'] + "_hand_comp",tactDataPortName)
# encoders port
self.encDataPort = yarp.BufferedPortBottle()
encDataPortName = "/gpc/" + self.params['whichHand'] + "_hand/analog:i"
self.encDataPort.open(encDataPortName)
yarp.Network.connect("/icub/" + self.params['whichHand'] + "_hand/analog:o",encDataPortName)
# log port
self.logPort = yarp.BufferedPortBottle()
logPortName = "/gpc/log:o"
self.logPort.open(logPortName)
yarp.Network.connect(logPortName,self.dataDumperPortName)
# grasp module log port
self.graspModuleOutLogPort = yarp.BufferedPortBottle()
graspModuleOutLogPortName = "/gpc/graspModuleLog:o"
self.graspModuleOutLogPort.open(graspModuleOutLogPortName)
yarp.Network.connect(graspModuleOutLogPortName,"/plantIdentification/policyActions:i")
# create driver and options
self.driver = yarp.PolyDriver()
options = yarp.Property()
# set driver options
options.put("robot",self.params['robot'])
options.put("device","remote_controlboard")
options.put("local","/gpc/encoders/" + self.params['whichHand'] + "_arm")
options.put("remote","/icub/" + self.params['whichHand'] + "_arm")
# open driver
print 'Opening motor driver'
self.driver.open(options)
if not self.driver.isValid():
print 'Cannot open driver!'
sys.exit()
# create interfaces
print 'enabling interfaces'
self.iPos = self.driver.viewIPositionControl()
if self.iPos is None:
print 'Cannot view position interface!'
sys.exit()
self.iPosDir = self.driver.viewIPositionDirect()
if self.iPosDir is None:
print 'Cannot view position direct interface!'
sys.exit()
self.iEnc = self.driver.viewIEncoders()
if self.iEnc is None:
print 'Cannot view encoders interface!'
sys.exit()
self.iVel = self.driver.viewIVelocityControl()
if self.iVel is None:
print 'Cannot view velocity interface!'
sys.exit()
self.iCtrl = self.driver.viewIControlMode2()
if self.iCtrl is None:
print 'Cannot view control mode interface!'
sys.exit()
self.iOlc = self.driver.viewIOpenLoopControl()
if self.iOlc is None:
print 'Cannot view open loop control mode interface!'
sys.exit()
self.numJoints = self.iPos.getAxes()
### HEAD ###
# create driver and options
self.headDriver = yarp.PolyDriver()
headOptions = yarp.Property()
# set driver options
headOptions.put("robot",self.params['robot'])
headOptions.put("device","remote_controlboard")
headOptions.put("local","/gpc/encodersHead/head")
headOptions.put("remote","/icub/head")
# open drivers
self.headDriver.open(headOptions)
if not self.headDriver.isValid():
print 'Cannot open head driver!'
sys.exit()
# create interfaces
print 'enabling head interfaces'
self.iPosHead = self.headDriver.viewIPositionControl()
if self.iPosHead is None:
print 'Cannot view head position interface!'
sys.exit()
# wait a bit for the interfaces to be ready
yarp.Time_delay(1.0)
# read encoders data for the first time
print 'checking encoders data'
self.previousEncodersData = yarp.Vector(self.numJoints)
ret = self.iEnc.getEncoders(self.previousEncodersData.data())
count = 0
while ret is False and count < 100:
yarp.Time_delay(0.01)
count = count + 1
ret = self.iEnc.getEncoders(self.previousEncodersData.data())
if count == 100:
print 'encoders reading failed'
# read encoders data from port for the first time
print 'checking encoders data from port'
self.previousEncodersDataFP = self.encDataPort.read(False)
count = 0
while self.previousEncodersDataFP is None and count < 100:
yarp.Time_delay(0.01)
count = count + 1
self.previousEncodersDataFP = self.encDataPort.read(False)
if count == 100:
print 'encoders data reading from port failed'
# read tactile data for the first time
print 'checking tactile data'
self.previousTactileData = self.tactDataPort.read(False)
count = 0
while self.previousTactileData is None and count < 100:
yarp.Time_delay(0.01)
count = count + 1
self.previousTactileData = self.tactDataPort.read(False)
if count == 100:
print 'tactile data reading failed'
self.isInterfaceLoaded = True