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