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)