def main(remote_port: 'Remote port running the AravisGigE grabber' = '/grabber' ): # Check for YARP network yarp.Network.init() if not yarp.Network.checkNetwork(): logging.error( 'Could not connect to YARP network. Please try running YARP server.' ) sys.exit(1) # Create and configure driver options = yarp.Property() options.put('device', 'remote_grabber') options.put('remote', remote_port) options.put('local', '/grabberControls2Gui') dd = yarp.PolyDriver(options) # View driver as FrameGrabber controls = dd.viewIFrameGrabberControls2() # Create Qt app app = QtWidgets.QApplication(sys.argv) # Create the widget and show it controller = GrabberControls2GuiBackend(controls) gui = GrabberControls2GuiGUI(controller) gui.show() # Run the app exit_code = app.exec_() dd.close() yarp.Network.fini() # disconnect from the YARP network sys.exit(exit_code)
def __init__(self): self.robot_ip = "localhost" #"192.168.26.135" # Initialise YARP yarp.Network.init() rospy.loginfo("motor_driver connected to yarp") # create ros node rospy.init_node('icub_motors_driver', anonymous=True) # create the subscribers target_pos_sub = rospy.Subscriber( '/icubRos/commands/move_to_joint_pos', JointPositions, self.move_to_joint_pos_callback, queue_size=10) self.props = [] self.joint_drivers = [] # encoders for each joint group,e.g. head, left_arm, etc. self.pos_control = [] # number of joints in each joint group self.num_joint = [] for j in range(len(data_keys.JointNames)): self.props.append(yarp.Property()) self.props[-1].put("device", "remote_controlboard") self.props[-1].put("local", "/client_motor/" + data_keys.JointNames[j]) self.props[-1].put("remote", "/icubSim/" + data_keys.JointNames[j]) self.joint_drivers.append(yarp.PolyDriver(self.props[-1])) self.pos_control.append( self.joint_drivers[-1].viewIPositionControl()) rospy.spin()
def __init__(self, robot, logger): GenericController.__init__(self, logger) self.__props__ = yarp.Property() self.__driver__ = yarp.PolyDriver() self.__props__.put("robot", robot) self.__props__.put("device","gazecontrollerclient") self.__props__.put("local","/gaze_client") self.__props__.put("remote","/iKinGazeCtrl") self.__driver__.open(self.__props__) if not self.__driver__.isValid(): self.__logger__.error('Cannot open GazeController driver!') else: self.__IGazeControl__ = self.__driver__.viewIGazeControl()
def cartesian_ctrl_yarp(): ###################################################################### ################# init variables from parameter-file ################# print('----- Init variables -----') pos = yarp.Vector(3) orient = yarp.Vector(4) ###################################################################### ############## Init cartesian controller for right arm ############### ##################### Prepare a property object ###################### props = yarp.Property() props.put("device", "cartesiancontrollerclient") props.put("remote", "/" + ROBOT_PREFIX + "/cartesianController/right_arm") props.put("local", "/" + CLIENT_PREFIX + "/right_arm") ######################## Create remote driver ######################## driver_rarm = yarp.PolyDriver(props) if not driver_rarm: sys.exit("Motor initialization failed!") ################### Query motor control interfaces ################### iCart = driver_rarm.viewICartesianControl() iCart.setPosePriority("position") time.sleep(1) ############ Move hand to inital position and orientation ############ print('----- Move hand to initial pose -----') welt_pos = pos_hand_world_coord init_hand_pos_r_np = np.dot(T_w2r, welt_pos.reshape((4, 1))).reshape((4,)) init_hand_pos_r_yarp = mot.npvec_2_yarpvec(init_hand_pos_r_np[0:3]) iCart.goToPoseSync(init_hand_pos_r_yarp, orient_hand) iCart.waitMotionDone(timeout=5.0) time.sleep(1) iCart.getPose(pos, orient) print('Hand position:', pos.toString()) print('Hand rientation:', orient.toString()) ############ Move hand to new position and orientation ############ print('----- Move hand to new pose -----') pos_hand_world_coord_new: np.ndarray = np.array([-0.111577, 0.27158, 0.501089, 0.1]) welt_pos_n = pos_hand_world_coord_new new_hand_pos_r_np = np.dot(T_w2r, welt_pos_n.reshape((4, 1))).reshape((4,)) new_hand_pos_r_yarp = mot.npvec_2_yarpvec(new_hand_pos_r_np[0:3]) iCart.goToPoseSync(new_hand_pos_r_yarp, orient_hand) iCart.waitMotionDone(timeout=5.0) time.sleep(1) iCart.getPose(pos, orient) print('Hand position:', pos.toString()) print('Hand orientation:', orient.toString()) time.sleep(5) ###################################################################### ################### Close network and motor cotrol ################### print('----- Close control devices and opened ports -----') driver_rarm.close() yarp.Network.fini()
def __init__(self, app_name, port_name): # prepare a property object self.props = yarp.Property() self.props.put('device', 'remote_controlboard') self.props.put('local', app_name + port_name) self.props.put('remote', port_name) # create remote driver self.armDriver = yarp.PolyDriver(self.props) # query motor control interfaces self.iPos = self.armDriver.viewIPositionControl() #self.iVel = self.armDriver.viewIVelocityControl() self.iEnc = self.armDriver.viewIEncoders() # retrieve number of joints self.jnts = self.iPos.getAxes() print('Controlling', self.jnts, 'joints of', port_name)
def __init__(self): yarp.Network.init() if yarp.Network.checkNetwork() != True: print "[error] Please try running yarp server" quit() robotOptions = yarp.Property() robotOptions.put('device', 'RobotClient') robotOptions.put('name', '/ecroSim') self.robotDevice = yarp.PolyDriver( robotOptions) # calls open -> connects self.motors = asrob_yarp_devices.viewIRobotManager( self.robotDevice) # view the actual interface
def __init__(self, robot, logger=YarpLogger.getLogger()): self.__logger__ = logger self.__props__ = yarp.Property() self.__driver__ = yarp.PolyDriver() self.__props__.put("robot", robot) self.__props__.put("device", "gazecontrollerclient") self.__props__.put("local", "/gaze_client") self.__props__.put("remote", "/iKinGazeCtrl") self.__driver__.open(self.__props__) if not self.__driver__.isValid(): self.__logger__.error('Cannot open GazeController driver!') else: self.__IGazeControl__ = self.__driver__.viewIGazeControl() self.__IGazeControl__.setTrackingMode(False) self.__IGazeControl__.stopControl() self.clearNeck() self.clearEyes()
def get_joint_pos_limits(): # Initialise YARP yarp.Network.init() joint_limits = [] for j in range(len(JointNames)): props = yarp.Property() props.put("device", "remote_controlboard") props.put("local", "/client/" + data_keys.JointNames[j]) props.put("remote", "/icubSim/" + data_keys.JointNames[j]) joint_driver = yarp.PolyDriver(props) encoder = joint_drivers.viewIEncoders() limits = yarp.Vector(joint_driver.viewIPositionControl().getAxes()) limits = encoder.getLimits(limits.data()) joint_limits.append( np.fromstring(limits.toString(-1, 1), dtype=float, sep=' ')) return joint_limits
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 __init__(self, simulation=True): self.__name__ = "iCubController" super(MiddlewareCommunicator, self).__init__() # prepare a property object props = yarp.Property() props.put("device", "remote_controlboard") props.put("local", "/client/head") if simulation: props.put("remote", "/icubSim/head") self.cam_props = { "port_cam": "/icubSim/cam", "port_cam_left": "/icubSim/cam/left", "port_cam_right": "/icubSim/cam/right" } else: props.put("remote", "/icub/head") self.cam_props = { "port_cam": "/icub/cam", "port_cam_left": "/icub/cam/left", "port_cam_right": "/icub/cam/right" } self._curr_eyes = [0, 0, 0] self._curr_head = [0, 0, 0] # create remote driver self._head_driver = yarp.PolyDriver(props) # query motor control interfaces self._ipos = self._head_driver.viewIPositionControl() self._ienc = self._head_driver.viewIEncoders() # retrieve number of joints self._num_jnts = self._ipos.getAxes() print('Controlling', self._num_jnts, 'joints') # read encoders self._encs = yarp.Vector(self._num_jnts) self._ienc.getEncoders(self._encs.data()) # control the listening properties from within the app self.activate_communication("receive_images", "listen")
def __init__(self): # Initialise YARP yarp.Network.init() # create ros node rospy.init_node('icub_sensors_driver', anonymous=True) # create the publishers self.joint_pos_pub = rospy.Publisher( '/icubRos/sensors/joint_positions', JointPositions, queue_size=10) self.joint_speed_pub = rospy.Publisher('/icubRos/sensors/joint_speeds', JointPositions, queue_size=10) self.props = [] self.joint_drivers = [] # encoders for each joint group,e.g. head, left_arm, etc. self.encoders = [] # number of joints in each joint group self.num_joint = [] for j in range(len(data_keys.JointNames)): self.props.append(yarp.Property()) self.props[-1].put("device", "remote_controlboard") self.props[-1].put("local", "/client/" + data_keys.JointNames[j]) self.props[-1].put("remote", "/icubSim/" + data_keys.JointNames[j]) self.joint_drivers.append(yarp.PolyDriver(self.props[-1])) self.encoders.append(self.joint_drivers[-1].viewIEncoders()) # initialise ROS messages self.joint_pos_msg = JointPositions() self.joint_speed_msg = JointPositions() self.current_time = time.time() rate = 10.0 #hertz #rate = rospy.Rate(10) # for some reasons, using rospy rate slows it down a lot (check why) while not rospy.is_shutdown(): self.read_and_publish_in_ros(verbose=False) time.sleep(1 / rate)
parser.add_argument('--target', default=-20.0, type=float, help='target position (deg)') parser.add_argument('--period', default=50, type=int, help='command period (ms)') args = parser.parse_args() yarp.Network.init() if not yarp.Network.checkNetwork(): print('Please start a yarp name server first') raise SystemExit options = yarp.Property() options.put('device', 'remote_controlboard') options.put('local', '/exampleOnlineTrajectoryRemotePush') options.put('remote', args.remote) dd = yarp.PolyDriver(options) if not dd.isValid(): print('Remote device not available') raise SystemExit mode = dd.viewIControlMode() enc = dd.viewIEncoders() posd = dd.viewIPositionDirect() if mode is None or enc is None or posd is None: print('Unable to acquire robot interfaces') raise SystemExit if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT): print('Unable to set position direct mode')
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() #-- Left Arm (LA) optionsLA = yarp.Property( ) # create an instance of Property, a nice YARP class for storing name-value (key-value) pairs optionsLA.put('device', 'remote_controlboard' ) # we add a name-value pair that indicates the YARP device optionsLA.put('remote', robot + '/leftArm') # we add info on to whom we will connect optionsLA.put( 'local', '/demo' + robot + '/leftArm' ) # we add info on how we will call ourselves on the YARP network ddLA = yarp.PolyDriver( optionsLA) # create a YARP multi-use driver with the given options posLA = ddLA.viewIPositionControl( ) # make a position controller object we call 'pos' axesLA = posLA.getAxes() # retrieve number of joints #-- Right Arm (RA) optionsRA = yarp.Property( ) # create an instance of Property, a nice YARP class for storing name-value (key-value) pairs optionsRA.put('device', 'remote_controlboard' ) # we add a name-value pair that indicates the YARP device optionsRA.put('remote', robot + '/rightArm') # we add info on to whom we will connect optionsRA.put( 'local', '/demo' + robot + '/rightArm' ) # we add info on how we will call ourselves on the YARP network ddRA = yarp.PolyDriver(
## @example{lineno} exampleRemoteJr3.py import yarp yarp.Network.init() if yarp.Network.checkNetwork() != True: print('[error] Please try running yarp server') quit() options = yarp.Property() options.put('device', 'analogsensorclient') options.put('remote', '/jr3/ch0:o') options.put('local', '/jr3/ch0:i') dd = yarp.PolyDriver(options) # calls open -> connects if not dd.isValid(): print('Cannot open the device!') quit() iAnalogSensor = dd.viewIAnalogSensor() # The following delay should avoid 0 channels and bad read print('delay(1)') yarp.Time.delay(1) channels = iAnalogSensor.getChannels() print('channels:', channels) # Of course we dislike while(1)
import yarp # imports YARP yarp.Network.init() # connect to YARP network if not yarp.Network.checkNetwork( ): # 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() options = yarp.Property( ) # create an instance of Property, a nice YARP class for storing name-value (key-value) pairs options.put('device', 'remote_controlboard' ) # we add a name-value pair that indicates the YARP device options.put('remote', '/robotName/partName') # we add info on to whom we will connect options.put('local', '/exampleRemoteControlboard' ) # we add info on how we will call ourselves on the YARP network dd = yarp.PolyDriver( options) # create a YARP multi-use driver with the given options if not dd.isValid(): print '[error] Please launch robot side' quit() pos = dd.viewIPositionControl( ) # make a position controller object we call 'pos' vel = dd.viewIVelocityControl( ) # make a velocity controller object we call 'vel' posd = dd.viewIPositionDirect( ) # make a direct position controller object we call 'posd' enc = dd.viewIEncoders() # make an encoder controller object we call 'enc' mode = dd.viewIControlMode( ) # make a operation mode controller object we call 'mode' ll = dd.viewIControlLimits() # make a limits controller object we call 'll'
def configure(self, rf): self.lock = threading.Lock() self.config = yarp.Property() self.config.fromConfigFile( '/code/icub_perceptual_optimisation/yarp/config.ini') self.width = self.config.findGroup('CAMERA').find( 'yarp_width').asInt32() self.height = self.config.findGroup('CAMERA').find( 'yarp_height').asInt32() self.target_width = self.config.findGroup('CAMERA').find( 'target_width').asInt32() self.target_height = self.config.findGroup('CAMERA').find( 'target_height').asInt32() self.max_dataset_size = self.config.findGroup('GENERAL').find( 'max_dataset_size').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("/dataCollector/camera_left") yarp.Network.connect("/icubSim/cam/left", "/dataCollector/camera_left") self.input_port_skin_left_hand = yarp.Port() self.input_port_skin_left_hand.open( "/dataCollector/skin/left_hand_comp") # yarp.Network.connect("/icubSim/skin/left_hand_comp", "/dataCollector/skin/left_hand_comp") self.input_port_skin_left_forearm = yarp.Port() self.input_port_skin_left_forearm.open( "/dataCollector/skin/left_forearm_comp") # yarp.Network.connect("/icubSim/skin/left_forearm_comp", "/dataCollector/skin/left_forearm_comp") self.input_port_command = yarp.Port() self.input_port_command.open("/dataCollector/command") # yarp.Network.connect("/client_babbling/left_arm/command", "/dataCollector/command") #self.input_port_skin.read(False); # clean the buffer #self.input_port_skin.read(False); # clean the buffer # 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.head_motors = 'head' self.head_motorprops = yarp.Property() self.head_motorprops.put("device", "remote_controlboard") self.head_motorprops.put("local", "/client_datacollector/" + 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_datacollector/" + 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_datacollector/" + self.right_motors) #self.right_motorprops.put("remote", "/icubSim/" + self.right_motors) # create remote driver self.head_driver = yarp.PolyDriver(self.head_motorprops) self.head_iEnc = self.head_driver.viewIEncoders() self.head_iPos = self.head_driver.viewIPositionControl() self.left_armDriver = yarp.PolyDriver(self.left_motorprops) self.left_iEnc = self.left_armDriver.viewIEncoders() self.left_iPos = self.left_armDriver.viewIPositionControl() #self.right_armDriver = yarp.PolyDriver(self.right_motorprops) #self.right_iEnc = self.right_armDriver.viewIEncoders() #self.right_iPos = self.right_armDriver.viewIPositionControl() # number of joints self.num_joints = self.left_iPos.getAxes() self.head_num_joints = self.head_iPos.getAxes() moduleName = rf.check("name", yarp.Value("DataCollectorModule")).asString() self.setName(moduleName) print('module name: ', moduleName) self.skin_left_hand_input = yarp.Bottle() self.skin_left_forearm_input = yarp.Bottle() #self.left_command_input = yarp.Bottle() self.left_command_input = yarp.Vector(self.num_joints) self.dataset_timestamps = [] self.dataset_images = [] self.dataset_skin_values = [] self.dataset_joint_encoders = [] self.dataset_motor_commands = [] print('starting now')
import yarp import kinematics_dynamics import numpy as np import scipy.linalg as lin yarp.Network.init() options = yarp.Property() options.put('device','CartesianControlClient') options.put('cartesianRemote','/teo/leftArm/CartesianControl') options.put('cartesianLocal','/cc/teo/leftArm') ddLeft = yarp.PolyDriver(options) options.put('cartesianRemote','/teo/rightArm/CartesianControl') options.put('cartesianLocal','/cc/teo/rightArm') ddRight = yarp.PolyDriver(options) iccLeft = kinematics_dynamics.viewICartesianControl(ddLeft) iccRight = kinematics_dynamics.viewICartesianControl(ddRight) xLeft = yarp.DVector() xRight = yarp.DVector() # Se calcula la FK (cinematica directa) # 6-element vector describing current position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians) iccLeft.stat(xLeft) iccRight.stat(xRight) # Esta matriz representa la matriz homogenea 4x4 que va de 0 a N (0~origen, N~gripper) H_left_0_N = np.eye(4)
#!/usr/bin/env python3 import yarp import roboticslab_vision detectorOptions = yarp.Property() detectorOptions.put("device", "HaarDetector") detectorDevice = yarp.PolyDriver(detectorOptions) if not detectorDevice.isValid(): print("Device not available") raise SystemExit iDetector = roboticslab_vision.viewIDetector(detectorDevice) rf = yarp.ResourceFinder() rf.setDefaultContext("HaarDetector") faceFullName = rf.findFileByName("tests/face-nc.pgm") yarpImgRgb = yarp.ImageRgb() if not yarp.read(yarpImgRgb, faceFullName, yarp.FORMAT_PGM): print("Image file not available") raise SystemExit print("detect()") detectedObjects = yarp.Bottle() if not iDetector.detect(yarpImgRgb, detectedObjects) or detectedObjects.size() == 0: print("Detector failed") raise SystemExit
# trunkRightArmOptions.put('device','remote_controlboard') # trunkRightArmOptions.put('remote','/teoSim/trunkAndRightArm') # trunkRightArmOptions.put('local','/teoSim/trunkAndRightArm') # trunkRightArmDevice = yarp.PolyDriver(trunkRightArmOptions) # calls open -> connects # trunkRightArmDevice.open(trunkRightArmOptions); # if not trunkRightArmDevice.isValid(): # print('Cannot open the device!') # raise SystemExit rightArmOptions = yarp.Property() rightArmOptions.put('device', 'remote_controlboard') rightArmOptions.put('remote', '/teoSim/rightArm') rightArmOptions.put('local', '/teoSim/rightArm') rightArmDevice = yarp.PolyDriver(rightArmOptions) # calls open -> connects rightArmDevice.open(rightArmOptions) if not rightArmDevice.isValid(): print('Cannot open the device!') raise SystemExit trunkOptions = yarp.Property() trunkOptions.put('device', 'remote_controlboard') trunkOptions.put('remote', '/teoSim/trunk') trunkOptions.put('local', '/teoSim/trunk') trunkDevice = yarp.PolyDriver(trunkOptions) # calls open -> connects trunkDevice.open(trunkOptions) if not trunkDevice.isValid(): print('Cannot open the device!') raise SystemExit
icub.init() name = 'test' robot = 'icubSim' arm_name = 'left_arm' # To use yarp log function log = yarp.Log() props = yarp.Property() props.put("device", "remote_controlboard") props.put("local", "/" + name + "/" + arm_name) props.put("remote", "/" + robot + "/" + arm_name) # create remote driver _armDriver = yarp.PolyDriver(props) # query motor control interfaces _iPos = _armDriver.viewIPositionControl() _iEnc_arm = _armDriver.viewIEncoders() # retrieve number of joints _jnts_arm = _iPos.getAxes() log.info('[{:s}] Controlling {:d} joints'.format(name, _jnts_arm)) # Cartesian Controller props_cart = yarp.Property("(device cartesiancontrollerclient)") props_cart.put("remote", ("/" + robot + "/cartesianController/" + arm_name)) props_cart.put("local", ("/" + name + "/cart_ctrl/" + arm_name)) _iCartDev = yarp.PolyDriver(props_cart)
#! /usr/bin/env python import yarp import asrob_yarp_devices yarp.Network.init() if not yarp.Network.checkNetwork(): print "[error] Please try running yarp server" quit() robotOptions = yarp.Property() robotOptions.put('device', 'RobotClient') robotOptions.put('name', '/ecroSim') robotDevice = yarp.PolyDriver(robotOptions) # calls open -> connects robot = asrob_yarp_devices.viewIRobotManager( robotDevice) # view the actual interface print "moveForward(4.3)" robot.moveForward(4.3) print "delay(4.5)" yarp.Time.delay(4.5) # delay in [seconds] print "robot.turnLeft(100.0)" robot.turnLeft(100.0) print "delay(2.0)" yarp.Time.delay(2.0) # delay in [seconds]
) != 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() #-- Trunk (TR) optionsTR = yarp.Property( ) # create an instance of Property, a nice YARP class for storing name-value (key-value) pairs optionsTR.put('device', 'remote_controlboard' ) # we add a name-value pair that indicates the YARP device optionsTR.put('remote', robot + '/trunk') # we add info on to whom we will connect optionsTR.put( 'local', '/demo' + robot + '/trunk') # we add info on how we will call ourselves on the YARP network ddTR = yarp.PolyDriver( optionsTR) # create a YARP multi-use driver with the given options posTR = ddTR.viewIPositionControl( ) # make a position controller object we call 'pos' encTR = ddTR.viewIEncoders() # encoders axesTR = posTR.getAxes() # retrieve number of joints #-- softNeck (SN) optionsSN = yarp.Property( ) # create an instance of Property, a nice YARP class for storing name-value (key-value) pairs optionsSN.put('device', 'remote_controlboard' ) # we add a name-value pair that indicates the YARP device optionsSN.put('remote', robot + '/softNeck') # we add info on to whom we will connect optionsSN.put( 'local', '/demo' + robot + '/softNeck' ) # we add NSNinfo on how we will call ourselves on the YARP network
def __getDriver__(self, robot_part): if not robot_part.name in self.__drivers__.keys(): props = self.__getRobotPartProperties__(robot_part) self.__drivers__[robot_part.name] = yarp.PolyDriver(props) return self.__drivers__[robot_part.name]
# Create viewer env.SetViewer('qtcoin') # Create OpenraveYarpPluginLoader and obtain environment pointer (penv) OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader') penvStr = OpenraveYarpPluginLoader.SendCommand('getPenv') penvNameValueStr = '(penv {' + penvStr + '})' # Controlboard using penv controlboardOptions = yarp.Property() controlboardOptions.fromString(penvNameValueStr) # put('penv',penvValue) controlboardOptions.put('device','YarpOpenraveControlboard') controlboardOptions.put('robotIndex',0) controlboardOptions.put('manipulatorIndex',0) controlboardDevice = yarp.PolyDriver(controlboardOptions) # Create Grabber using penv grabberOptions = yarp.Property() grabberOptions.fromString(penvNameValueStr) # put('penv',penvValue) grabberOptions.put('device','YarpOpenraveGrabber') grabberOptions.put('robotIndex',0) grabberOptions.put('sensorIndex',0) grabberDevice = yarp.PolyDriver(grabberOptions) # View specific interfaces pos = controlboardDevice.viewIPositionControl() controls = grabberDevice.viewIFrameGrabberControls() grabber = grabberDevice.viewIFrameGrabberImage() # do stuff
#mot.motor_init("head", "position", ROBOT_PREFIX, CLIENT_PREFIX) yarp.Network.init() if not yarp.Network.checkNetwork(): sys.exit('[ERROR] Please try running yarp server') ###################################################################### ################## Init motor control for the head ################### print('----- Init head motor control -----') props = yarp.Property() props.put("device", "remote_controlboard") props.put("local", "/" + CLIENT_PREFIX + "/head") props.put("remote", "/" + ROBOT_PREFIX + "/head") # create remote driver Driver_head = yarp.PolyDriver(props) #Driver_arms = yarp.PolyDriver(props) # query motor control interfaces iPos_head = Driver_head.viewIPositionControl() iEnc_head = Driver_head.viewIEncoders() # #iPos_arms = Driver_arms.viewIPositionControl() # retrieve number of joints jnts_head = iPos_head.getAxes() #jnts_arms ###################### Go to head zero position ###################### mot.goto_zero_head_pos(iPos_head, iEnc_head, jnts_head)
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')
orient_hand = mot.npvec_2_yarpvec(orientation_robot_hand) pos = yarp.Vector(3) orient = yarp.Vector(4) ###################################################################### ############## Init cartesian controller for right arm ############### ##################### Prepare a property object ###################### props = yarp.Property() props.put("device", "cartesiancontrollerclient") props.put("remote", "/" + ROBOT_PREFIX + "/cartesianController/right_arm") props.put("local", "/" + CLIENT_PREFIX + "/right_arm") ######################## Create remote driver ######################## Driver_rarm: yarp.PolyDriver = yarp.PolyDriver(props) ################### Query motor control interfaces ################### iCart = Driver_rarm.viewICartesianControl() print(Driver_rarm, iCart) iCart.setPosePriority("position") time.sleep(1) ############ Move hand to inital position and orientation ############ print('----- Move hand to initial pose -----') welt_pos = pos_hand_world_coord init_hand_pos_r_np = np.dot(T_w2r, welt_pos.reshape((4, 1))).reshape((4,)) init_hand_pos_r_yarp = mot.npvec_2_yarpvec(init_hand_pos_r_np[0:3]) iCart.goToPoseSync(init_hand_pos_r_yarp, orient_hand) iCart.waitMotionDone(timeout=5.0)
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
else: print('error: illegal prefix parameter, choose "/teo" or "/teoSim"') quit() yarp.Network.init() if not yarp.Network.checkNetwork(): print('error: please try running yarp server') quit() options = yarp.Property() options.put('device', 'remote_controlboard') options.put('remote', robotPrefix + '/rightArm') options.put('local', playerPrefix + '/rightArm') 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=',')
props_torso.put("device","remote_controlboard") props_torso.put("local","/client/torso") props_torso.put("remote","/icubSim/torso") propsKC = yarp.Property() propsKC.put("device","cartesiancontrollerclient") propsKC.put("local","/clientKC/right_arm") propsKC.put("remote","/icubSim/cartesianController/right_arm") propsKC_left = yarp.Property() propsKC_left.put("device","cartesiancontrollerclient") propsKC_left.put("local","/clientKC/left_arm") propsKC_left.put("remote","/icubSim/cartesianController/left_arm") # create remote driver armDriver = yarp.PolyDriver(props) armDriver_left = yarp.PolyDriver(props_left) torsoDriver = yarp.PolyDriver(props_torso) clientCartCtrl = yarp.PolyDriver(propsKC) clientCartCtrl_left = yarp.PolyDriver(propsKC_left) if (clientCartCtrl.isValid()): icart = clientCartCtrl.viewICartesianControl() icart_left = clientCartCtrl_left.viewICartesianControl() #query motor control interfaces iPos = armDriver.viewIPositionControl() iEnc = armDriver.viewIEncoders() iPos_left = armDriver_left.viewIPositionControl() iEnc_left = armDriver_left.viewIEncoders()