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 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):
yarp.RFModule.__init__(self)
# Right Arm device
self.rightArmOptions = yarp.Property()
self.rightArmDevice = None
self.rightArmIEncoders = None
self.numRightArmJoints = 0
self.rightArmIPositionControl = None
self.rightArmIPositionControl = None
self.rightArmIControlMode = None
self.rightArmIPositionDirect = None
self.rightArmIRemoteVariables = False
self.rigthArmIControlLimits = None
# Trunk device
self.trunkOptions = yarp.Property()
self.trunkDevice = None
self.trunkIEncoders = None
self.numTrunkJoints = 0
self.trunkIPositionControl = None
self.trunkIControlMode = None
self.trunkIPositionDirect = None
self.trunkIRemoteVariables = False
self.trunkIControlLimits = None
# tts client
self.rpcClientTts = yarp.RpcClient()
# Trunk and Right Arm solver device
self.trunkRightArmSolverOptions = yarp.Property()
self.trunkRightArmSolverDevice = None
self.jointsGoalPositionPort = yarp.BufferedPortBottle()
self.demo_object_input_port_name = "/executeTrajectoryDMP/goalJoints:i"
self.q_min = []
self.q_max = []
self.dmp = None
self.dt = 0.001
self.execution_time = 1.0
self.n_features = 50
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)
################ Import parameter from parameter file ################
from example_parameter import CLIENT_PREFIX, ROBOT_PREFIX
######################################################################
######################### Init YARP network ##########################
######################################################################
#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()
def __getRobotPartProperties__(self, robot_part):
props = yarp.Property()
props.put("device", "remote_controlboard")
props.put("local", "/client/" + self.__robot__ + "/" + robot_part.name)
props.put("remote", "/" + self.__robot__ + "/" + robot_part.name)
return props
robot = '/teo'
from time import sleep
import thread
# YARP
import yarp # imports 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()
#-- 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(
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
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')
color = [0.5, 1, 0]
thick_dirt = thick / 2.0
dim_dirt = [0.05, (thick / 2) + (thick_dirt / 2.0), 0.05]
pos_dirt = pos + [0.05, thick_dirt, 0.0]
color_dirt = [0.6, 0.15 , 0.2]
white_table = wc.create_object('sbox', dim, pos, color)
red_square = wc.create_object('sbox', dim_dirt, pos_dirt, color_dirt)
raw_input('Press enter to continue: ')
raw_input('Press enter to continue: ')
# prepare a property object
props = yarp.Property()
props.put("device","remote_controlboard")
props.put("local","/client/right_arm")
props.put("remote","/icubSim/right_arm")
props_left = yarp.Property()
props_left.put("device","remote_controlboard")
props_left.put("local","/client/left_arm")
props_left.put("remote","/icubSim/left_arm")
props_torso = yarp.Property()
props_torso.put("device","remote_controlboard")
props_torso.put("local","/client/torso")
props_torso.put("remote","/icubSim/torso")
propsKC = yarp.Property()
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)
def generate_3d_positions(args):
# Load yarp ResourceFinder
rf = yarp.ResourceFinder();
rf.setVerbose();
# Set the same default context of the iCubSkinGui for convenience loading
# .ini position containing the 2D location of the patches
rf.setDefaultContext("skinGui/skinGui");
prop = yarp.Property();
skinGuiConfFile = args.skinGui_conf_file[0];
skinGuiConfFullName = rf.findFileByName(skinGuiConfFile);
prop.fromConfigFile(skinGuiConfFullName);
print("Reading 2D taxel positions from " + skinGuiConfFullName)
sens_group = prop.findGroup("SENSORS")
completePart = triangleCluster();
maxTriangleNumber = -100;
completePart.trianglesNumbersList = [];
for i in range(1,sens_group.size()):
triangle_group = sens_group.get(i).asList();
triangle = {}
triangle["type"] = triangle_group.get(0).asString();
triangle["number"] = triangle_group.get(1).asInt();
maxTriangleNumber = max(triangle["number"],maxTriangleNumber);
completePart.trianglesNumbersList.append(triangle["number"]);
triangle["u"] = triangle_group.get(2).asInt();
triangle["v"] = triangle_group.get(3).asInt();
triangle["orient"] = triangle_group.get(4).asInt();
triangle["gain"] = triangle_group.get(5).asInt();
triangle["mirror"] = triangle_group.get(6).asInt();
completePart.triangles[triangle["number"]] = triangle
# taxel positions in triangle frame (expressed in millimeters)
taxelsPosInTriangle = []
# taxel 0
taxelsPosInTriangle.append(np.array([6.533, 0.0]))
# taxel 1
taxelsPosInTriangle.append(np.array([9.8, -5.66]))
# taxel 2
taxelsPosInTriangle.append(np.array([3.267, -5.66]))
# taxel 3
taxelsPosInTriangle.append(np.array([0.0, 0.0]))
# taxel 4
taxelsPosInTriangle.append(np.array([-3.267, -5.66]))
# taxel 5
taxelsPosInTriangle.append(np.array([-9.8, -5.66]))
# taxel 6 (thermal pad!)
taxelsPosInTriangle.append(np.array([-6.533, -3.75]))
# taxel 7
taxelsPosInTriangle.append(np.array([-6.533, 0]))
# taxel 8
taxelsPosInTriangle.append(np.array([-3.267, 5.66]))
# taxel 9
taxelsPosInTriangle.append(np.array([0.0, 11.317]))
# taxel 10 (thermal pad)
taxelsPosInTriangle.append(np.array([0, 7.507]))
# taxel 11
taxelsPosInTriangle.append(np.array([3.267, 5.66]))
# generate taxel list, we allocate a list of the total number of triangles
# dummy values (for the foot are 32) and then we overwrite the taxels
# for the real triangles
dummy_taxel = get_dummy_taxel();
# the total number of the triangles is composed by both real triangles
# and dummy triangles, is given by the length of the yarp vector published
# on the port, divided by 12 (for the torso: 384/12 = 32).
# Alternativly the total number of triangle for a skin part can be computed
# from the number of patches present in the skin part: each skin patch contains
# (a maximum of) 16 triangles, and for each skin patch 16*12 = 192 taxels are always
# streamed in the YARP ports, regardless of the actual presense of a physical triangle
# in the skin, so the total number of triangles is given by number_of_patches*16 .
# For more info see http://wiki.icub.org/wiki/Tactile_sensors_(aka_Skin)
# If we do not know apriori the number of patches in this part, we can easily get
# the total number of triangle from the skinGui configuration file: we just need to find the triangle with
# the maximum number, and then find the lowest multiple of 16 bigger then the maximum triangle number (plus one
# because the triangle number is 0-based
total_number_of_patches = (maxTriangleNumber/16)+1;
total_number_of_triangles = total_number_of_patches*16;
# number of taxels for triangle
taxel_per_triangle = 12
# list of taxels (from 0 to taxel_per_triangle) that are thermal
thermal_taxels_list = [6,10]
# taxel that is the center of the triangle
center_taxel = 3;
# pre-populate the taxels vector with all dummy taxels
taxelsList = total_number_of_triangles*taxel_per_triangle*[dummy_taxel]
for triangleNumber in completePart.triangles:
triangle = completePart.triangles[triangleNumber];
for i in range(0,taxel_per_triangle):
theta = np.pi*triangle["orient"]/180
rotMatrix = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
offset = rotMatrix.dot(taxelsPosInTriangle[i])
taxel = {}
# index of the taxel in the skin part YARP port
taxel["index"] = triangle["number"]*taxel_per_triangle+i;
taxel["triangleNumber"] = triangle["number"]
if( i in thermal_taxels_list ):
taxel["type"] = "thermal"
# u,v are the coordinates in millimeters of the taxels in
# the iCubSkinGui
# compute the offset of the taxel with respect to the triangle center
taxel["u"] = triangle["u"] + offset[0]
taxel["v"] = triangle["v"] + offset[1]
# the taxel x, y, z position in skin frame will be filled by
# the interpolation procedure
else:
taxel["type"] = "tactile"
taxel["u"] = triangle["u"] + offset[0]
taxel["v"] = triangle["v"] + offset[1]
taxel["x"] = None
taxel["y"] = None
taxel["z"] = None
taxelsList[taxel["index"]] = taxel
completePart.taxels = taxel_list_to_taxel_dict(taxelsList);
# Get triangle centers from CAD (passing through the URDF)
centersAndNormals = get_triangle_centers_from_urdf(args.urdf[0],args.link[0],args.skin_frame[0],completePart.trianglesNumbersList);
# Build interpolation cluster (for now depending on indipendent_patches switch)
interpolationClusters = [];
if( args.indipendent_patches ):
for patchId in range(0,total_number_of_patches):
interpolationClusters.append(completePart.getTrianglesInPatch(patchId))
else:
interpolationClusters.append(completePart)
completePartUnknownX = [];
completePartUnknownY = [];
completePartUnknownZ = [];
completePartCentersX = [];
completePartCentersY = [];
completePartCentersZ = [];
completePartNormX = [];
completePartNormY = [];
completePartNormZ = [];
for cluster in interpolationClusters:
trainingPointsU = []
trainingPointsV = []
unknownPointsU = []
unknownPointsV = []
valuesX = []
valuesY = []
valuesZ = []
for triangleNumber in cluster.triangles:
trainingPointsU.append(cluster.triangles[triangleNumber]["u"]);
trainingPointsV.append(cluster.triangles[triangleNumber]["v"]);
valuesX.append(centersAndNormals['centers'][triangleNumber][0])
valuesY.append(centersAndNormals['centers'][triangleNumber][1]);
valuesZ.append(centersAndNormals['centers'][triangleNumber][2]);
for taxelId in cluster.taxels:
taxel = cluster.taxels[taxelId];
unknownPointsU.append(taxel["u"]);
unknownPointsV.append(taxel["v"]);
assert(len(trainingPointsU) == len(trainingPointsV))
assert(len(trainingPointsU) == len(valuesX))
assert(len(trainingPointsU) == len(valuesY))
assert(len(trainingPointsU) == len(valuesZ))
ret = interpolate_using_griddata(trainingPointsU,trainingPointsV,valuesX,valuesY,valuesZ,unknownPointsU,unknownPointsV,cluster.taxels,centersAndNormals,cluster.taxel_offset);
# Plot results
if( args.plot ):
plot_points_in_3d(valuesX, valuesY, valuesZ, ret.unknownX, ret.unknownY, ret.unknownZ);
# Append the results
completePartUnknownX.extend(ret.unknownX);
completePartUnknownY.extend(ret.unknownY);
completePartUnknownZ.extend(ret.unknownZ);
completePartNormX.extend(ret.normX);
completePartNormY.extend(ret.normY);
completePartNormZ.extend(ret.normZ);
completePartCentersX.extend(valuesX);
completePartCentersY.extend(valuesY);
completePartCentersZ.extend(valuesZ);
# Export results
exportSkinManagerPositionTxtFile(completePart.taxels,completePartUnknownX,completePartUnknownY,completePartUnknownZ,completePartNormX,completePartNormY,completePartNormZ,args.link[0],args.skinManager_conf_file[0],taxel_per_triangle,center_taxel);
rotate_cmd = yarp.Bottle()
rotate_cmd.clear()
rotate_cmd.addString("world")
rotate_cmd.addString("rot")
rotate_cmd.addString("smodel")
rotate_cmd.addInt(mark_id + 1)
rotate_cmd.addDouble(rotation[0])
rotate_cmd.addDouble(rotation[1])
rotate_cmd.addDouble(rotation[2])
print(rotate_cmd.toString())
if self._is_success(self._execute(rotate_cmd)):
print('rotated marker ', str(mark_id))
wc = WorldController()
config = yarp.Property()
config.fromConfigFile('/code/icub_perceptual_optimisation/yarp/config.ini')
max_num_objects = config.findGroup('GENERAL').find('max_num_objects').asInt32()
if many_balls:
max_num_objects = max_num_objects * 3
# make marker objects
cmd = yarp.Bottle()
cmd.clear()
cmd.addString("world")
cmd.addString("set")
cmd.addString("mdir")
cmd.addString("/code/icub_perceptual_optimisation/yarp/data/markers")
if wc._is_success(wc._execute(cmd)):
print('changed model folder')
# create marker objects
rf.setVerbose(True)
rf.setDefaultContext("myContext")
rf.setDefaultConfigFile("default.ini")
# trunkRightArmOptions = yarp.Property()
# 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')
V_rad[i] = V[i] * icub.CTRL_DEG2RAD
return V_rad
yarp.Network.init()
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))
#! /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]
#!/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
def from_yarp_rgb_Image_to_numpy_array(yarp_image, numpy_array):
for l1 in range(0, yarp_image.height()):
for l2 in range(0, yarp_image.width()):
currPixel = yarp.PixelRgb()
currPixel = yarp_image.pixel(l2, l1)
numpy_array[l1][l2][0] = currPixel.r
numpy_array[l1][l2][1] = currPixel.g
numpy_array[l1][l2][2] = currPixel.b
# define the height
height = -0.01
# prepare a property object
props = yarp.Property()
props.put("device", "remote_controlboard")
props.put("local", "/client/head")
props.put("remote", "/icubSim/head")
props_torso = yarp.Property()
props_torso.put("device", "remote_controlboard")
props_torso.put("local", "/client/torso")
props_torso.put("remote", "/icubSim/torso")
output_port = yarp.Port()
output_port.open("/dc-image")
input_port = yarp.BufferedPortImageRgb()
input_port.open("/homo_image_read")
try:
# Create environment
RaveInitialize()
env = Environment()
env.Load("data/testwamcamera.env.xml")
# 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
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')
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')