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')