def __init__(self, simulation=True, face=False):
if simulation == True:
self.robot_type = ROBOT_TYPE.ICUB_SIMULATOR
else:
self.robot_type = ROBOT_TYPE.ICUB
self.__icub__ = iCub(self.robot_type, logtype=YarpLogger.DEBUG)
self.__lock__ = threading.Lock()
self.__rightarm__ = self.__icub__.getPositionController(
ICUB_PARTS.RIGHT_ARM)
self.__leftarm__ = self.__icub__.getPositionController(
ICUB_PARTS.LEFT_ARM)
self.__icub__.gaze.reset()
self.__icub__.gaze.setTrackingMode(True)
# for the future:
if face:
self.face = True
self.__icub__.emo.neutral()
self.__icub__.face.sendRaw("S48")
else:
self.face = False
self.robot_deciding = False
# Copyright (C) 2019 Davide De Tommaso
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>
import yarp
import time
import sys
sys.path.append('../../')
from pyicub.api.iCubHelper import iCub, ROBOT_TYPE, ICUB_PARTS
yarp.Network.init()
icub = iCub(ROBOT_TYPE.ICUB)
head_ctrl = icub.getPositionController(ICUB_PARTS.HEAD)
head_ctrl.move(target_joints=[-15.0, 0.0, 0.0, 0.0, 0.0, 5.0], req_time=1.0)
return True
return False
def cb1():
global single_event_detected
if not single_event_detected:
print("Watching at ZERO detected!")
single_event_detected = True
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)
icub = iCub(ROBOT_TYPE.ICUB, logtype="DEBUG")
watch_zero = iCubWatcher("/iKinGazeCtrl/angles:o", activate_function=af1, callback=cb1)
lookat(icub, UP)
watch_zero.start()
for _ in range(0,3):
single_event_detected = False
lookat(icub, DOWN)
single_event_detected = False
lookat(icub, UP)
watch_zero.stop()
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>
import yarp
import time
import sys
sys.path.append('../../')
from pyicub.api.iCubHelper import iCub, ROBOT_TYPE, ICUB_PARTS
yarp.Network.init()
icub = iCub(ROBOT_TYPE.ICUB_SIMULATOR, logtype="DEBUG")
rightarm_ctrl = icub.getPositionController(ICUB_PARTS.RIGHT_ARM)
torso_ctrl = icub.getPositionController(ICUB_PARTS.TORSO)
icub.gaze.setTrackingMode(True)
icub.gaze.lookAt3DPoint(-0.2, 0.2, 1.0)
torso_ctrl.move(target_joints=[5.0, -4.0, -2.0], req_time=1.0)
rightarm_ctrl.move(target_joints=[10.06, 99.47, 5.31, 102.67, -13.50, -4.21],
req_time=1.0,
joints_list=[0, 1, 2, 3, 4, 5],
waitMotionDone=True)
rightarm_ctrl.move(target_joints=[9.97, 99.63, 6.34, 97.23, -10.95, -3.11],
req_time=1.0,
joints_list=[0, 1, 2, 3, 4, 5],
waitMotionDone=True)
rightarm_ctrl.move(target_joints=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],