def move(self,
target_joints,
req_time,
joints_list=None,
waitMotionDone=False):
if joints_list is None:
joints_list = self.__joints_list__
disp = [0] * len(joints_list)
speed_head = [0] * len(joints_list)
tmp = yarp.Vector(len(joints_list))
encs = yarp.Vector(16)
while not self.__IEncoders__.getEncoders(encs.data()):
self.__logger__.warning("Data from encoders not available!")
time.sleep(0.1)
i = 0
for j in joints_list:
tmp.set(i, target_joints[i])
disp[i] = target_joints[i] - encs[j]
if disp[i] < 0.0:
disp[i] = -disp[i]
speed_head[i] = disp[i] / req_time
self.__IPositionControl__.setRefSpeed(j, speed_head[i])
self.__IPositionControl__.positionMove(j, tmp[i])
i += 1
if waitMotionDone is True:
self.__waitMotionDone__(timeout=req_time)
def skin_sensor_data_reader(self) -> Tuple[List, List]:
"""Read skin sensor data """
tactile_hand: yarp.Vector = yarp.Vector(192)
self.input_port_skin_hand.read(tactile_hand)
tactile_hand_l = yarp.Vector(192)
self.input_port_skin_hand_l.read(tactile_hand_l)
data_hand: List = []
data_hand_l: List = []
for j in range(len(skin_idx_r_hand)):
#print(skin_idx_hand[j])
if skin_idx_r_hand[j] > 0:
print("read Data:", tactile_hand.get(j))
data_hand.append(tactile_hand.get(j))
for j in range(len(skin_idx_l_hand)):
if skin_idx_l_hand[j] > 0:
print("read Data left:", tactile_hand_l.get(j))
data_hand_l.append(tactile_hand_l.get(j))
print("Data hand:")
print(data_hand)
print(data_hand_l)
time.sleep(0.5)
return data_hand, data_hand_l
def read_and_publish_in_ros(self, verbose=False):
if verbose:
start_time = time.time()
# set ROS timestamp
self.joint_pos_msg.header.stamp = rospy.Time.now()
# lock to this thread
lock.acquire()
for j in range(len(data_keys.JointNames)):
self.current_position = yarp.Vector(self.get_num_joints(j))
self.current_speed = yarp.Vector(self.get_num_joints(j))
# get data from encoders
self.encoders[j].getEncoders(self.current_position.data())
self.encoders[j].getEncoderSpeeds(self.current_speed.data())
# copy values into ROS msg
self.joint_pos_msg = data_keys.set_joint_pos_msg_value(
self.joint_pos_msg, data_keys.JointNames[j],
self.current_position.toString(-1, 1), self.get_num_joints(j))
self.joint_speed_msg = data_keys.set_joint_pos_msg_value(
self.joint_speed_msg, data_keys.JointNames[j],
self.current_speed.toString(-1, 1), self.get_num_joints(j))
# release lock
lock.release()
if verbose:
end_time = time.time()
elapsed = end_time - start_time
print("reading time (seconds): ", elapsed, " time_btw ",
end_time - self.current_time, " time now ", end_time,
" ros_stamp ", self.joint_pos_msg.header.stamp)
self.current_time = end_time
# publish ROS message
self.joint_pos_pub.publish(self.joint_pos_msg)
self.joint_speed_pub.publish(self.joint_speed_msg)
def lookAtFixationPoint(self, x, y, z, waitMotionDone=True):
p = yarp.Vector(3)
p.set(0, x)
p.set(1, y)
p.set(2, z)
angles = yarp.Vector(3)
self.IGazeControl.getAnglesFrom3DPoint(p, angles)
self.__lookAtAbsAngles__(angles, waitMotionDone)
def setJointLimits(self, IControlLimits, numJoints, startingIndex):
for joint in range(numJoints):
min = yarp.Vector(1)
max = yarp.Vector(1)
IControlLimits.getLimits(joint, min.data(), max.data())
self.q_min[startingIndex + joint] = min.get(0)
self.q_max[startingIndex + joint] = max.get(0)
self.q_min[0] = -31.0
self.q_max[0] = 31.0
self.q_max[1] = 16.5
def test_vector_copy_costructor(self):
vecSize = 10
vec = yarp.Vector(vecSize)
self.assertEqual(vec.size(), vecSize)
vecCheck = yarp.Vector(vec)
self.assertEqual(vec.size(), vecCheck.size())
# Check copy constructor from a Vector
# returned by a function
mat = yarp.Matrix(3, 3)
mat.eye()
vecTest = yarp.Vector(mat.getRow(0))
self.assertEqual(vecTest.size(), 3)
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 lookat(icub, position):
p = yarp.Vector(3)
p.set(0, position[0]) # Azimuth
p.set(1, position[1]) # Elevation
p.set(2, position[2]) # Vergence
icub.gaze.getIGazeControl().lookAtAbsAnglesSync(p)
icub.gaze.getIGazeControl().waitMotionDone(timeout=5.0)
def control_gaze(self, head=(0, 0, 0), eyes=(0, 0, 0)):
"""
Control the iCub head and eyes
:param head: Head coordinates (tilt, swing, pan)
:param eyes: Eye coordinates (tilt, pan, divergence)
:return: None
"""
# wait for the action to complete
# while not self._ipos.checkMotionDone():
# pass
# initialize a new tmp vector identical to encs
self.init_pos = yarp.Vector(self._num_jnts, self._encs.data())
# head control
self.init_pos.set(0, self.init_pos.get(0) + head[0]) # tilt/pitch
self.init_pos.set(1, self.init_pos.get(1) + head[1]) # swing/roll
self.init_pos.set(2, self.init_pos.get(2) + head[2]) # pan/yaw
# eye control
self.init_pos.set(3, self.init_pos.get(3) + eyes[0]) # eye tilt
self.init_pos.set(4, self.init_pos.get(4) + eyes[1]) # eye pan
self.init_pos.set(
5,
self.init_pos.get(5) +
eyes[2]) # the divergence between the eyes (to align, set to 0)
self._ipos.positionMove(self.init_pos.data())
self._curr_head = list(head)
self._curr_eyes = list(eyes)
def test_send_and_recv_setExt(self):
port_in = yarp.Port()
in_port_name = '/test/img:i'
self.assertTrue(port_in.open(in_port_name))
port_out = yarp.Port()
port_out.enableBackgroundWrite(True);
out_port_name = '/test/img:o'
self.assertTrue(port_out.open(out_port_name))
self.assertTrue(yarp.Network.connect(port_out.getName(), port_in.getName()))
yarp.delay(0.5)
height = 240
width = 320
yarp_img_out = yarp.ImageMono()
yarp_img_out.resize(width, height)
yarp_img_in = yarp.ImageMono()
self.assertEqual(240, yarp_img_out.height())
self.assertEqual(320, yarp_img_out.width())
yarp_vector = yarp.Vector()
yarp_vector.resize(320*240, 0)
self.assertTrue(240*320, yarp_vector.size())
yarp_img_out.setExternal(yarp_vector, width, height)
self.assertTrue(port_out.write(yarp_img_out))
yarp.delay(0.5)
self.assertTrue(port_in.read(yarp_img_in))
self.assertEqual(240, yarp_img_in.height())
self.assertEqual(320, yarp_img_in.width())
port_out.close()
port_in.close()
def waitMotionDone(self,
target_angles,
period=WAITMOTION_PERIOD,
timeout=WAITMOTIONDONE_TIMEOUT):
self.__logger__.info(
"""Waiting for motion done STARTED! target_angles: %s""" %
str([target_angles[0], target_angles[1], target_angles[2]]))
angles = yarp.Vector(6)
max_attempts = int(timeout / period)
for _ in range(0, max_attempts):
self.IGazeControl.getAngles(angles)
v = []
w = []
for i in range(0, 6):
v.append(angles[i])
w.append(target_angles[i])
if len(v) != len(w):
break
dist = utils.vector_distance(v, w)
if dist < GazeController.MIN_JOINTS_DIST:
self.__logger__.info(
"""Motion done DETECTED! target_angles: %s""" %
str([target_angles[0], target_angles[1], target_angles[2]
]))
return True
yarp.delay(period)
self.__logger__.warning(
"""Motion done TIMEOUT! target_angles: %s""" %
str([target_angles[0], target_angles[1], target_angles[2]]))
return False
def readEncodersData(self):
encodersData = yarp.Vector(self.numJoints)
ret = self.iEnc.getEncoders(encodersData.data())
if ret is False:
encodersData = self.previousEncodersData
else:
self.previousEncodersData = encodersData
return encodersData
def lookAt3DPoint(self, x, y, z, waitMotionDone=False, timeout=0.0):
p = yarp.Vector(3)
p.set(0, x)
p.set(1, y)
p.set(2, z)
self.__IGazeControl__.lookAtFixationPoint(p)
if waitMotionDone is True:
self.__waitMotionDone__(timeout)
def get(self):
# read encoders
encs = yarp.Vector(self.jnts)
self.iEnc.getEncoders(encs.data())
values = ()
for i in range(self.jnts):
values += (encs.get(i), )
return values
def __lookatrel__(self, position):
with self.__lock__:
p = yarp.Vector(3)
p.set(0, position[0]) # Azimuth
p.set(1, position[1]) # Elevation
p.set(2, position[2]) # Vergence
self.__icub__.gaze.getIGazeControl().lookAtRelAnglesSync(p)
self.__icub__.gaze.getIGazeControl().waitMotionDone(
timeout=self.TIMEOUT_LOOKAT)
def go_to_starting_pos(self):
# starting position (open hand in front on the left camera
start_left = yarp.Vector(self.num_joints)
start_left[0] = -80
start_left[1] = 16
start_left[2] = 30
start_left[3] = 65
start_left[4] = -5 # was -80
start_left[5] = 0
start_left[6] = 0
start_left[7] = 58.8
start_left[8] = 20
start_left[9] = 19.8
start_left[10] = 19.8
start_left[11] = 9.9
start_left[12] = 10.8
start_left[13] = 9.9
start_left[14] = 10.8
start_left[15] = 10.8
start_right = yarp.Vector(self.num_joints)
start_right[0] = -80
start_right[1] = 16
start_right[2] = 30
start_right[3] = 65
start_right[4] = -5 # was -80
start_right[5] = 0
start_right[6] = 0
start_right[7] = 58.8
start_right[8] = 20
start_right[9] = 19.8
start_right[10] = 19.8
start_right[11] = 9.9
start_right[12] = 10.8
start_right[13] = 9.9
start_right[14] = 10.8
start_right[15] = 10.8
self.left_startingPos = yarp.Vector(self.num_joints, start_left.data())
self.right_startingPos = yarp.Vector(self.num_joints, start_right.data())
print ('left start : ', self.left_startingPos.toString())
print('right start : ', self.right_startingPos.toString())
self.left_iPos.positionMove(self.left_startingPos.data())
self.right_iPos.positionMove(self.right_startingPos.data())
def moveRefVel(self,
req_time,
target_joints,
joints_list=None,
vel_list=None,
waitMotionDone=False):
if joints_list is None:
joints_list = self.__joints_list__
jl = yarp.Vector(len(joints_list))
vl = yarp.Vector(len(vel_list))
i = 0
for j in joints_list:
jl.set(i, target_joints[i])
vl.set(i, vel_list[i])
self.__IPositionControl__.setRefSpeed(j, vl[i])
self.__IPositionControl__.positionMove(j, jl[i])
i += 1
if waitMotionDone is True:
self.__waitMotionDone__(timeout=req_time)
def read_encoders(self):
left_encs = yarp.Vector(self.num_joints)
#right_encs=yarp.Vector(self.num_joints)
self.left_iEnc.getEncoders(left_encs.data())
#self.right_iEnc.getEncoders(right_encs.data())
data = []
for i in range(self.num_joints):
data.append(left_encs.get(i))
print('encoders ', str(data))
self.dataset_joint_encoders.append(deepcopy(data))
def test_vector_copy_costructor(self):
vecSize = 10
vec = yarp.Vector(vecSize)
self.assertEqual(vec.size(), vecSize)
vecCheck = yarp.Vector(vec)
self.assertEqual(vec.size(), vecCheck.size())
# Check copy constructor from a Vector
# returned by a function
mat = yarp.Matrix(3, 3)
mat.eye()
vecTest = yarp.Vector(mat.getRow(0))
self.assertEqual(vecTest.size(), 3)
# Initializer list constructor
vec2 = yarp.Vector([1.0, 2.0, 3.0])
self.assertEqual(vec2.size(), 3)
self.assertEqual(vec2.get(0), 1.0)
self.assertEqual(vec2.get(1), 2.0)
self.assertEqual(vec2.get(2), 3.0)
def move(self,
target_joints,
req_time,
joints_list=None,
waitMotionDone=True):
self.__logger__.info("""Moving joints STARTED!
target_joints:%s
req_time:%.2f,
joints_list=%s,
waitMotionDone=%s""" %
(str(target_joints), req_time, str(joints_list),
str(waitMotionDone)))
if joints_list is None:
joints_list = self.__joints_list__
disp = [0] * len(joints_list)
speed = [0] * len(joints_list)
tmp = yarp.Vector(len(joints_list))
encs = yarp.Vector(16)
while not self.__IEncoders__.getEncoders(encs.data()):
yarp.delay(0.005)
i = 0
for j in joints_list:
tmp.set(i, target_joints[i])
disp[i] = target_joints[i] - encs[j]
if disp[i] < 0.0:
disp[i] = -disp[i]
speed[i] = disp[i] / req_time
self.__IPositionControl__.setRefSpeed(j, speed[i])
self.__IPositionControl__.positionMove(j, tmp[i])
i += 1
if waitMotionDone is True:
self.waitMotionDone(target_joints,
joints_list,
timeout=2 * req_time)
self.__logger__.info("""Moving joints COMPLETED!
target_joints:%s
req_time:%.2f,
joints_list=%s,
waitMotionDone=%s""" %
(str(target_joints), req_time, str(joints_list),
str(waitMotionDone)))
def __init__(self, ArmSide: str = "right_arm", CurrCoordinates: L = None):
# --------------------------------------------------------------
# ------------------- Init Arm Side and Joints -----------------
if (ArmSide == "right_arm"):
self.__HandSide: Tuple = "Right Hand", "RightHand"
elif (ArmSide == "left_arm"):
self.__HandSide: Tuple = "Left Hand", "LeftHand"
# ------------------------------------------------------------------
# print('-------------------- Init YARP variables ---------------------')
self._T_r2w: Transfermat_robot2world = Transfermat_robot2world
self._T_w2r: Transfermat_world2robot = Transfermat_world2robot
self._orient_hand: yarp = mot.npvec_2_yarpvec(orientation_robot_hand)
self._pos: yarp.Vector = yarp.Vector(3)
self._orient: yarp.Vector = yarp.Vector(4)
# ------------------ Init cartesian controller for arm --------------
# ----------------- Prepare a property object --------------------
# print("-------------- Create remote driver: Driver-----------------")
# print("------------- Query motor control interfaces: iCart and iEnc----------")
self.__iCart, self.__driver = motor_init_cartesian(ArmSide) # "right_arm" or "left_arm"
# print(f"{self.__HandSide[0]} (driver, iCart):", self.__driver, self.__iCart)
# ------------------------------ create a holder variable ---------------------
try:
self.__HandCoordinatesHolder: List[List[Dict]] = [self.__initHandCoordinates()]
except Exception as e:
logging.info(e)
if not (CurrCoordinates):
print("Usage of the class is appropiate for countinous hand movement")
elif (CurrCoordinates):
print("Usage of the class takes appropiately just a final coordinates")
if(isinstance(CurrCoordinates, List)):
CurrCoordinates: np.ndarray = np.array(CurrCoordinates)
self.__moveToGivenPositionCoordinates(CurrCoordinates)
def get_joint_pos_limits():
# Initialise YARP
yarp.Network.init()
joint_limits = []
for j in range(len(data_keys.JointNames)):
props = yarp.Property()
props.put("device", "remote_controlboard")
props.put("local", "/client_lim/" + data_keys.JointNames[j])
props.put("remote", "/icubSim/" + data_keys.JointNames[j])
joint_driver = yarp.PolyDriver(props)
ctrl_lim = joint_driver.viewIControlLimits()
limits_low = yarp.Vector(joint_driver.viewIPositionControl().getAxes())
limits_up = yarp.Vector(joint_driver.viewIPositionControl().getAxes())
ctrl_lim.getLimits(j, limits_low.data(), limits_up.data())
joint_limits.append([
np.fromstring(limits_low.toString(-1, 1), dtype=float, sep=' '),
np.fromstring(limits_up.toString(-1, 1), dtype=float, sep=' ')
])
return joint_limits
def np_to_yarp(v):
if len(v.shape) == 1: # Vector
yarp_vec = yarp.Vector(v.shape[0])
for i in range(v.shape[0]):
yarp_vec[i] = v[i]
return yarp_vec
elif len(v.shape) == 2: # Matrix
yarp_mat = yarp.Matrix(v.shape[0], v.shape[1])
for i in range(v.shape[0]):
for j in range(v.shape[1]):
yarp_mat[i, j] = v[i, j]
return yarp_mat
else:
raise Exception("Only 1D or 2D numpy arrays are supported")
def moveRefVel(self,
req_time,
target_joints,
joints_list=None,
vel_list=None,
waitMotionDone=True):
self.__logger__.info(
"""Moving joints in position control STARTED!
target_joints:%s
req_time:%.2f,
vel_list=%s,
waitMotionDone=%s""" %
(str(target_joints), req_time, str(vel_list), str(waitMotionDone)))
if joints_list is None:
joints_list = self.__joints_list__
jl = yarp.Vector(len(joints_list))
vl = yarp.Vector(len(vel_list))
i = 0
for j in joints_list:
jl.set(i, target_joints[i])
vl.set(i, vel_list[i])
self.__IPositionControl__.setRefSpeed(j, vl[i])
self.__IPositionControl__.positionMove(j, jl[i])
i += 1
if waitMotionDone is True:
self.waitMotionDone(target_joints,
joints_list,
timeout=2 * req_time)
self.__logger__.debug(
"""Moving joints COMPLETED!
target_joints:%s
req_time:%.2f,
vel_list=%s,
waitMotionDone=%s""" %
(str(target_joints), req_time, str(vel_list), str(waitMotionDone)))
def go_to_starting_pos(self):
start_head = yarp.Vector(self.head_num_joints)
start_head[0] = -25
start_head[1] = 0
start_head[2] = 40
start_head[3] = 0
start_head[4] = 0
start_head[5] = 0
# starting position (open hand in front on the left camera
start_left = yarp.Vector(self.num_joints)
start_left[0] = -80 # l_shoulder_pitch
start_left[1] = 16 # l_shoulder_roll
start_left[2] = 30 # l_shoulder_yaw
start_left[3] = 65 # l_elbow
start_left[4] = -5 # was -80 # l_wrist_prosup
start_left[5] = 0 # l_wrist_pitch
start_left[6] = 0 # l_wrist_yaw
start_left[7] = 58.8 # l_hand_finger adduction/abduction
start_left[8] = 20 # l_thumb_oppose
start_left[9] = 19.8 # l_thumb_proximal
start_left[10] = 19.8 # l_thumb_distal
start_left[11] = 9.9 # l_index_proximal
start_left[12] = 10.8 # l_index_distal
start_left[13] = 9.9 # l_middle_proximal
start_left[14] = 10.8 # l_middle_distal
start_left[15] = 10.8 # l_pinky
start_right = yarp.Vector(self.num_joints)
start_right[0] = -40
start_right[1] = 16
start_right[2] = 70
start_right[3] = 80
start_right[4] = -5 # was -80
start_right[5] = 0
start_right[6] = 0
start_right[7] = 58.8
start_right[8] = 20
start_right[9] = 19.8
start_right[10] = 19.8
start_right[11] = 9.9
start_right[12] = 10.8
start_right[13] = 9.9
start_right[14] = 10.8
start_right[15] = 10.8
self.head_startingPos = yarp.Vector(self.head_num_joints,
start_head.data())
self.left_startingPos = yarp.Vector(self.num_joints, start_left.data())
self.right_startingPos = yarp.Vector(self.num_joints,
start_right.data())
self.head_iPos.positionMove(self.head_startingPos.data())
if not hold_hands:
self.left_iPos.positionMove(self.left_startingPos.data())
self.right_iPos.positionMove(self.right_startingPos.data())
def waitMotionDone(self, target_joints, joints_list, timeout):
self.__logger__.info("""Waiting for motion done STARTED!""")
encs = yarp.Vector(16)
max_attempts = int(timeout / PositionController.WAITMOTIONDONE_PERIOD)
for _ in range(0, max_attempts):
while not self.__IEncoders__.getEncoders(encs.data()):
yarp.delay(0.05)
v = []
for j in joints_list:
v.append(encs[j])
dist = utils.vector_distance(v, target_joints)
if dist < PositionController.MIN_JOINTS_DIST:
self.__logger__.info("""Motion done DETECTED!""")
return True
yarp.delay(PositionController.WAITMOTIONDONE_PERIOD)
self.__logger__.warning("""Motion done TIMEOUT!""")
return False
def set(self,values=(), \
joint0=None,joint1=None,joint2=None,joint3=None,joint4=None,joint5=None,joint6=None,joint7=None, \
joint8=None,joint9=None,joint10=None,joint11=None,joint12=None,joint13=None,joint14=None,joint15=None):
# read encoders
encs = yarp.Vector(self.jnts)
self.iEnc.getEncoders(encs.data())
# adjust joint positions
for i in range(min(self.jnts, len(values))):
if values[i] != None:
encs.set(i, values[i])
for i in range(16):
value = eval('joint' + str(i))
if value != None:
print('joint', i, '=', value)
encs.set(i, value)
# write to motors
self.iPos.positionMove(encs.data())
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 __skin_sensor_data_reader_left(self) -> A:
"""Read skin sensor data from the left hand """
# print('---------------left {} with {} sensors-------------------'.format(self.flag, self.numSensors))
tactile_arm_l: yarp.Vector = yarp.Vector(self.numSensors)
while (not self.__input_port_skin_left.read(tactile_arm_l)):
print("none left conection!")
yarp.delay(0.001)
#self.__input_port_skin_left.read(tactile_arm_l)
#self.__input_port_skin_left.read(tactile_arm_l)
data_hand_l: List = []
for j in range(len(self.__idx_skin_left)):
if self.__idx_skin_left[j] > 0:
#print("read Data Left: {}".format(j), tactile_hand_l.get(j))
data_hand_l.append(tactile_arm_l.get(j))
# print("Data left {}:".format(self.flag))
# print(data_hand_l)
# time.sleep(0.5)
return data_hand_l
