def doBlocking(self, move, lastMove=False):
"""Same as do, but blocking. :lastmove: determines whether to affect its value (default: False)."""
global lockDo, sharedDo, sharedTimeout
global lockDone, sharedDone
global lockIsrunning
if self.config.motion != None:
lockIsrunning.acquire()
lockDo.acquire()
lockLookat.acquire()
if lastMove:
sharedLookat = None
sharedDo = None
lockDone.acquire()
sharedDone = move
lockDone.release()
try:
if self.config.isRecording:
self.exportFile.write(
time.strftime("%d.%m.%Y %H:%M:%S") + " doBlocking(" +
move + ", " + str(lastMove) + ")\n")
timeout = sharedTimeout - time.time()
if timeout > 0:
time.sleep(timeout)
sharedTimeout = time.time() + 100
movement = {'names': [], 'times': [], 'keys': []}
if self.config.isDebug:
print("Doing blocking move: ", move)
mvdata = mvdb.get(move)
if mvdata:
movement = mvdata
else:
self.config.posture.goToPosture(move, 0.2)
time.sleep(0.2)
if movement['names'] != []:
if "HeadYaw" in movement["names"]:
self.config.motion.setAngles(
"HeadYaw",
self.config.motion.getAngles("HeadYaw", True),
0.05)
if "HeadPitch" in movement["names"]:
self.config.motion.setAngles(
"HeadPitch",
self.config.motion.getAngles("HeadPitch", True),
0.05)
self.config.motion.angleInterpolationBezier(
movement['names'], movement['times'],
movement['keys']) # blocking
time.sleep(movement['timeout'])
finally:
sharedTimeout = time.time()
lockDo.release()
lockLookat.release()
lockIsrunning.release()
def run(self):
global lockTimeout, sharedTimeout
global lockDo, sharedDo
global lockDone, sharedDone
global lockLookat, sharedLookat
global lockIsrunning, sharedIsrunning
lockIsrunning.acquire()
while sharedIsrunning:
lockIsrunning.release()
# Make copy of shared arrays
lockDo.acquire()
localDo = sharedDo
sharedDo = None
lockDo.release()
lockTimeout.acquire()
timeout = sharedTimeout - time.time()
lockTimeout.release()
movement = {'names': [], 'times': [], 'keys': []}
if localDo != None:
if self.config.isDebug:
print("Doing move: ", localDo)
mvdata = mvdb.get(localDo)
if mvdata:
movement = mvdata
elif sharedTimeout - time.time() <= 0:
self.config.posture.goToPosture(localDo, 0.2)
sharedTimeout = 0.2
if movement['names'] != [] and timeout <= 0:
lockDone.acquire()
sharedDone = localDo
lockDone.release()
if "HeadYaw" in movement["names"]:
self.config.motion.setAngles(
"HeadYaw",
self.config.motion.getAngles("HeadYaw", True), 0.05)
if "HeadPitch" in movement["names"]:
self.config.motion.setAngles(
"HeadPitch",
self.config.motion.getAngles("HeadPitch", True), 0.05)
sharedTimeout = time.time(
) + movement['times'][0][-1] + movement['timeout']
self.config.motion.angleInterpolationBezier(
movement['names'], movement['times'],
movement['keys']) # blocking
sharedTimeout = time.time() + movement['timeout']
time.sleep(0.03)
lockIsrunning.acquire()
lockIsrunning.release()
# sit and relax
self.config.motion.rest()
def run(self):
global lockTimeout, sharedTimeout
global lockDone, sharedDone
global lockLookat, sharedLookat
global lockIsrunning, sharedIsrunning
global sharedLLeg # does not need lock, changed only on startup
lockIsrunning.acquire()
while sharedIsrunning:
lockIsrunning.release()
lockLookat.acquire()
localLookat = sharedLookat
sharedLookat = None
lockLookat.release()
lockTimeout.acquire()
timeout = sharedTimeout - time.time()
lockTimeout.release()
lockDone.acquire()
localDone = sharedDone
lockDone.release()
# Get transformation from torso to head (only Z is non-0 and without rotation)
torsoToHead = self.config.motion.getPosition(
"Head", 0,
True)[2] + 0.03 # getPosition returns quaternion format
# When moving, load its sequence into movement, so lookAt knows whether it can be used
movement = {'names': []}
if localDone != None:
mvdata = mvdb.get(localDone)
if mvdata:
movement = mvdata
# determine look-at angles
if not localLookat is None and (
(not "HeadYaw" in movement["names"]
and not "HeadPitch" in movement["names"]) or timeout < 0):
localLookat[2] -= torsoToHead # Now at head position
yawGoal = math.atan2(localLookat[1], localLookat[0])
pitchGoal = -math.atan2(localLookat[2], localLookat[0])
# From-leg calibration
pitchGoal += self.config.memory.getData(
"Device/SubDeviceList/LHipPitch/Position/Sensor/Value"
) - sharedLLeg
# change look-at so it is within eligible limits, yaw is more important (left-right)
limitsYaw = self.config.safeMotion.jointLimits["HeadYaw"]
limitedYawGoal = np.clip(yawGoal, limitsYaw[0], limitsYaw[1])
limitedYawGoal = np.clip(limitedYawGoal, -0.5078, 0.5078)
try:
limitsPitch = self.config.safeMotion.headPitchLimitsAtYaw(
limitedYawGoal)
limitedPitchGoal = np.clip(pitchGoal, limitsPitch[0],
limitsPitch[1])
#if self.config.isDebug:
# print("Looking at: ", limitedYawGoal, limitedPitchGoal, localDone)
angles = self.config.motion.getAngles(
["HeadYaw", "HeadPitch"], True)
a = limitedYawGoal - angles[0]
b = limitedPitchGoal - angles[1]
self.config.motion.setAngles(
["HeadYaw", "HeadPitch"],
[limitedYawGoal, limitedPitchGoal],
math.sqrt(a**2 + b**2) * 0.2) # non-blocking
except NameError: # Yaw not within limits, should never occur, because clip
continue
time.sleep(0.02)
lockIsrunning.acquire()
lockIsrunning.release()