def test_send_and_recv_setExt(self):
port_in = yarp.Port()
in_port_name = '/test/img:i'
self.assertTrue(port_in.open(in_port_name))
port_out = yarp.Port()
port_out.enableBackgroundWrite(True);
out_port_name = '/test/img:o'
self.assertTrue(port_out.open(out_port_name))
self.assertTrue(yarp.Network.connect(port_out.getName(), port_in.getName()))
yarp.delay(0.5)
height = 240
width = 320
yarp_img_out = yarp.ImageMono()
yarp_img_out.resize(width, height)
yarp_img_in = yarp.ImageMono()
self.assertEqual(240, yarp_img_out.height())
self.assertEqual(320, yarp_img_out.width())
yarp_vector = yarp.Vector()
yarp_vector.resize(320*240, 0)
self.assertTrue(240*320, yarp_vector.size())
yarp_img_out.setExternal(yarp_vector, width, height)
self.assertTrue(port_out.write(yarp_img_out))
yarp.delay(0.5)
self.assertTrue(port_in.read(yarp_img_in))
self.assertEqual(240, yarp_img_in.height())
self.assertEqual(320, yarp_img_in.width())
port_out.close()
port_in.close()
def test_open_and_connect_comm(self):
p = yarp.BufferedPortBottle()
p2 = yarp.BufferedPortBottle()
bt_out = p.prepare()
bt_out.addInt32(10)
self.assertTrue(p.open("/python/out"))
self.assertTrue(p2.open("/python/in"))
self.assertTrue(yarp.Network.connect(p.getName(),p2.getName()))
yarp.delay(0.5)
p.write()
yarp.delay(0.5)
bt_in=p2.read()
self.assertEqual(1, bt_in.size())
self.assertEqual(10, bt_in.get(0).asInt32())
p.close()
p2.close()
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.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.delay(0.01)
# closing the driver
driver.close()
yarp.Network.fini()
def __skin_sensor_data_reader_right(self) -> A:
"""Read skin sensor data from the right hand"""
# print('----------------right {} with {} sensors-----------------'.format(self.flag, self.numSensors))
tactile_arm: yarp.Vector = yarp.Vector(self.numSensors)
while (not self.__input_port_skin_right.read(tactile_arm)):
print("none right conection!")
yarp.delay(0.001)
#self.__input_port_skin_right.read(tactile_arm)
#self.__input_port_skin_right.read(tactile_arm)
data_hand: List = []
for j in range(len(self.__idx_skin_right)):
if self.__idx_skin_right[j] > 0:
#print("read Data Right: {}".format(j),tactile_hand.get(j))
data_hand.append(tactile_arm.get(j))
# print("Data right {}:".format(self.flag))
#print(data_hand)
#time.sleep(0.5)
return data_hand
def __skin_sensor_data_reader_left(self) -> A:
"""Read skin sensor data from the left hand """
# print('---------------left {} with {} sensors-------------------'.format(self.flag, self.numSensors))
tactile_arm_l: yarp.Vector = yarp.Vector(self.numSensors)
while (not self.__input_port_skin_left.read(tactile_arm_l)):
print("none left conection!")
yarp.delay(0.001)
#self.__input_port_skin_left.read(tactile_arm_l)
#self.__input_port_skin_left.read(tactile_arm_l)
data_hand_l: List = []
for j in range(len(self.__idx_skin_left)):
if self.__idx_skin_left[j] > 0:
#print("read Data Left: {}".format(j), tactile_hand_l.get(j))
data_hand_l.append(tactile_arm_l.get(j))
# print("Data left {}:".format(self.flag))
# print(data_hand_l)
# time.sleep(0.5)
return data_hand_l
def dv(self, v, dt, log, sem, event_loop):
"""Delayed value"""
dv = delay(v, dt, loop=event_loop)
def on_change(value):
log.append((event_loop.time(), value))
sem.release()
dv.on_value_changed(on_change)
return dv
def move(self,
target_joints,
req_time,
joints_list=None,
waitMotionDone=True):
self.__logger__.info("""Moving joints STARTED!
target_joints:%s
req_time:%.2f,
joints_list=%s,
waitMotionDone=%s""" %
(str(target_joints), req_time, str(joints_list),
str(waitMotionDone)))
if joints_list is None:
joints_list = self.__joints_list__
disp = [0] * len(joints_list)
speed = [0] * len(joints_list)
tmp = yarp.Vector(len(joints_list))
encs = yarp.Vector(16)
while not self.__IEncoders__.getEncoders(encs.data()):
yarp.delay(0.005)
i = 0
for j in joints_list:
tmp.set(i, target_joints[i])
disp[i] = target_joints[i] - encs[j]
if disp[i] < 0.0:
disp[i] = -disp[i]
speed[i] = disp[i] / req_time
self.__IPositionControl__.setRefSpeed(j, speed[i])
self.__IPositionControl__.positionMove(j, tmp[i])
i += 1
if waitMotionDone is True:
self.waitMotionDone(target_joints,
joints_list,
timeout=2 * req_time)
self.__logger__.info("""Moving joints COMPLETED!
target_joints:%s
req_time:%.2f,
joints_list=%s,
waitMotionDone=%s""" %
(str(target_joints), req_time, str(joints_list),
str(waitMotionDone)))
def waitMotionOnset(self,
speed_ref=0,
period=WAITMOTION_PERIOD,
max_attempts=50):
self.__logger__.info("""Waiting for gaze motion onset STARTED!
speed_ref=%s""" % str(speed_ref))
q = yarp.Vector(6)
for _ in range(0, max_attempts):
self.IGazeControl.getJointsVelocities(q)
v = []
for i in range(0, 6):
v.append(q[i])
speed = utils.norm(v)
if speed > speed_ref:
self.__logger__.info(
"""Motion onset DETECTED! speed_ref=%s""" % str(speed_ref))
return True
yarp.delay(period)
self.__logger__.warning("""Motion onset TIMEOUT! speed_ref=%s""" %
str(speed_ref))
return False
async def test_instantaneous(self, event_loop):
value = Value()
delayed_value = delay(value, 0.1, loop=event_loop)
assert delayed_value.value is NoValue
# Monitor changes
evt = asyncio.Event(loop=event_loop)
m = Mock(side_effect=lambda *_: evt.set())
delayed_value.on_value_changed(m)
# Trigger a change for later...
before = time.time()
value.set_instantaneous_value(123)
assert delayed_value.value is NoValue
assert not m.mock_calls
await evt.wait()
assert time.time() - before >= 0.1
m.assert_called_once_with(123)
assert delayed_value.value is NoValue
def configure(self, rf):
self.config = yarp.Property()
self.config.fromConfigFile(
'/code/icub_perceptual_optimisation/yarp/config.ini')
self.width = self.config.findGroup('CAMERA').find('width').asInt32()
self.height = self.config.findGroup('CAMERA').find('height').asInt32()
if self.config.findGroup('GENERAL').find('show_images').asBool():
import matplotlib
matplotlib.use('TKAgg')
self.ax_left = plt.subplot(1, 2, 1)
self.ax_right = plt.subplot(1, 2, 2)
# prepare motor driver
self.head_motors = 'head'
self.head_motorprops = yarp.Property()
self.head_motorprops.put("device", "remote_controlboard")
self.head_motorprops.put("local",
"/client_babbling/" + self.head_motors)
self.head_motorprops.put("remote", "/icubSim/" + self.head_motors)
self.left_motors = 'left_arm'
self.left_motorprops = yarp.Property()
self.left_motorprops.put("device", "remote_controlboard")
self.left_motorprops.put("local",
"/client_babbling/" + self.left_motors)
self.left_motorprops.put("remote", "/icubSim/" + self.left_motors)
self.right_motors = 'right_arm'
self.right_motorprops = yarp.Property()
self.right_motorprops.put("device", "remote_controlboard")
self.right_motorprops.put("local",
"/client_babbling/" + self.right_motors)
self.right_motorprops.put("remote", "/icubSim/" + self.right_motors)
# create remote driver
self.head_driver = yarp.PolyDriver(self.head_motorprops)
print('head motor driver prepared')
# query motor control interfaces
self.head_iPos = self.head_driver.viewIPositionControl()
self.head_iVel = self.head_driver.viewIVelocityControl()
self.head_iEnc = self.head_driver.viewIEncoders()
self.head_iCtlLim = self.head_driver.viewIControlLimits()
self.left_armDriver = yarp.PolyDriver(self.left_motorprops)
print('left motor driver prepared')
# query motor control interfaces
self.left_iPos = self.left_armDriver.viewIPositionControl()
self.left_iVel = self.left_armDriver.viewIVelocityControl()
self.left_iEnc = self.left_armDriver.viewIEncoders()
self.left_iCtlLim = self.left_armDriver.viewIControlLimits()
self.right_armDriver = yarp.PolyDriver(self.right_motorprops)
print('right motor driver prepared')
# query motor control interfaces
self.right_iPos = self.right_armDriver.viewIPositionControl()
self.right_iVel = self.right_armDriver.viewIVelocityControl()
self.right_iEnc = self.right_armDriver.viewIEncoders()
self.right_iCtlLim = self.right_armDriver.viewIControlLimits()
# number of joints
self.num_joints = self.left_iPos.getAxes()
print('Num joints: ', self.num_joints)
self.head_num_joints = self.head_iPos.getAxes()
print('Num head joints: ', self.head_num_joints)
self.head_limits = []
for i in range(self.head_num_joints):
head_min = yarp.DVector(1)
head_max = yarp.DVector(1)
self.head_iCtlLim.getLimits(i, head_min, head_max)
print('lim head ', i, ' ', head_min[0], ' ', head_max[0])
self.head_limits.append([head_min[0], head_max[0]])
self.left_limits = []
self.right_limits = []
for i in range(self.num_joints):
left_min = yarp.DVector(1)
left_max = yarp.DVector(1)
self.left_iCtlLim.getLimits(i, left_min, left_max)
#print('lim left ', i, ' ', left_min[0], ' ', left_max[0])
self.left_limits.append([left_min[0], left_max[0]])
right_min = yarp.DVector(1)
right_max = yarp.DVector(1)
self.right_iCtlLim.getLimits(i, right_min, right_max)
#print('lim right ', i, ' ', right_min[0], ' ', right_max[0])
self.right_limits.append([right_min[0], right_max[0]])
self.go_to_starting_pos()
moduleName = rf.check("name", yarp.Value("BabblingModule")).asString()
self.setName(moduleName)
print('module name: ', moduleName)
yarp.delay(5.0)
print('starting now')
csvFile = "imu.csv"
# YARP
yarp.Network.init() # connect to YARP network
if yarp.Network.checkNetwork(
) != True: # let's see if there was actually a reachable YARP network
print('[error] Please try running yarp server'
) # tell the user to start one with 'yarp server' if there isn't any
quit()
#create a new input port and open it
in_port = yarp.Port()
in_port.open("/softimu/in")
#connect up the output port to our input port
yarp.Network.connect("/softimu/out", "/softimu/in")
# writing CSV
start = yarp.now()
imu = yarp.Vector(2)
with open(csvFile, 'w') as csvOutfile:
csvwriter = csv.writer(csvOutfile, delimiter=',')
csvwriter.writerow(['timestamp', 'imu-roll', 'imu-pitch'])
while (1):
in_port.read(imu)
print('timestam: ', round(yarp.now() - start, 3))
print('IMU: [', imu[0], ', ', imu[1], ']')
csvwriter.writerow([round(yarp.now() - start, 3), imu[0], imu[1]])
yarp.delay(DELAY)
print("in DataProcessor.read")
if not(connection.isValid()):
print("Connection shutting down")
return False
bin = yarp.Bottle()
bout = yarp.Bottle()
print("Trying to read from connection")
ok = bin.read(connection)
if not(ok):
print("Failed to read input")
return False
print("Received [%s]"%bin.toString())
bout.addString("Received:")
bout.append(bin)
print("Sending [%s]"%bout.toString())
writer = connection.getWriter()
if writer==None:
print("No one to reply to")
return True
return bout.write(writer)
p = yarp.Port()
r = DataProcessor()
p.setReader(r)
p.open("/python");
yarp.delay(100)
print("Test program timer finished")
yarp.Network.fini();
# This software may be modified and distributed under the terms of the
# BSD-3-Clause license. See the accompanying LICENSE file for details.
import yarp
yarp.Network.init()
rf = yarp.ResourceFinder()
rf.setVerbose(True);
rf.setDefaultContext("myContext");
rf.setDefaultConfigFile("default.ini");
p = yarp.BufferedPortBottle()
p.open("/python");
top = 100;
for i in range(1,top):
bottle = p.prepare()
bottle.clear()
bottle.addString("count")
bottle.addInt32(i)
bottle.addString("of")
bottle.addInt32(top)
print ("Sending", bottle.toString())
p.write()
yarp.delay(0.5)
p.close();
yarp.Network.fini();
rightArmDevice = yarp.PolyDriver(options)
rightArmEnc = rightArmDevice.viewIEncoders()
rightArmMode = rightArmDevice.viewIControlMode()
rightArmPosd = rightArmDevice.viewIPositionDirect()
rightArmAxes = rightArmEnc.getAxes()
# single joint
rightArmMode.setControlMode(joint, yarp.VOCAB_CM_POSITION_DIRECT)
with open(csvInPath, 'r') as csvInFile:
start = yarp.now()
i = 1
csvreader = csv.reader(csvInFile)
with open(csvOutPath, 'w', newline='') as csvOutfile:
csvwriter = csv.writer(csvOutfile, delimiter=',')
csvwriter.writerow(['timestamp', 'value'])
for row in csvreader:
if True:
print('reading >> ', row[3])
rightArmPosd.setPosition(joint, float(row[3])) # set position
print('encoder >> ', rightArmEnc.getEncoder(joint))
csvwriter.writerow([yarp.now(), rightArmEnc.getEncoder(joint)])
delay = DELAY * i - (yarp.now() - start)
yarp.delay(delay)
i = i + 1
rightArmDevice.close()
yarp.Network.fini()
#
# This software may be modified and distributed under the terms of the
# BSD-3-Clause license. See the accompanying LICENSE file for details.
import yarp
yarp.Network.init()
rf = yarp.ResourceFinder()
rf.setDefaultContext("myContext")
rf.setDefaultConfigFile("default.ini")
p = yarp.BufferedPortBottle()
p.open("/python")
top = 100
for i in range(1, top):
bottle = p.prepare()
bottle.clear()
bottle.addString("count")
bottle.addInt32(i)
bottle.addString("of")
bottle.addInt32(top)
print("Sending", bottle.toString())
p.write()
yarp.delay(0.5)
p.close()
yarp.Network.fini()
writingThread = threading.Thread(target=writePossToCsv)
writingThread.start()
# move position
def moveTrunk(axial, frontal):
tr = yarp.DVector(axesTR, 0.0)
tr = list([axial, frontal])
posTR.positionMove(yarp.DVector(tr))
while not posTR.checkMotionDone():
sleep(0.1)
# Trunk movements
moveTrunk(20, -12)
yarp.delay(timeSpace)
moveTrunk(0, 0)
yarp.delay(timeSpace)
moveTrunk(20, 12)
yarp.delay(timeSpace)
moveTrunk(0, 0)
yarp.delay(timeSpace)
moveTrunk(-20, -12)
yarp.delay(timeSpace)
moveTrunk(0, 0)
yarp.delay(timeSpace)
moveTrunk(-20, 12)
yarp.delay(timeSpace)
moveTrunk(0, 0)
options.put('device','remote_grabber')
options.put('remote', '/grabber')
options.put('local','/python/grabber')
dd = yarp.PolyDriver(options)
if not dd.isValid():
print '[error] Please launch camera side'
quit()
# View grabber interfaces
grabberControls = dd.viewIFrameGrabberControls()
grabber = dd.viewIFrameGrabberImage()
# Check if a feature exists
print(grabberControls.hasFeature(yarp.YARP_FEATURE_GAIN))
# Get the current value of a feature
gain = grabberControls.getFeature(yarp.YARP_FEATURE_GAIN)
print(gain)
# Set a new value for a feature
grabberControls.setFeature(yarp.YARP_FEATURE_GAIN, 10)
# Obtain an image
yarp.delay(0.5) # May have to previously wait
yarpImg = yarp.ImageRgb()
grabber.getImage(yarpImg)
# Close device and disconnect from the YARP network
dd.close()
yarp.Network.fini()
diffSumSquare = 0
# for i in range(len(x_current)):
# diffSumSquare += (x_current[i] - x_vector[i])*(x_current[i]-x_vector[i])
# if diffSumSquare>0.0000001:
if (trunkRightArmICartesianSolver.invKin(x_vector, current_Q, desireQ)):
# print("i: ", i)
# print("Current Q:",current_Q[0], current_Q[1], current_Q[2], current_Q[3], current_Q[4], current_Q[5], current_Q[6], current_Q[7])
# print("Desire Q",desireQ[0], desireQ[1], desireQ[2], desireQ[3], desireQ[4], desireQ[5], desireQ[6], desireQ[7])
for joint in range(0, numRightArmJoints, 1):
rightArmIPositionControl.positionMove(joint, desireQ[joint + 2])
for joint in range(numTrunkJoints):
print(numTrunkJoints, joint, desireQ[joint])
trunkIPositionControl.positionMove(joint, desireQ[joint])
yarp.delay(0.35)
i = i + 1
else:
if (trunkRightArmICartesianSolver.invKin(x_vector, current_Q,
desireQ)):
# print("i: ", i)
# print("Current Q:",current_Q[0], current_Q[1], current_Q[2], current_Q[3], current_Q[4], current_Q[5], current_Q[6], current_Q[7])
# print("Desire Q",desireQ[0], desireQ[1], desireQ[2], desireQ[3], desireQ[4], desireQ[5], desireQ[6], desireQ[7])
for joint in range(0, numRightArmJoints, 1):
rightArmIPositionControl.positionMove(joint,
desireQ[joint + 2])
for joint in range(numTrunkJoints):
trunkIPositionControl.positionMove(joint, desireQ[joint])
yarp.delay(0.35)
grabberOptions.put('sensorIndex',0)
grabberDevice = yarp.PolyDriver(grabberOptions)
# View specific interfaces
pos = controlboardDevice.viewIPositionControl()
grabberControls = grabberDevice.viewIFrameGrabberControls()
grabber = grabberDevice.viewIFrameGrabberImage()
# do stuff
grabberControls.setFeature(yarp.YARP_FEATURE_ZOOM, 0.3)
npImg = np.zeros((grabber.height(), grabber.width(), 3), dtype = np.uint8)
yarpImg = yarp.ImageRgb()
yarpImg.resize(grabber.width(), grabber.height())
yarpImg.setExternal(npImg, npImg.shape[1], npImg.shape[0])
yarp.delay(0.5)
grabber.getImage(yarpImg) # refeshes npImg
#matplotlib.pylab.imshow(npImg,interpolation='none')
#matplotlib.pylab.show() # blocking
r,g,b = cv.split(npImg)
colorMinusColor = r-g
res, binary = cv.threshold(colorMinusColor,55,255,cv.THRESH_BINARY)
kernel = np.ones((2,2),np.uint8)
binary = cv.dilate(binary,kernel,iterations = 1)
binary = cv.erode(binary,kernel,iterations = 1)
contours = cv.findContours(binary, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)
contours = contours[1] # cv2
contoursSorted = sorted(contours, key=lambda x: cv.contourArea(x), reverse=True)
lmin = yarp.DVector(1)
lmax = yarp.DVector(1)
ll.getLimits(0, lmin, lmax) # retrieve limits of joint 0
print('lmin', lmin[0], 'lmax', lmax[0])
# use the object to set the device to position mode (as opposed to velocity mode)(note: stops the robot)
mode.setControlModes(yarp.IVector(axes, yarp.encode('pos')))
print 'positionMove(1,-35) -> moves motor 1 (start count at motor 0) to -35 degrees'
pos.positionMove(1, -35)
done = False
while not done:
print 'wait to reach...'
yarp.delay(1.0) # [s]
done = pos.checkMotionDone()
v = yarp.DVector(
axes
) # create a YARP vector of doubles the size of the number of elements read by enc, call it 'v'
enc.getEncoders(v) # read the encoder values and put them into 'v'
print 'v[1] is: ' + str(
v[1]) # print element 1 of 'v', note that motors and encoders start at 0
targets = list(range(0, 10 + 5 * axes, 5))
print 'positionMove(...) -> [multiple axes] moves motor 0 to 10 degrees, motor 1 to 15 degrees and so on'
pos.positionMove(yarp.DVector(targets))
done = False
while not done:
def tactile_prcptn_yarp():
# Open and connect YARP-Port to read right upper arm skin sensor data
input_port_skin_rarm = yarp.Port()
if not input_port_skin_rarm.open("/" + CLIENT_PREFIX + "/skin_read/rarm"):
print("[ERROR] Could not open skin arm port")
if not yarp.Network.connect("/" + ROBOT_PREFIX + "/skin/right_arm_comp",
input_port_skin_rarm.getName()):
print("[ERROR] Could not connect skin arm port!")
# Open and connect YARP-Port to read right forearm skin sensor data
input_port_skin_rforearm = yarp.Port()
if not input_port_skin_rforearm.open("/" + CLIENT_PREFIX +
"/skin_read/rforearm"):
print("[ERROR] Could not open skin forearm port!")
if not yarp.Network.connect(
"/" + ROBOT_PREFIX + "/skin/right_forearm_comp",
input_port_skin_rforearm.getName()):
print("[ERROR] Could not connect skin forearm port!")
# Open and connect YARP-Port to read right hand skin sensor data
input_port_skin_rhand = yarp.Port()
if not input_port_skin_rhand.open("/" + CLIENT_PREFIX +
"/skin_read/rhand"):
print("[ERROR] Could not open skin hand port!")
if not yarp.Network.connect("/" + ROBOT_PREFIX + "/skin/right_hand_comp",
input_port_skin_rhand.getName()):
print("[ERROR] Could not connect skin hand port!")
######################################################################
####################### Read skin sensor data ########################
######################################################################
for i in range(10):
tactile_rarm = yarp.Vector(768)
while (not input_port_skin_rarm.read(tactile_rarm)):
yarp.delay(0.001)
tactile_rforearm = yarp.Vector(384)
while (not input_port_skin_rforearm.read(tactile_rforearm)):
yarp.delay(0.001)
tactile_rhand = yarp.Vector(192)
while (not input_port_skin_rhand.read(tactile_rhand)):
yarp.delay(0.001)
data_rarm = []
data_rforearm = []
data_rhand = []
for j in range(len(skin_idx_r_arm)):
if skin_idx_r_arm[j] > 0:
data_rarm.append(tactile_rarm.get(j))
for j in range(len(skin_idx_r_forearm)):
if skin_idx_r_forearm[j] > 0:
data_rforearm.append(tactile_rforearm.get(j))
for j in range(len(skin_idx_r_hand)):
if skin_idx_r_hand[j] > 0:
data_rhand.append(tactile_rhand.get(j))
print("Data arm:")
print(data_rarm)
print("Data forearm:")
print(data_rforearm)
print("Data hand:")
print(data_rhand)
time.sleep(2.0)
######################################################################
######################## Closing the program: ########################
#### Delete objects/models and close ports, network, motor cotrol ####
print('----- Close opened ports -----')
# disconnect the ports
if not yarp.Network.disconnect("/" + ROBOT_PREFIX + "/skin/right_arm_comp",
input_port_skin_rarm.getName()):
print("[ERROR] Could not disconnect skin arm port!")
if not yarp.Network.disconnect(
"/" + ROBOT_PREFIX + "/skin/right_forearm_comp",
input_port_skin_rforearm.getName()):
print("[ERROR] Could not disconnect skin forearm port!")
if not yarp.Network.disconnect(
"/" + ROBOT_PREFIX + "/skin/right_hand_comp",
input_port_skin_rhand.getName()):
print("[ERROR] Could not disconnect skin hand port!")
# close the ports
input_port_skin_rarm.close()
input_port_skin_rforearm.close()
input_port_skin_rhand.close()
yarp.Network.fini()
if not (connection.isValid()):
print("Connection shutting down")
return False
bin = yarp.Bottle()
bout = yarp.Bottle()
print("Trying to read from connection")
ok = bin.read(connection)
if not (ok):
print("Failed to read input")
return False
print("Received [%s]" % bin.toString())
bout.addString("Received:")
bout.append(bin)
print("Sending [%s]" % bout.toString())
writer = connection.getWriter()
if writer == None:
print("No one to reply to")
return True
return bout.write(writer)
p = yarp.Port()
r = DataProcessor()
p.setReader(r)
p.open("/python")
yarp.delay(100)
print("Test program timer finished")
yarp.Network.fini()
def configure(self, rf):
self.config = yarp.Property()
self.config.fromConfigFile('/code/icub_intrinsic_motivation/yarp/config.ini')
self.width = self.config.findGroup('CAMERA').find('width').asInt32()
self.height = self.config.findGroup('CAMERA').find('height').asInt32()
if self.config.findGroup('GENERAL').find('show_images').asBool():
import matplotlib
matplotlib.use('TKAgg')
self.ax_left = plt.subplot(1, 2, 1)
self.ax_right = plt.subplot(1, 2, 2)
# Create a port and connect it to the iCub simulator virtual camera
self.input_port_cam = yarp.Port()
self.input_port_cam.open("/icub/camera_left")
yarp.Network.connect("/icubSim/cam/left", "/icub/camera_left")
# prepare image
self.yarp_img_in = yarp.ImageRgb()
self.yarp_img_in.resize(self.width, self.height)
self.img_array = np.ones((self.height, self.width, 3), dtype=np.uint8)
# yarp image will be available in self.img_array
self.yarp_img_in.setExternal(self.img_array.data, self.width, self.height)
# prepare motor driver
self.left_motors = 'left_arm'
self.left_motorprops = yarp.Property()
self.left_motorprops.put("device", "remote_controlboard")
self.left_motorprops.put("local", "/client/" + self.left_motors)
self.left_motorprops.put("remote", "/icubSim/" + self.left_motors)
self.right_motors = 'right_arm'
self.right_motorprops = yarp.Property()
self.right_motorprops.put("device", "remote_controlboard")
self.right_motorprops.put("local", "/client/" + self.right_motors)
self.right_motorprops.put("remote", "/icubSim/" + self.right_motors)
# create remote driver
self.left_armDriver = yarp.PolyDriver(self.left_motorprops)
print('left motor driver prepared')
# query motor control interfaces
self.left_iPos = self.left_armDriver.viewIPositionControl()
self.left_iVel = self.left_armDriver.viewIVelocityControl()
self.left_iEnc = self.left_armDriver.viewIEncoders()
self.left_iCtlLim = self.left_armDriver.viewIControlLimits()
self.right_armDriver = yarp.PolyDriver(self.right_motorprops)
print('right motor driver prepared')
# query motor control interfaces
self.right_iPos = self.right_armDriver.viewIPositionControl()
self.right_iVel = self.right_armDriver.viewIVelocityControl()
self.right_iEnc = self.right_armDriver.viewIEncoders()
self.right_iCtlLim = self.right_armDriver.viewIControlLimits()
# number of joints
self.num_joints = self.left_iPos.getAxes()
print('Num joints: ', self.num_joints)
self.left_limits = []
self.right_limits = []
for i in range(self.num_joints):
left_min =yarp.DVector(1)
left_max =yarp.DVector(1)
self.left_iCtlLim.getLimits(i, left_min, left_max)
print('lim left ', i, ' ', left_min[0], ' ', left_max[0])
self.left_limits.append([left_min[0], left_max[0]])
right_min =yarp.DVector(1)
right_max =yarp.DVector(1)
self.right_iCtlLim.getLimits(i, right_min, right_max)
print('lim right ', i, ' ', right_min[0], ' ', right_max[0])
self.right_limits.append([right_min[0], right_max[0]])
self.go_to_starting_pos()
moduleName = rf.check("name", yarp.Value("BabblingModule")).asString()
self.setName(moduleName)
print('module name: ',moduleName)
yarp.delay(1.0)
print('starting now')
