def start(self, data):
"""
Ließt einige werte aus der Config, und setzt die Startparameter
"""
#arme am anfang richtig setzen, bevor multithreading das kaputt
#macht
config = data[DATA_KEY_CONFIG]
self.set_init(
DATA_KEY_WALKING_HIP_PITCH_OUT,
config["ZMPConfig"][config["RobotTypeName"]]["HipPitch"], data)
self.set_init(DATA_KEY_WALKING_FORWARD, 0, data)
self.set_init(DATA_KEY_WALKING_SIDEWARD, 0, data)
self.set_init(DATA_KEY_WALKING_ANGULAR, 0, data)
self.set_init(DATA_KEY_WALKING_FORWARD_REAL, 0, data)
self.set_init(DATA_KEY_WALKING_SIDEWARD_REAL, 0, data)
self.set_init(DATA_KEY_WALKING_ANGULAR_REAL, 0, data)
self.set_init(DATA_KEY_WALKING_ACTIVE, False, data)
self.set_init(DATA_KEY_WALKING_RUNNING, False, data)
self.set_init(DATA_KEY_WALKING_ARMS, True)
self.set_init(DATA_KEY_WALKING_ARMS,
not data[DATA_KEY_CONFIG]["walking.armsOff"])
self.nice = Nice(self.debug)
class ZMPWalkingModule(AbstractProcessModule):
"""
Das Walking Module läuft als eigener Process um etwas rechenlast auf
einen anderen Prozessorkern zu verlagern, und um möglichst
unabhängig von den anderen Modulen zu sein, da schon leichte
verzögerungen in den berechnungen des Walkings schnell zum umfallen
beim laufen führen. Es eignet sich recht gut für einen eigenen
Prozess da nur wenige Daten übertragen werden müssen und die nicht
wirklich geschwindigkeitskritisch sind (das beendenn eines Schrittes
dauert sowieso mehrere Sekunden da sind einige ms egal).
"""
def __init__(self):
super(ZMPWalkingModule, self).__init__(
requires=[DATA_KEY_IPC, DATA_KEY_CONFIG, DATA_KEY_GAME_STATUS,
DATA_KEY_WALKING_FORWARD, DATA_KEY_WALKING_SIDEWARD,
DATA_KEY_WALKING_ANGULAR, DATA_KEY_WALKING_ACTIVE, DATA_KEY_WALKING_ARMS],
provides=[
DATA_KEY_WALKING_FORWARD_REAL, DATA_KEY_WALKING_SIDEWARD_REAL,
DATA_KEY_WALKING_ANGULAR_REAL, DATA_KEY_WALKING_ARMS,
DATA_KEY_WALKING_RUNNING, DATA_KEY_ZMP_WALKING])
self.USE_GYRO = config["USE_GYRO"]
zmpconfig = config["ZMPConfig"][config["RobotTypeName"]]
self.forward_factor = zmpconfig["FORWARD_FACTOR"]
self.angle_factor = zmpconfig["ANGLE_FACTOR"]
self.sideward_factor = zmpconfig["SIDEWARD_FACTOR"]
self.sleeptime = 0.005
def start(self, data):
"""
Ließt einige werte aus der Config, und setzt die Startparameter
"""
#arme am anfang richtig setzen, bevor multithreading das kaputt
#macht
config = data[DATA_KEY_CONFIG]
self.set_init(
DATA_KEY_WALKING_HIP_PITCH_OUT,
config["ZMPConfig"][config["RobotTypeName"]]["HipPitch"], data)
self.set_init(DATA_KEY_WALKING_FORWARD, 0, data)
self.set_init(DATA_KEY_WALKING_SIDEWARD, 0, data)
self.set_init(DATA_KEY_WALKING_ANGULAR, 0, data)
self.set_init(DATA_KEY_WALKING_FORWARD_REAL, 0, data)
self.set_init(DATA_KEY_WALKING_SIDEWARD_REAL, 0, data)
self.set_init(DATA_KEY_WALKING_ANGULAR_REAL, 0, data)
self.set_init(DATA_KEY_WALKING_ACTIVE, False, data)
self.set_init(DATA_KEY_WALKING_RUNNING, False, data)
self.set_init(DATA_KEY_WALKING_ARMS, True)
self.set_init(DATA_KEY_WALKING_ARMS,
not data[DATA_KEY_CONFIG]["walking.armsOff"])
self.nice = Nice(self.debug)
def update(self, data):
"""
Hier wird mit absicht nichts getan, Warum das so ist wird
in :func:`post` erklärt
"""
# Tut beabsichtigt nichts, siehe self.post()
pass
def post(self, data):
"""
Hier wird das Synkronisieren mit dem Data dict gemacht was
normalerweise in :func:`update` passiert. Das ist so gelöst
um probleme beim require und provide zu umgehen, da andere
Module das Walking requiren, genaugenommen die Werte dafür
aber Providen. Damit Trotz das die Walking in der abhängigkeits
Kette vorne steht die werde erst am ende ausgelesen werden
wird das hier gemacht
"""
super(ZMPWalkingModule, self).update(data)
def run(self):
#Später import, da das ZMPWalking nich mehr gebaut wird, sondern ein Teil der Motion ist @Robert 04.08.2014
from bitbots.motion.zmpwalking import ZMPWalkingEngine
ipc = self.get(DATA_KEY_IPC)
config = self.get(DATA_KEY_CONFIG)
zmp_config = config["ZMPConfig"]
ipc_was_controllable = False
self.walking = ZMPWalkingEngine()
self.walking.r_shoulder_roll_offset = zmp_config["r_shoulder_roll"]
self.walking.l_shoulder_roll_offset = zmp_config["l_shoulder_roll"]
self.walking.hip_pitch_offset = zmp_config["hip_pitch"]
self.walking.hip_pitch = zmp_config["HipPitch"]
max_speed = zmp_config["MAX_SPEED"]
update_intervall = zmp_config["UPDATE_INTERVALL"]
speed_delta = zmp_config["SPEED_DELTA"]
debug_m(
2,
"Hip Pitch auf %d gesetzt" % config["ZMPConfig"]["HipPitch"])
# wir wollen maximal mögliche aufmerksamkeit von scheduler
self.nice.set_realtime()
log_counter = -1
last_time = time.time()
velocity = (0, 0, 0)
lastforward = 0
lastsideward = 0
lastangular = 0
forward = 0
sideward = 0
angular = 0
updateslower = time.time()
stop_request = 0
while True:
log_counter += 1
if log_counter > 60:
log_counter = 0
if not ipc.controlable or \
self.get(DATA_KEY_GAME_STATUS) != DATA_VALUE_STATE_PLAYING:
time.sleep(0.5)
if not ipc.controlable:
debug_m(4, "IPC not Controllable -> continue")
ipc_was_controllable = False
continue
debug_m(4, "Game State not STATE_PLAYING -> continue")
self.stop_walking()
continue
if self.get("Walking.Active") is False:
self.stop_walking()
if self.walking.running is False:
if log_counter is 0:
debug_m(4, "Walking not active -> Continue")
time.sleep(self.sleeptime)
# Damit bei Walking.active=false nicht getrippelt wird
continue
#this code prevents the robot to run too fast
#and prevents too abrupt canges of speeds
if time.time() - updateslower > update_intervall:
newforward = int(self.get("Walking.Forward"))
if newforward == lastforward:
forward = newforward
elif lastforward < newforward:
if lastforward < 0:
forward = min(
lastforward + speed_delta,
newforward)
else: # newforward >= 0
forward = min(
lastforward + speed_delta,
newforward,
max_speed)
else: # lastforward >= newforward:
if lastforward < 0:
forward = max(
lastforward - speed_delta,
newforward,
-max_speed)
else: # lastforward >= 0
forward = max(
lastforward - speed_delta,
newforward)
newsideward = int(self.get("Walking.Sideward"))
if newsideward == lastsideward:
sideward = newsideward
elif lastsideward < newsideward:
if lastsideward < 0:
sideward = min(
lastsideward + speed_delta,
newsideward)
else: # newforward >= 0
sideward = min(
lastsideward + speed_delta,
newsideward,
max_speed)
else: # lastforward >= newforward:
if lastsideward < 0:
sideward = max(
lastsideward - speed_delta,
newsideward,
-max_speed)
else: # lastforward >= 0
sideward = max(
lastsideward - speed_delta,
newsideward)
newangular = int(self.get("Walking.Angular"))
if newangular == lastangular:
angular = newangular
elif lastangular < newangular:
if lastangular < 0:
angular = min(lastangular + speed_delta, newangular)
else: # newforward >= 0
angular = min(
lastangular + speed_delta,
newangular,
max_speed)
else: # lastforward >= newforward:
if lastangular < 0:
angular = max(
lastangular - speed_delta,
newangular,
-max_speed)
else: # lastforward >= 0
angular = max(
lastangular - speed_delta,
newangular)
updateslower = time.time()
lastforward = forward
lastsideward = sideward
lastangular = angular
if not (velocity == (forward, sideward, angular)):
debug_m(2, "Set new velocity (%.2d %.2d %.2d)" % (forward,
sideward,
angular))
velocity = (forward, sideward, angular)
if self.walking.running is False:
self.reset_walking()
self.walking.velocity = (forward / self.forward_factor,
sideward / self.sideward_factor,
angular / self.angle_factor)
x_move_amplitude, y_move_amplitude, a_move_amplitude = self.walking.velocity
self.set(
"Walking.Forward.Real",
x_move_amplitude * self.forward_factor)
self.set(
"Walking.Sideward.Real",
y_move_amplitude * self.sideward_factor)
self.set(
"Walking.Angular.Real",
a_move_amplitude * self.angle_factor)
if log_counter is 0:
debug_m(3, "Running", self.walking.running)
debug_m(3, "XAmplitude", x_move_amplitude)
debug_m(3, "YAmplitude", y_move_amplitude)
debug_m(3, "AAmplitude", a_move_amplitude)
gyro = ipc.get_gyro()
gyro_x = gyro.x
gyro_y = gyro.y
gyro_z = gyro.z
self.walking.gyro = (gyro_x, gyro_y, gyro_z)
with Timer("Walking", self.debug):
# Etwas bewegen
if ipc_was_controllable is False or \
self.walking.running is False:
self.reset_walking()
self.walking.process()
if self.walking.running:
self.set("Walking.Running", True)
pose = self.walking.pose
# Pose updaten
with Timer("Update IPC Pose", self.debug):
ipc.update(pose)
pose.reset()
else:
stop_request = stop_request + 1
if stop_request is 100:
self.set("Walking.Running", False)
stop_request = 0
# Etwas warten und geht weiter!
t = time.time()
dt = t - last_time
last_time = t
time.sleep(max(0, self.sleeptime - dt))
ipc_was_controllable = ipc.controlable
def stop_walking(self):
self.set("Walking.Forward", 0)
self.set("Walking.Sideward", 0)
self.set("Walking.Angular", 0)
self.set("Walking.Forward.Real", 0)
self.set("Walking.Sideward.Real", 0)
self.set("Walking.Angular.Real", 0)
self.set("Walking.Running", False)
velocity = (0, 0, 0)
self.walking.velocity = velocity
self.walking.stop()
def reset_walking(self):
self.walking.start()
self.walking.stance_reset()
