def compress_shoulder(self, function_name, **kwargs):
'''
This function compresses the shoulder (should be in ribs rotation direction)
to help reduce agent width.
:param function_name:
:param kwargs: bone='shoulder.L' OR bone='shoulder.R'
:return:applies the shoulder compression with set_rotation on Morse side
'''
#Check the max rotation
print("Compress Shoulder")
if self.busy:
return
self.busy = True
maxReached = False
minReached = False
kwargs.update(self.function_map[function_name][1])
#if kwargs['bone'] == 'shoulder.R':
# kwargs['radians'] = kwargs['radians'] * 1
if kwargs['bone'] == 'shoulder.L':
kwargs['radians'] = kwargs['radians'] * -1
minR, maxR = self._boneProperties[kwargs['bone']][kwargs['axis']]
#print("MINR", minR)
if Decimal(kwargs['radians']).quantize(
Decimal('0.000'),
rounding=ROUND_HALF_UP) >= Decimal(maxR).quantize(
Decimal('0.000'), rounding=ROUND_HALF_UP):
print("SET TO MAX")
maxReached = True
kwargs['radians'] = maxR
if Decimal(kwargs['radians']).quantize(
Decimal('0.000'),
rounding=ROUND_HALF_UP) <= Decimal(minR).quantize(
Decimal('0.000'), rounding=ROUND_HALF_UP):
print("SET TO MIN")
minReached = True
kwargs['radians'] = minR
print("RADIANS", kwargs['radians'])
middleware.send(self.function_map[function_name][0], **kwargs)
#middleware.send('rotate_torso',axis=axis, radians=radians)
pattern = 'type:proprioception ' + 'bone:' + kwargs['bone']
matcher = Pattern(pattern)
for obj in self._internalChunks:
#if axis='0.0'
if matcher.match(obj) != None:
obj.rotation0 = kwargs['radians']
if kwargs['radians'] < 0:
obj.rotation_direction = 'right'
elif kwargs['radians'] > 0:
obj.rotation_direction = 'left'
if maxReached:
obj.rotation0_quality = 'max'
if minReached:
obj.rotation0_quality = 'min'
self.busy = False
self.update_posture()
def compress_shoulder(self,function_name,**kwargs):
'''
This function compresses the shoulder (should be in ribs rotation direction)
to help reduce agent width.
:param function_name:
:param kwargs: bone='shoulder.L' OR bone='shoulder.R'
:return:applies the shoulder compression with set_rotation on Morse side
'''
#Check the max rotation
print("Compress Shoulder")
if self.busy:
return
self.busy = True
maxReached = False
minReached = False
kwargs.update(self.function_map[function_name][1])
#if kwargs['bone'] == 'shoulder.R':
# kwargs['radians'] = kwargs['radians'] * 1
if kwargs['bone'] == 'shoulder.L':
kwargs['radians'] = kwargs['radians'] * -1
minR,maxR = self._boneProperties[kwargs['bone']][kwargs['axis']]
#print("MINR", minR)
if Decimal(kwargs['radians']).quantize(Decimal('0.000'),rounding=ROUND_HALF_UP) >= Decimal(maxR).quantize(Decimal('0.000'),rounding=ROUND_HALF_UP):
print("SET TO MAX")
maxReached=True
kwargs['radians'] = maxR
if Decimal(kwargs['radians']).quantize(Decimal('0.000'),rounding=ROUND_HALF_UP) <= Decimal(minR).quantize(Decimal('0.000'),rounding=ROUND_HALF_UP):
print("SET TO MIN")
minReached = True
kwargs['radians'] = minR
print("RADIANS",kwargs['radians'])
middleware.send(self.function_map[function_name][0],**kwargs)
#middleware.send('rotate_torso',axis=axis, radians=radians)
pattern='type:proprioception ' + 'bone:' + kwargs['bone']
matcher=Pattern(pattern)
for obj in self._internalChunks:
#if axis='0.0'
if matcher.match(obj)!=None:
obj.rotation0=kwargs['radians']
if kwargs['radians'] < 0:
obj.rotation_direction = 'right'
elif kwargs['radians'] > 0:
obj.rotation_direction = 'left'
if maxReached:
obj.rotation0_quality='max'
if minReached:
obj.rotation0_quality='min'
self.busy=False
self.update_posture()
def rotate_torso(self,function_name,**kwargs):
'''Rotate ribs on axis by radians'''
#Check the max rotation
print("ROTATE TORSO")
if self.busy:
return
self.busy = True
print("ROTATE TORSO2")
maxReached = False
minReached = False
kwargs.update(self.function_map[function_name][1])
print("KW", kwargs)
minR,maxR = self._boneProperties['part.torso'][kwargs['axis']]
#print("MINR", minR)
if kwargs['radians'] >= maxR:
#print("SET TO MAX")
maxReached=True
kwargs['radians'] = maxR
if kwargs['radians'] <= minR:
#print("SET TO MIN")
minReached = True
kwargs['radians'] = minR
#print("RADIANS",radians)
middleware.send(self.function_map[function_name][0],**kwargs)
#middleware.send('rotate_torso',axis=axis, radians=radians)
pattern='type:proprioception bone:torso'
matcher=Pattern(pattern)
for obj in self._internalChunks:
#if axis='0.0'
if matcher.match(obj)!=None:
obj.rotation0=kwargs['radians']
if kwargs['radians'] < 0:
obj.rotation_direction = 'right'
elif kwargs['radians'] > 0:
obj.rotation_direction = 'left'
if maxReached:
obj.rotation0_quality='max'
if minReached:
obj.rotation0_quality='min'
self.busy=False
self.update_posture()
def rotate_torso(self, function_name, **kwargs):
'''Rotate ribs on axis by radians'''
#Check the max rotation
print("ROTATE TORSO")
if self.busy:
return
self.busy = True
print("ROTATE TORSO2")
maxReached = False
minReached = False
kwargs.update(self.function_map[function_name][1])
print("KW", kwargs)
minR, maxR = self._boneProperties['part.torso'][kwargs['axis']]
#print("MINR", minR)
if kwargs['radians'] >= maxR:
#print("SET TO MAX")
maxReached = True
kwargs['radians'] = maxR
if kwargs['radians'] <= minR:
#print("SET TO MIN")
minReached = True
kwargs['radians'] = minR
#print("RADIANS",radians)
middleware.send(self.function_map[function_name][0], **kwargs)
#middleware.send('rotate_torso',axis=axis, radians=radians)
pattern = 'type:proprioception bone:torso'
matcher = Pattern(pattern)
for obj in self._internalChunks:
#if axis='0.0'
if matcher.match(obj) != None:
obj.rotation0 = kwargs['radians']
if kwargs['radians'] < 0:
obj.rotation_direction = 'right'
elif kwargs['radians'] > 0:
obj.rotation_direction = 'left'
if maxReached:
obj.rotation0_quality = 'max'
if minReached:
obj.rotation0_quality = 'min'
self.busy = False
self.update_posture()
def lower_arms(self, function_name, **kwargs):
'''
This function lowers the arms completly.
:param function_name: 'lower_arms'
:param kwargs: {}
:return: Moves the arms by so they are pointing downward
:special note: The arms currently do not map properly to the desired arms for the model.
Because the armature start somewhere near 45 (or -45) deg., when you lower the
arms, you have to lower them to -45 (or 45) deg.,. On the ACT-R side, I'm calling
this 0 deg. '''
print("LOWER ARMS", kwargs)
if self.busy:
return
self.busy = True
# minR,maxR = self._boneProperties['part.torso'][kwargs['axis']]
# #print("MINR", minR)
# if kwargs['radians'] > maxR:
# #print("SET TO MAX")
# maxReached=True
# kwargs['radians'] = maxR
# if kwargs['radians'] < minR:
# #print("SET TO MIN")
# minReached = True
# kwargs['radians'] = minR
patterns = [
'type:proprioception bone:upper_arm.R',
'type:proprioception bone:upper_arm.L'
]
for pattern in patterns:
matcher = Pattern(pattern)
for obj in self._internalChunks:
#if axis='0.0'
if matcher.match(obj) != None:
obj.rotation0 = '0'
obj.overall_quality = 'lowered'
self.busy = False
self.update_posture()
middleware.send(self.function_map[function_name][0], **kwargs)
def lower_arms(self,function_name,**kwargs):
'''
This function lowers the arms completly.
:param function_name: 'lower_arms'
:param kwargs: {}
:return: Moves the arms by so they are pointing downward
:special note: The arms currently do not map properly to the desired arms for the model.
Because the armature start somewhere near 45 (or -45) deg., when you lower the
arms, you have to lower them to -45 (or 45) deg.,. On the ACT-R side, I'm calling
this 0 deg. '''
print("LOWER ARMS", kwargs)
if self.busy:
return
self.busy = True
# minR,maxR = self._boneProperties['part.torso'][kwargs['axis']]
# #print("MINR", minR)
# if kwargs['radians'] > maxR:
# #print("SET TO MAX")
# maxReached=True
# kwargs['radians'] = maxR
# if kwargs['radians'] < minR:
# #print("SET TO MIN")
# minReached = True
# kwargs['radians'] = minR
patterns = ['type:proprioception bone:upper_arm.R',
'type:proprioception bone:upper_arm.L']
for pattern in patterns:
matcher=Pattern(pattern)
for obj in self._internalChunks:
#if axis='0.0'
if matcher.match(obj)!=None:
obj.rotation0='0'
obj.overall_quality='lowered'
self.busy=False
self.update_posture()
middleware.send(self.function_map[function_name][0],**kwargs)
def move_forward(self,function_name,**kwargs):
'''Move forward by some distance'''
#middleware.send('move_forward',[distance])
middleware.send(self.function_map[function_name][0],**kwargs)
def async_test2(self,value):
middleware.send('async_test2',[value])
def set_speed(self,speed=0.01):
'''Move forward @speed in m/s'''
middleware.send('set_speed',[speed])
def move_forward(self, function_name, **kwargs):
'''Move forward by some distance'''
#middleware.send('move_forward',[distance])
middleware.send(self.function_map[function_name][0], **kwargs)
def async_test2(self, value):
middleware.send('async_test2', [value])
def set_speed(self, speed=0.01):
'''Move forward @speed in m/s'''
middleware.send('set_speed', [speed])