def start(self, data):
self.ipc = data[DATA_KEY_IPC]
self.pose = self.ipc.get_pose()
#Bereit machen
play_animation("walkready_haende", self.ipc)
self.pose.head_tilt.speed = 90
self.pose.head_pan.speed = 90
self.pose.head_tilt.goal = -74
self.pose.head_pan.goal = 0
self.ipc.update(self.pose)
self.wait_for_end()
def start(self, data):
# Für Kinematik:
self.ipc = data[DATA_KEY_IPC]
self.pose = self.ipc.get_pose()
self.robot = Robot()
self.task = KinematicTask(self.robot)
self.robot.update(self.pose)
play_animation("walkready", self.ipc)
self.pose.head_tilt.speed = 90
self.pose.head_pan.speed = 90
self.pose.head_tilt.goal = -74
self.pose.head_pan.goal = 0
self.ipc.update(self.pose)
self.wait_for_end()
def post(self, data):
"""
This module does its work in :func:`post` to directly play anitmaitons which were provided in this cycle.
"""
ipc = data[DATA_KEY_IPC]
if self.next is None and data.get(DATA_KEY_ANIMATION, None) is not None:
# Animation merken
self.next = data[DATA_KEY_ANIMATION]
debug_m(2, "Next", str(self.next))
data[DATA_KEY_ANIMATION] = None
if self.next is None:
# Wir haben bisher keine Animation
return
if (not ipc.controlable) and ipc.get_motion_state() != STATE_WALKING:
# Wenn wir im state Walking sind müssen wir einfach noch etwas
# Warten, das stoppen wir dann gleich.
debug_m(1, "Could not start Annimation %s." %
str(self.next) + "Ipc is in state %s" % ipc.get_motion_state())
# Wir haben keine Kontrolle oder spielen schon was,
# ignoriere die aktuelle Animation
if isinstance(self.next, (tuple, list)):
# callback mit fail aufruffen
self.next[1](False)
self.next = None
return
# Walking jetzt stoppen
data[DATA_KEY_WALKING_ACTIVE] = False
# Wenn Walking nicht mehr läuft, Animation starten
if self.next is not None and \
not data.get(DATA_KEY_WALKING_RUNNING, False):
# debug.log("Animation abspielen")
if isinstance(self.next, (tuple, list)):
# da ist ein callback dabei
if not play_animation(self.next[0], ipc, self.next[1]):
# schon beim starten gescheitert, wir ruffen das callback
# mit False auf
self.next[1](False)
else:
#ohne callback
play_animation(self.next, ipc)
self.next = None
debug_m(2, "Next", str(self.next))
elif data.get(DATA_KEY_WALKING_RUNNING, False):
debug_m(2, "Wait for stopping of the walking")
def play(self, animation, ipc):
"""
Spielt eine Animation dann ab, wenn gerade keine anderen Bewegungen stattfinden
"""
self.wait_for_end()
play_animation(animation, ipc)
def kick(self, ballV):
"""
Führt den Schuss aus
:param ballV: Sollte data[DATA_KEY_BALL_INFO].v sein.
Steuert, wie weit Außen der Tritt erfolgt
:type ballV: int
"""
param = abs(ballV) - 50 # Position in Parameter umrechnen, geht vielleicht schlauer?
root = 0
# Entscheidung für die Seite
if ballV > 0: # links
lr = 1
end_joint = 35
angle_task_joints = [16]
ignore_joints = [8, 18]
start = "lk_start"
ende = "lk_end"
else: # rechts
lr = -1
end_joint = 34
angle_task_joints = [15]
ignore_joints = [7, 17]
start = "rk_start"
ende = "rk_end"
if param > 100:
param = 100
if param < 0:
param = 0
# Berechnung der Kinematischen Positionen
x = 140 - 0.4 * param
y1 = lr * (40 + 0.5 * param)
y2 = y1 + lr * (0.05 * param) # Für Stabilität vielleicht y2 = y1
# Starten des Schussvorgangs
play_animation(start, self.ipc)
self.wait_for_end()
self.robot.update(self.ipc.get_pose())
# 1. kinematische Position
self.task.perform(
root,
end_joint,
[(1, 0, 0), np.array((0, y1, -280))],
(1e-2, 1),
(0, 3),
100,
angle_task_joints,
ignore_joints,
)
self.robot.set_angles_to_pose(self.pose)
self.ipc.update(self.pose)
self.wait_for_end()
self.robot.update(self.ipc.get_pose())
# 2. kinematische Position
self.task.perform(
root,
end_joint,
[(1, 0, 0), np.array((x, y2, -240))],
(1e-2, 1),
(0, 3),
100,
angle_task_joints,
ignore_joints,
)
self.robot.set_angles_to_pose(self.pose)
# Bewegung der Arme und des anderen Beins für Gleichgewicht/Stabilität
if lr == 1:
self.pose.l_shoulder_pitch.goal = 61.69921875
self.pose.l_elbow.goal = -0.703125
self.pose.r_knee.goal = 55.283203125
self.pose.r_ankle_roll.goal = -9.4921875
self.pose.r_ankle_pitch.goal = 27.421875
self.pose.r_hip_yaw.goal = -0.703125
self.pose.r_hip_roll.goal = 2.8125
self.pose.r_hip_pitch.goal = -40.517578125
self.pose.r_shoulder_pitch.goal = -15.64453125
self.pose.r_elbow.goal = 49.39453125
else:
self.pose.r_shoulder_pitch.goal = -61.69921875
self.pose.r_elbow.goal = 0.703125
self.pose.l_knee.goal = -55.283203125
self.pose.l_ankle_roll.goal = 9.4921875
self.pose.l_ankle_pitch.goal = -27.421875
self.pose.l_hip_yaw.goal = 0.703125
self.pose.l_hip_roll.goal = -2.8125
self.pose.l_hip_pitch.goal = 40.517578125
self.pose.l_shoulder_pitch.goal = 15.64453125
self.pose.l_elbow.goal = -49.39453125
self.ipc.update(self.pose)
self.wait_for_end()
play_animation(ende, self.ipc)
self.wait_for_end()
play_animation("walkready", self.ipc)
self.wait_for_end()
def play_anim(self, anim, block=False):
self.anim_playing = True
play_animation(anim, ipc, self.anim_calback)
if block:
while self.anim_playing:
pass
l_end_joint = 35
angle_task_joints = [15, 16]
ignore_joints = [7, 8, 17, 18]
def update(ipc, robot, id=-1, time=0.0):
pose = ipc.get_pose()
robot.set_angles_to_pose(pose, id, time)
ipc.update(pose)
iteration_time = 0.25
steptime = 0.005
if __name__ == "__main__":
args=sys.argv
if len(args) > 1:
play_animation(args[1])
sleep(2)
robot.update(ipc.get_pose())
#Radius in mm
factor = 45
#Kreismittelpunkt
z_offset = 0
if config["RobotTypeName"] == "Hambot":
z_offset = 300
#base_target = np.array((170, 90, -30))
r_y = robot.get_joint_by_id(r_end_joint).get_endpoint()[1]
l_y = robot.get_joint_by_id(l_end_joint).get_endpoint()[1]
r_base_target = np.array((10, r_y, -290 - z_offset))
l_base_target = np.array((10, l_y, -290 - z_offset))
r_chain = task.create_chain(root, r_end_joint, (0, 3), angle_task_joints, ignore_joints)
l_chain = task.create_chain(root, l_end_joint, (0, 3), angle_task_joints, ignore_joints)