# motProxys implements the motProxyInterface from naoqi import ALProxy import time import math motProxy = ALProxy('ALMotion', '127.0.0.1', 9559) posProxy = ALProxy('ALRobotPosture', '127.0.0.1', 9559) EXPANDED_NAMES = dict() for group_name in ['Body', 'Head', 'LArm', 'LLeg', 'RArm', 'RLeg']: EXPANDED_NAMES[group_name] = motProxy.getBodyNames(group_name) for joint_name in EXPANDED_NAMES['Body']: EXPANDED_NAMES[joint_name] = [joint_name] CONFLICTING_JOINT_NAMES = set(['LHand', 'LWristYaw', 'RHand', 'RWristYaw']) memProxy = ALProxy('ALMemory', '127.0.0.1', 9559) HEAD_VERSION = map(int, memProxy.getData('RobotConfig/Head/Version').split('.')) HEAD_ID = memProxy.getData('RobotConfig/Head/FullHeadId') BODY_VERSION = map(int, memProxy.getData('RobotConfig/Body/Version').split('.')) BODY_ID = memProxy.getData('Device/DeviceList/ChestBoard/Bootloader/BoardId') BODY_NAME = memProxy.getData('Device/DeviceList/ChestBoard/BodyNickName') JOINT_COUNT = len(EXPANDED_NAMES['Body']) del memProxy motProxy.setExternalCollisionProtectionEnabled('Move', False) motProxy.setFallManagerEnabled(False) def flatten_list(x): result = [] for element in x: if hasattr(element, "__iter__") and not isinstance(element, basestring): result.extend(flatten_list(element)) else: result.append(element) return result def angleInterpolation(names, angles, times, isAbsolute): names = flatten_list([names]) expanded_names = list() for name in names: expanded_names.extend(EXPANDED_NAMES[name]) if JOINT_COUNT == 22: if not CONFLICTING_JOINT_NAMES.isdisjoint(expanded_names): conflicts = list() for i in range(len(expanded_names)): if expanded_names[i] in CONFLICTING_JOINT_NAMES: conflicts.append(i) if len(indices) >= max_conflicts: break for i in sorted(conflicts, reverse=True): angles.pop(i) times.pop(i) elif JOINT_COUNT == 26: if not CONFLICTING_JOINT_NAMES.isdisjoint(expanded_names): conflict_count = 0 max_conflict = len(CONFLICTING_JOINT_NAMES) angles = flatten_list([angles]) times = flatten_list([times]) for i in range(len(expanded_names)): if expanded_names[i] in CONFLICTING_JOINT_NAMES: #angles.insert(i, OPTIMAL_ANGLE[expanded_names[i]]) angles.insert(i, 0) times.insert(i, times[i-1]) conflict_count += 1 if conflict_count >= max_conflict: break return motProxy.angleInterpolation(names, angles, times, isAbsolute) ''' All motions are to be specified here in alphabetical order: Name: Function: TODO: -------------------------------------------------------------------------------------------------- backToStand() stand up lying back down TEST! improve speed bellyToStand() stand up lying face down TEST! improve speed changeHead(yaw, pitch) relative headmotion TEST! x experimentalKick(angle) heelball TEST! further design diveLeft() dive towards left (keeper) TEST! safer fall? + faster getting up? diveRight() dive towards right(keeper) TEST! footLeft() sidestep left (keeper) TEST! perhaps preventing ball from footRight() sidestep right (keeper) TEST! getting stuck between legs getHeadPos() return current [yaw,pitch] TEST! x getPose() return current pose (belly, etc) x hak() second attempt at heelball TEST! improvement or removal.. hakje() first attempt at heelball TEST! improvement or removal.. isLyingBack() True if robot is on its back x isLyingBelly() True if robot is on its belly x isWalking() return motProxy.walkIsActive() TEST! x keepNormalPose() normal pose for keeper TEST! improve speed/lessen load on joints kick(angle) kick based on given angle further implementation (more cases) kill() remove stiffness (body) TEST! x killKnees() remove knee stiffness TEST! improve speed (safely) or use only in finished state killWalk() stops walking if active TEST! x lKickAngled(angle) kick based on given angle TEST! improve or remove entirely (dynamic kick?) normalPose(force) stand in normal pose, can be forced TEST! improve speed (safely), arm position postWalkTo(x,y,angle) nonblocking walking call TEST! x rKickAngled(angle) kick based on given angle TEST! improve or remove entirely (dynamic kick?) setHead(yaw,pitch) set head position TEST! improve speed? not sure if needed sideLeftKick() kick softly towards the left TEST! is meant to be for passing, could sideRightKick() right TEST! maybe be faster or speed as input? stance() keeper stance() TEST! reduce load on knees somehow? Not sure if possible - TEST! removing knee stiffness has scary results. TEST! Dont try to remove kneestiffness before consulting anyone. standUp() stand up if fallen, return fallen TEST! x stiff() set stiffness for body TEST! x stiffKnees() set stiffness for knees alone TEST! use in soul SWTV(x,y,t,f) setwalktargetvelocity call TEST! x walkTo(x,y,angle) walkTo call TEST! x ''' def isLyingBack(): return posProxy.getPostureFamily() in ['Back', 'LyingBack'] def isLyingBelly(): return posProxy.getPostureFamily() in ['LyingBelly', 'Belly'] # Stand up from the back def backToStand(): names = list() times = list() angles = list() names.append('HeadYaw') times.append([0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ -0.02459, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.00000, [ 3, -0.33333, -0.01222], [ 3, 0.26667, 0.00977]], [ 0.04138, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.00000], [ 3, 0.23333, -0.00000]], [ 0.00000, [ 3, -0.23333, 0.00000], [ 3, 0.16667, -0.00000]], [ 0.00000, [ 3, -0.16667, 0.00000], [ 3, 0.26667, -0.00000]], [ -0.51393, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.30224, [ 3, -0.30000, -0.12958], [ 3, 0.30000, 0.12958]], [ 0.26354, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.12043, [ 3, -0.20000, 0.04549], [ 3, 0.20000, -0.04549]], [ -0.00940, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('HeadPitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.06285, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.51490, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.67200, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.00000, [ 3, -0.23333, -0.19854], [ 3, 0.16667, 0.14181]], [ 0.34907, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.17177, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.34511, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.01047, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.02967, [ 3, -0.20000, -0.01920], [ 3, 0.20000, 0.01920]], [ 0.37886, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.5, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LShoulderPitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 2.08560, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 2.09440, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.03865, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.08560, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.08560, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 2.08560, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.03251, [ 3, -0.26667, 0.05309], [ 3, 0.30000, -0.05973]], [ 1.28698, [ 3, -0.30000, 0.17922], [ 3, 0.30000, -0.17922]], [ 0.95717, [ 3, -0.30000, 0.11076], [ 3, 0.20000, -0.07384]], [ 0.73321, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.73321, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 1.59225, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LShoulderRoll') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.98018, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.36652, [ 3, -0.33333, 0.02182], [ 3, 0.26667, -0.01745]], [ 0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.36652, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.29909, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.07052, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.26687, [ 3, -0.30000, -0.08560], [ 3, 0.20000, 0.05706]], [ 0.49851, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.49851, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.21932, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LElbowYaw') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ -0.76091, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.15708, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.12268, [ 3, -0.26667, 0.01164], [ 3, 0.26667, -0.01164]], [ 0.08727, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.08727, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.08727, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.96049, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.85448, [ 3, -0.30000, -0.28507], [ 3, 0.30000, 0.28507]], [ -0.25008, [ 3, -0.30000, -0.18751], [ 3, 0.20000, 0.12501]], [ 0.08308, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.08308, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LElbowRoll') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ -0.62430, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.69813, [ 3, -0.23333, -0.30205], [ 3, 0.16667, 0.21575]], [ -0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.13333, 0.00000]], [ -1.04615, [ 3, -0.13333, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.87280, [ 3, -0.30000, -0.03605], [ 3, 0.30000, 0.03605]], [ -0.82985, [ 3, -0.30000, -0.01349], [ 3, 0.20000, 0.00899]], [ -0.80535, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.80535, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RShoulderPitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 2.08560, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 2.09440, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.95283, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.08560, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.08560, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 2.08560, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 2.07708, [ 3, -0.26667, 0.00759], [ 3, 0.30000, -0.00854]], [ 2.03720, [ 3, -0.30000, 0.03988], [ 3, 0.30000, -0.03988]], [ 1.44047, [ 3, -0.30000, 0.22918], [ 3, 0.20000, -0.15279]], [ 0.89131, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.89131, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 1.46961, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RShoulderRoll') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ -1.07384, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.36652, [ 3, -0.33333, -0.02182], [ 3, 0.26667, 0.01745]], [ -0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.34907, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.36652, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.45717, [ 3, -0.26667, 0.08719], [ 3, 0.30000, -0.09809]], [ -0.56455, [ 3, -0.30000, 0.04142], [ 3, 0.30000, -0.04142]], [ -0.70568, [ 3, -0.30000, 0.06162], [ 3, 0.20000, -0.04108]], [ -0.87266, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.68068, [ 3, -0.20000, -0.05778], [ 3, 0.63333, 0.18298]], [ -0.15037, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RElbowYaw') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.76542, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.15708, [ 3, -0.33333, 0.12584], [ 3, 0.26667, -0.10067]], [ -0.25775, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.08727, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.08727, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.08727, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.08134, [ 3, -0.26667, -0.00285], [ 3, 0.30000, 0.00321]], [ -0.06907, [ 3, -0.30000, -0.00920], [ 3, 0.30000, 0.00920]], [ -0.02612, [ 3, -0.30000, -0.01394], [ 3, 0.20000, 0.00929]], [ 0.00062, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.00062, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RElbowRoll') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.71028, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.56210, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.69813, [ 3, -0.23333, 0.30205], [ 3, 0.16667, -0.21575]], [ 0.00870, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.09668, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.02152, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.19026, [ 3, -0.30000, -0.08645], [ 3, 0.20000, 0.05764]], [ 0.45379, [ 3, -0.20000, -0.06162], [ 3, 0.20000, 0.06162]], [ 0.55995, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LHipYawPitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ -0.47090, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.25090, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, -0.00964], [ 3, 0.26667, 0.00771]], [ 0.00771, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.00000, [ 3, -0.26667, 0.00771], [ 3, 0.23333, -0.00675]], [ -0.65498, [ 3, -0.23333, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.49851, [ 3, -0.43333, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.85900, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.69639, [ 3, -0.30000, -0.09135], [ 3, 0.20000, 0.06090]], [ -0.40225, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.40225, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LHipRoll') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.04453, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.05833, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.02189], [ 3, 0.23333, -0.01915]], [ -0.06478, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.54105, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.15498, [ 3, -0.26667, 0.11230], [ 3, 0.30000, -0.12633]], [ -0.17483, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.00925, [ 3, -0.30000, -0.07336], [ 3, 0.20000, 0.04891]], [ 0.19199, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.17453, [ 3, -0.20000, 0.00321], [ 3, 0.63333, -0.01015]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LHipPitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.25008, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.37434, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.17453, [ 3, -0.26667, 0.27599], [ 3, 0.23333, -0.24149]], [ -1.17808, [ 3, -0.23333, 0.27150], [ 3, 0.16667, -0.19393]], [ -1.57080, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.85746, [ 3, -0.26667, -0.23750], [ 3, 0.30000, 0.26719]], [ -0.05672, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.45556, [ 3, -0.30000, 0.15970], [ 3, 0.20000, -0.10647]], [ -0.85521, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.83599, [ 3, -0.20000, -0.01922], [ 3, 0.63333, 0.06087]], [ -0.4, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LKneePitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.09200, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.67552, [ 3, -0.33333, -0.35708], [ 3, 0.26667, 0.28566]], [ 2.02024, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.67552, [ 3, -0.26667, 0.16755], [ 3, 0.23333, -0.14661]], [ 1.07777, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 1.67552, [ 3, -0.16667, -0.13243], [ 3, 0.26667, 0.21189]], [ 2.11075, [ 3, -0.26667, -0.00135], [ 3, 0.30000, 0.00152]], [ 2.11227, [ 3, -0.30000, -0.00026], [ 3, 0.30000, 0.00026]], [ 2.11253, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ 2.11253, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 2.11253, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.95, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LAnklePitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.82525, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.24435, [ 3, -0.33333, 0.23692], [ 3, 0.26667, -0.18953]], [ -0.45411, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.24435, [ 3, -0.26667, -0.20117], [ 3, 0.23333, 0.17602]], [ 0.67748, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.66323, [ 3, -0.16667, 0.01425], [ 3, 0.26667, -0.02280]], [ -0.45564, [ 3, -0.26667, 0.27681], [ 3, 0.30000, -0.31141]], [ -1.10145, [ 3, -0.30000, 0.08806], [ 3, 0.30000, -0.08806]], [ -1.18952, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ -1.18952, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -1.18952, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.55, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LAnkleRoll') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ -0.03371, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.03371, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, -0.03371], [ 3, 0.23333, 0.02949]], [ 0.17109, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.10472, [ 3, -0.16667, 0.07267], [ 3, 0.26667, -0.11627]], [ -0.39573, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -0.09967, [ 3, -0.30000, -0.07312], [ 3, 0.30000, 0.07312]], [ 0.04299, [ 3, -0.30000, -0.04355], [ 3, 0.20000, 0.02904]], [ 0.11810, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.08727, [ 3, -0.20000, 0.02037], [ 3, 0.63333, -0.06450]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RHipRoll') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ -0.14876, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.02297, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, -0.01509], [ 3, 0.23333, 0.01320]], [ 0.06190, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ -0.54105, [ 3, -0.16667, 0.01080], [ 3, 0.26667, -0.01728]], [ -0.55833, [ 3, -0.26667, 0.01089], [ 3, 0.30000, -0.01225]], [ -0.61049, [ 3, -0.30000, 0.01137], [ 3, 0.30000, -0.01137]], [ -0.62657, [ 3, -0.30000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.29671, [ 3, -0.20000, -0.10152], [ 3, 0.20000, 0.10152]], [ -0.01745, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RHipPitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.18557, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.28221, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ -0.17453, [ 3, -0.26667, 0.25720], [ 3, 0.23333, -0.22505]], [ -1.16456, [ 3, -0.23333, 0.27150], [ 3, 0.16667, -0.19393]], [ -1.57080, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ -1.52484, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ -1.56012, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ -1.02974, [ 3, -0.30000, -0.13086], [ 3, 0.20000, 0.08724]], [ -0.90583, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ -0.90583, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.4, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RKneePitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.19179, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.67552, [ 3, -0.33333, -0.33975], [ 3, 0.26667, 0.27180]], [ 2.02645, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.67552, [ 3, -0.26667, 0.16579], [ 3, 0.23333, -0.14507]], [ 1.09386, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 1.67552, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 1.22111, [ 3, -0.26667, 0.09029], [ 3, 0.30000, -0.10158]], [ 1.09992, [ 3, -0.30000, 0.01607], [ 3, 0.30000, -0.01607]], [ 1.08385, [ 3, -0.30000, 0.01607], [ 3, 0.20000, -0.01071]], [ 0.87616, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 1.76278, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ 0.95, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RAnklePitch') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.46638, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.24435, [ 3, -0.33333, 0.15567], [ 3, 0.26667, -0.12454]], [ -0.37425, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.24435, [ 3, -0.26667, -0.19407], [ 3, 0.23333, 0.16981]], [ 0.71737, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.66323, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.78545, [ 3, -0.26667, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.78392, [ 3, -0.30000, 0.00153], [ 3, 0.30000, -0.00153]], [ 0.44157, [ 3, -0.30000, 0.05760], [ 3, 0.20000, -0.03840]], [ 0.40317, [ 3, -0.20000, 0.03840], [ 3, 0.20000, -0.03840]], [ -0.57945, [ 3, -0.20000, 0.00000], [ 3, 0.63333, 0.00000]], [ -0.55, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RAnkleRoll') times.append([ 0.5, 1.0, 1.4, 1.8, 2.4, 2.8, 3.3, 3.8, 4.5, 5.2, 5.9, 7.0]) angles.append([ [ 0.16725, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00000, [ 3, -0.33333, 0.03835], [ 3, 0.26667, -0.03068]], [ -0.03984, [ 3, -0.26667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00000, [ 3, -0.26667, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.16084, [ 3, -0.23333, 0.00000], [ 3, 0.16667, 0.00000]], [ 0.10472, [ 3, -0.16667, 0.00000], [ 3, 0.26667, 0.00000]], [ 0.00925, [ 3, -0.26667, 0.03182], [ 3, 0.30000, -0.03580]], [ -0.09813, [ 3, -0.30000, 0.00000], [ 3, 0.30000, 0.00000]], [ 0.44331, [ 3, -0.30000, -0.15555], [ 3, 0.20000, 0.10370]], [ 0.67960, [ 3, -0.20000, 0.00000], [ 3, 0.20000, 0.00000]], [ 0.27751, [ 3, -0.20000, 0.04933], [ 3, 0.63333, -0.15622]], [ 0.0, [ 3, -0.63333, 0.00000], [ 3, 0.00000, 0.00000]]]) motProxy.angleInterpolationBezier(names, times, angles) # Stand up from belly def bellyToStand(): names = list() times = list() angles = list() names.append('HeadYaw') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.17453, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.22689, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.28623, [ 3, -0.46667, -0.01333], [ 3, 0.36667, 0.01047]], [ 0.29671, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.49567, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.29671, [ 3, -0.23333, -0.07104], [ 3, 0.36667, 0.11164]], [ 0.05236, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.39095, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('HeadPitch') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ -0.57683, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.54768, [ 3, -0.33333, -0.02915], [ 3, 0.46667, 0.04081]], [ 0.10734, [ 3, -0.46667, -0.19834], [ 3, 0.36667, 0.15584]], [ 0.51487, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.38048, [ 3, -0.43333, 0.01726], [ 3, 0.23333, -0.00930]], [ 0.37119, [ 3, -0.23333, 0.00930], [ 3, 0.36667, -0.01461]], [ -0.10472, [ 3, -0.36667, 0.13827], [ 3, 0.43333, -0.16341]], [ -0.53387, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.5, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LShoulderPitch') times.append([0.2, 0.5, 0.8, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [2, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.02757, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.51146, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -1.25025, [ 3, -0.46667, -0.26120], [ 3, 0.36667, 0.20523]], [ 0.07206, [ 3, -0.36667, -0.38566], [ 3, 0.43333, 0.45578]], [ 1.27409, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.75573, [ 3, -0.23333, 0.00333], [ 3, 0.36667, -0.00524]], [ 0.75049, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 1.29154, [ 3, -0.43333, -0.15226], [ 3, 0.36667, 0.12884]], [ 1.2, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LShoulderRoll') times.append([0.3, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 1.55390, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.01683, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.07666, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.07052, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.15643, [ 3, -0.43333, -0.08590], [ 3, 0.23333, 0.04626]], [ 0.93899, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.67719, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.84648, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.2, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LElbowYaw') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ -2.07694, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.58006, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -1.60461, [ 3, -0.46667, 0.02454], [ 3, 0.36667, -0.01928]], [ -1.78715, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.32695, [ 3, -0.43333, -0.11683], [ 3, 0.23333, 0.06291]], [ -1.24791, [ 3, -0.23333, -0.04593], [ 3, 0.36667, 0.07218]], [ -0.97260, [ 3, -0.36667, -0.01072], [ 3, 0.43333, 0.01267]], [ -0.95993, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LElbowRoll') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ -0.00873, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.35278, [ 3, -0.33333, 0.08741], [ 3, 0.46667, -0.12238]], [ -0.63810, [ 3, -0.46667, 0.09306], [ 3, 0.36667, -0.07312]], [ -0.85133, [ 3, -0.36667, 0.13944], [ 3, 0.43333, -0.16480]], [ -1.55083, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.73304, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.73653, [ 3, -0.36667, 0.00349], [ 3, 0.43333, -0.00413]], [ -1.15506, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RShoulderPitch') times.append([0.2, 0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [2, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.02757, [ 3, -0.33333, 0.00000 ], [ 3, 0.33333, 0.00000]], [ -1.51146, [ 3, -0.33333, 0.00000 ], [ 3, 0.46667, 0.00000]], [ -1.22256, [ 3, -0.46667, -0.23805], [ 3, 0.36667, 0.18704]], [ -0.23619, [ 3, -0.36667, -0.22007], [ 3, 0.43333, 0.26008]], [ 0.21787, [ 3, -0.43333, -0.14857 ], [ 3, 0.23333, 0.08000]], [ 0.44950, [ 3, -0.23333, -0.09028 ], [ 3, 0.36667, 0.14187]], [ 0.91431, [ 3, -0.36667, -0.03894 ], [ 3, 0.43333, 0.04602]], [ 0.96033, [ 3, -0.43333, -0.04602 ], [ 3, 0.36667, 0.03894]], [ 1.2, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RShoulderRoll') times.append([0.3, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ -1.53558, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.19199, [ 3, -0.33333, -0.07793], [ 3, 0.46667, 0.10911]], [ -0.08288, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.08288, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.22707, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.18259, [ 3, -0.23333, -0.02831], [ 3, 0.36667, 0.04448]], [ -0.00870, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.13197, [ 3, -0.43333, 0.01994], [ 3, 0.36667, -0.01687]], [ -0.2, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RElbowYaw') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 2.07694, [ 3, -0.33333, 0.00000 ], [ 3, 0.33333, 0.00000]], [ 1.56157, [ 3, -0.33333, 0.00000 ], [ 3, 0.46667, 0.00000]], [ 1.61373, [ 3, -0.46667, -0.02319], [ 3, 0.36667, 0.01822]], [ 1.68582, [ 3, -0.36667, -0.05296], [ 3, 0.43333, 0.06259]], [ 1.96041, [ 3, -0.43333, 0.00000 ], [ 3, 0.23333, 0.00000]], [ 1.95121, [ 3, -0.23333, 0.00920 ], [ 3, 0.36667, -0.01445]], [ 0.66571, [ 3, -0.36667, 0.22845 ], [ 3, 0.43333, -0.26998]], [ 0.39573, [ 3, -0.43333, 0.00000 ], [ 3, 0.36667, 0.00000]], [ 0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RElbowRoll') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.10472, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.38201, [ 3, -0.33333, -0.07367], [ 3, 0.46667, 0.10313]], [ 0.63512, [ 3, -0.46667, -0.21934], [ 3, 0.36667, 0.17234]], [ 1.55705, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.00870, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.00870, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.42343, [ 3, -0.36667, -0.09786], [ 3, 0.43333, 0.11566]], [ 0.64926, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LHipYawPitch') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ -0.03371, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.03491, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -0.43561, [ 3, -0.46667, 0.15197], [ 3, 0.36667, -0.11941]], [ -0.77923, [ 3, -0.36667, 0.09257], [ 3, 0.43333, -0.10940]], [ -1.04154, [ 3, -0.43333, 0.07932], [ 3, 0.23333, -0.04271]], [ -1.530, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -1, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.56754, [ 3, -0.43333, -0.16414], [ 3, 0.36667, 0.13889]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LHipRoll') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.06294, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00158, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.37732, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.29755, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.29755, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.19486, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.12736, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LHipPitch') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.06140, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.06136], [ 3, 0.46667, -0.08590]], [ -1.56924, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -1.28085, [ 3, -0.36667, -0.08132], [ 3, 0.43333, 0.09611]], [ -1.03694, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -1.15966, [ 3, -0.23333, 0.01464], [ 3, 0.36667, -0.02301]], [ -1.18267, [ 3, -0.36667, 0.01687], [ 3, 0.43333, -0.01994]], [ -1.27011, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.4, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LKneePitch') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.12043, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.98968, [ 3, -0.33333, -0.08775], [ 3, 0.46667, 0.12285]], [ 2.11253, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.28221, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.40493, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.35738, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.71940, [ 3, -0.36667, -0.25311], [ 3, 0.43333, 0.29913]], [ 2.01409, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.95, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LAnklePitch') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.92189, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.02974, [ 3, -0.33333, 0.08628], [ 3, 0.46667, -0.12080]], [ -1.15054, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.21625, [ 3, -0.36667, -0.28428], [ 3, 0.43333, 0.33597]], [ 0.71020, [ 3, -0.43333, -0.15307], [ 3, 0.23333, 0.08242]], [ 0.92275, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.82525, [ 3, -0.36667, 0.09750], [ 3, 0.43333, -0.11522]], [ -0.50166, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.55, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('LAnkleRoll') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ -0.00149, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ -0.00149, [ 3, -0.46667, 0.00153], [ 3, 0.36667, -0.00121]], [ -0.45249, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.30062, [ 3, -0.43333, -0.07246], [ 3, 0.23333, 0.03901]], [ -0.11808, [ 3, -0.23333, -0.03361], [ 3, 0.36667, 0.05281]], [ -0.04138, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.12114, [ 3, -0.43333, 0.01632], [ 3, 0.36667, -0.01381]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RHipRoll') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.03142, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00158, [ 3, -0.46667, -0.00153], [ 3, 0.36667, 0.00121]], [ 0.31144, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.25469, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 0.32065, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.22707, [ 3, -0.36667, 0.06047], [ 3, 0.43333, -0.07146]], [ -0.07512, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RHipPitch') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.07666, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -0.00004, [ 3, -0.33333, 0.07670], [ 3, 0.46667, -0.10738]], [ -1.57699, [ 3, -0.46667, 0.10738], [ 3, 0.36667, -0.08437]], [ -1.66136, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.19963, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -1.59847, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.32218, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -0.71028, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.4, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RKneePitch') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ -0.07819, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 1.98968, [ 3, -0.33333, -0.06900], [ 3, 0.46667, 0.09660]], [ 2.08628, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ 1.74267, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 2.12019, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ 2.12019, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ 2.12019, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 2.12019, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ 0.95, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RAnklePitch') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.92965, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ -1.02974, [ 3, -0.33333, 0.07746], [ 3, 0.46667, -0.10844]], [ -1.13819, [ 3, -0.46667, 0.02925], [ 3, 0.36667, -0.02298]], [ -1.18645, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.18645, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.58901, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -1.18645, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ -1.18645, [ 3, -0.43333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.55, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) names.append('RAnkleRoll') times.append([0.5, 1.0, 1.7, 2.3, 2.9, 3.7, 4.4, 5.2, 6.5]) angles.append([ [ 0.18850, [ 3, -0.33333, 0.00000], [ 3, 0.33333, 0.00000]], [ 0.00004, [ 3, -0.33333, 0.00000], [ 3, 0.46667, 0.00000]], [ 0.00618, [ 3, -0.46667, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.00456, [ 3, -0.36667, 0.01074], [ 3, 0.43333, -0.01269]], [ -0.09813, [ 3, -0.43333, 0.00000], [ 3, 0.23333, 0.00000]], [ -0.01376, [ 3, -0.23333, 0.00000], [ 3, 0.36667, 0.00000]], [ -0.09507, [ 3, -0.36667, 0.00000], [ 3, 0.43333, 0.00000]], [ 0.03532, [ 3, -0.43333, -0.02825], [ 3, 0.36667, 0.02390]], [ 0.0, [ 3, -0.36667, 0.00000], [ 3, 0.00000, 0.00000]]]) motProxy.angleInterpolationBezier(names, times, angles) # non blocking call, relative def changeHead(yaw,pitch): motProxy.changeAngles(['HeadYaw', 'HeadPitch'], [yaw, pitch], 0.3) # dive towards the left (keeper) def diveLeft(): names = list() angles = list() times = list() names.extend(['LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll']) angles.extend([[0.438], [-0.8], [2.11], [-1.186], [-0.09]]) times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]]) names.append('LHipYawPitch') angles.append([0.1]) times.append([0.5]) names.extend(['RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll']) angles.extend([[0], [-0.3], [0.6871], [-0.3099], [0.54788]]) times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]]) names.extend(['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll']) angles.extend([[-1.1], [0.5], [0.1883], [-0.5016]]) times.extend([[0.2], [0.2], [0.2], [0.2]]) names.extend(['RShoulderPitch','RShoulderRoll', 'RElbowYaw', 'RElbowRoll']) angles.extend([[1.2, 1.5], [-0.11, 0.3403], [-0.1, 0.1226], [0.25, 0.903]]) times.extend([[0.2,0.5], [0.2,0.5], [0.2,0.5], [0.2, 0.5]]) angleInterpolation(names, angles, times, True) motProxy.setStiffnesses('Body', 0) time.sleep(2) motProxy.setStiffnesses('RArm', 0.8) motProxy.setStiffnesses('LLeg', 0.6) motProxy.setStiffnesses('RLeg', 0.6) # move arm around body to shift balance angleInterpolation(['RShoulderPitch','RShoulderRoll', 'RElbowYaw', 'RElbowRoll'], [[1.372, 2.085], [-1.2916, -0.105], [-0.154, -0.130],[1.1888, 0.380]], [[0.4, 0.8], [0.4, 0.8], [0.4, 0.8], [0.4, 0.8]], True) motProxy.setAngles('RLeg', [-0.5,0,0,0.6,0.2,-0.2], 0.2) motProxy.setAngles('LLeg', [-0.5,0,0,0.6,0.2, 0.2], 0.2) motProxy.setStiffnesses('RArm', 0) while not isLyingBack() and not isLyingBelly(): pass motProxy.setStiffnesses('Body',0.8) current_posture = posProxy.getPosture() if isLyingBack(): backToStand() motProxy.walkTo(0.1, 0.05, 1.5) elif isLyingBelly(): bellyToStand() motProxy.walkTo(-0.1, -0.05, -1.5) # dive towards the right (keeper) def diveRight(): names = list() angles = list() times = list() names.extend(['RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll']) angles.extend([[-0.438], [-0.8], [2.11], [-1.186], [0.09]]) times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]]) names.append('LHipYawPitch') angles.append([0.1]) times.append([0.5]) names.extend(['LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll']) angles.extend([[0], [-0.3], [0.6871], [-0.3099], [-0.54788]]) times.extend([[0.5], [0.5], [0.5], [0.5], [0.5]]) names.extend(['RShoulderPitch','RShoulderRoll', 'RElbowYaw', 'RElbowRoll']) angles.extend([[-1.1], [-0.5], [0.1883], [0.5016]]) times.extend([[0.2], [0.2], [0.2], [0.2]]) names.extend(['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll']) angles.extend([[1.2, 1.5], [0.11, -0.3403], [-0.1, 0.1226], [-0.25, -0.903]]) times.extend([[0.2,0.5], [0.2,0.5], [0.2,0.5], [0.2, 0.5]]) angleInterpolation(names, angles, times, True) motProxy.setStiffnesses('Body', 0) time.sleep(2) motProxy.setStiffnesses('LArm', 0.8) motProxy.setStiffnesses('LLeg', 0.6) motProxy.setStiffnesses('RLeg', 0.6) angleInterpolation(['LShoulderPitch','LShoulderRoll', 'LElbowYaw', 'LElbowRoll'], [[1.372, 2.085], [1.2916, 0.105], [-0.154, -0.130], [-1.1888, -0.380]], [[0.4, 0.8], [0.4, 0.8], [0.4, 0.8], [0.4, 0.8]], True) motProxy.setAngles('LLeg', [-0.5,0,0,0.6,0.2, 0.2], 0.2) motProxy.setAngles('RLeg', [-0.5,0,0,0.6,0.2,-0.2], 0.2) motProxy.setStiffnesses('RArm', 0) while not isLyingBack() and not isLyingBelly(): pass motProxy.setStiffnesses('Body',0.8) current_posture = posProxy.getPosture() if isLyingBack(): backToStand() motProxy.walkTo(0.1, -0.05, -1.5) elif isLyingBelly(): bellyToStand() motProxy.walkTo(-0.1, 0.05, 1.5) # heelball, not finished. def experimentalKick(angle): names = list() angles = list() times = list() names.append('RShoulderRoll') angles.append([-0.3]) times.append([0.5]) names.append('RShoulderPitch') angles.append([0.4]) times.append([0.5]) names.append('LShoulderRoll') angles.append([0.5]) times.append([0.5]) names.append('LShoulderPitch') angles.append([1.0]) times.append([0.5]) names.append('LHipYawPitch') angles.append([angle]) times.append([1.2]) names.append('RHipRoll') angles.append([0]) times.append([0.5]) names.append('RHipPitch') angles.append([-0.3, -1.2, -1.5, -1.2]) times.append( [0.5, 1.0, 1.5, 2.0]) names.append('RKneePitch') angles.append([1.0, 2.0, 1.2, 1.0]) times.append( [0.5, 1.0, 1.5, 2.0]) names.append('RAnklePitch') angles.append([-0.6, -1.2, 0.15, 0.3]) times.append([ 0.5, 1.0, 1.5, 2.0]) names.append('RAnkleRoll') angles.append([0.2]) times.append([0.4]) names.append('LHipRoll') angles.append([0]) times.append([0.5]) names.append('LHipPitch') angles.append([-0.4]) times.append([0.4]) names.append('LKneePitch') angles.append([0.95]) times.append([0.4]) names.append('LAnklePitch') angles.append([-0.55]) times.append([0.4]) names.append('LAnkleRoll') angles.append([0.25]) times.append([0.4]) angleInterpolation(names, angles, times, True) # sidestep left (keeper) def footLeft(): names = list() times = list() angles = list() names.extend( ['LShoulderPitch','LShoulderRoll','LElbowYaw','LElbowRoll','RShoulderPitch','RShoulderRoll']) angles.extend([[1.9], [0.5], [-0.3], [-0.05], [1.2], [-0.43] ]) times.extend( [[0.4], [0.4], [0.4], [0.4], [0.75], [1.0 ] ]) names.extend( ['RElbowYaw','RElbowRoll','LHipYawPitch','LHipRoll','LHipPitch','LKneePitch','LAnklePitch']) angles.extend([[-0.61], [0.44], [-0.98], [0.68], [-1.5,-1.1],[0.7, 0.0], [1.0, 2.0]]) times.extend( [[ 0.75], [0.75], [0.6], [0.75], [0.75, 1.2],[0.75,1.2], [0.75, 1.2]]) names.extend( ['LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'] ) angles.extend([[-0.05], [0.1426], [-0.2222], [2.11], [-0.9], [0.079]]) times.extend( [[0.75], [0.75], [0.75], [0.75], [0.75], [0.75]] ) angleInterpolation(names, angles, times, True ) # wait to stop the ball time.sleep(1) angles = list() times = list() ''' converts an entire bodyAngleList from left- to right-oriented. def function(input): allangle = list() for j in range(len(input)): oneangle = list() for i in range(len(input[0])): if i >= 2 and i <= 5: if i == 3 or i == 5: oneangle.append(-input[j][i+16]) else: oneangle.append(input[j][i+16]) elif i >= 6 and i <= 11: if i == 7 or i == 11: oneangle.append(-input[j][i+6]) else: oneangle.append(input[j][i+6]) elif i >= 12 and i <= 17: if i == 13 or i == 17: oneangle.append(-input[j][i-6]) else: oneangle.append(input[j][i-6]) elif i >= 18: if i == 19 or i == 21: oneangle.append(-input[j][i-16]) else: oneangle.append(input[j][i-16]) else: oneangle.append(input[j][i]) allangle.append(oneangle) return allangle ''' anglelist = [ [0.056716039776802063, 0.31442803144454956, 1.9497560262680054, 0.4939899742603302, -0.30070596933364868, -0.09361596405506134, -0.95717406272888184, 0.68565607070922852, -1.3116120100021362, 0.69034194946289063, 0.93200582265853882, -0.001575961709022522, -0.95717406272888184, 0.14568804204463959, -0.24233004450798035, 2.112546443939209, -0.88209199905395508, 0.0076280385255813599, 1.3023240566253662, -0.33437004685401917, -0.61364197731018066, 0.42641004920005798], [0.05825003981590271, 0.3159620463848114, 1.9497560262680054, 0.50012600421905518, -0.29917198419570923, -0.090547963976860046, -0.96484404802322388, 0.68565607070922852, -1.5647220611572266, 1.2747960090637207, 0.6535259485244751, -4.1961669921875e-05, -0.96484404802322388, 0.14568804204463959, -0.21165004372596741, 2.112546443939209, -0.90816998481750488, 0.0076280385255813599, 1.3406740427017212, -0.33590406179428101, -0.61364197731018066, 0.42487603425979614], [0.056716039776802063, 0.31442803144454956, 1.9359500408172607, 0.50012600421905518, -0.30070596933364868, -0.085945963859558105, -0.97251403331756592, 0.68719005584716797, -1.5800620317459106, 1.4711480140686035, 0.45103797316551208, -0.18565596640110016, -0.97251403331756592, 0.14568804204463959, -0.20858204364776611, 2.112546443939209, -1.1229300498962402, 0.018366038799285889, 1.4465200901031494, -0.33437004685401917, -0.61364197731018066, 0.42487603425979614], [0.056716039776802063, 0.3159620463848114, 1.9359500408172607, 0.50012600421905518, -0.30070596933364868, -0.084411963820457458, -0.97251403331756592, 0.68719005584716797, -1.5800620317459106, 1.831637978553772, 0.1381019651889801, -0.19332596659660339, -0.97251403331756592, 0.14262004196643829, -0.20704804360866547, 2.112546443939209, -1.132133960723877, -0.016915962100028992, 1.4557241201400757, -0.33437004685401917, -0.61364197731018066, 0.42487603425979614], [0.056716039776802063, 0.3159620463848114, 1.9328820705413818, 0.50012600421905518, -0.30070596933364868, -0.084411963820457458, -0.98325204849243164, 0.68872404098510742, -1.5754599571228027, 2.112546443939209, -0.22392204403877258, -0.27616196870803833, -0.98325204849243164, 0.14415404200553894, -0.20858204364776611, 2.112546443939209, -1.1894419193267822, 0.0045600384473800659, 1.4833360910415649, -0.33437004685401917, -0.61057400703430176, 0.42487603425979614], [0.064386039972305298, 0.3159620463848114, 1.9313479661941528, 0.47711598873138428, -0.29917198419570923, -0.085945963859558105, -0.77616202831268311, 0.55066406726837158, -0.86521798372268677, 1.8837940692901611, -0.35431206226348877, -0.30837595462799072, -0.77616202831268311,-0.1381019651889801, -0.3451080322265625, 2.112546443939209, -1.1894419193267822, -0.10588795691728592, 1.6198620796203613, -0.33437004685401917, -0.61057400703430176, 0.42487603425979614], [0.065920040011405945, 0.31442803144454956, 1.9313479661941528, 0.47711598873138428, -0.29917198419570923, -0.085945963859558105, -0.74855005741119385, 0.41107004880905151, -0.53540796041488647, 2.0755441188812256, -0.88047409057617188, -0.22553996741771698, -0.74855005741119385, -0.16571396589279175, -0.37272006273269653, 2.112546443939209, -1.1894419193267822, -0.10435395687818527, 1.6290661096572876, -0.33437004685401917, -0.61057400703430176, 0.42487603425979614], [0.059784039855003357, 0.30982604622840881, 1.925212025642395, 0.4740479588508606, -0.29917198419570923, -0.085945963859558105, -0.75315207242965698, 0.40953606367111206, -0.55381596088409424, 2.1016221046447754, -0.92496007680892944, -0.16264596581459045, -0.75315207242965698, -0.17338396608829498, -0.38039004802703857, 2.112546443939209, -1.1894419193267822, -0.053731963038444519, 1.6428720951080322, -0.32363206148147583, -0.60903996229171753, 0.42180806398391724], [0.078192040324211121, 0.31289404630661011, 1.0615699291229248, 0.81612998247146606, -0.055265963077545166, -0.75783801078796387, -0.63656806945800781,0.25153404474258423, -0.22247196733951569, 1.7825499773025513, -0.77002608776092529, -0.27769595384597778, -0.63656806945800781, -0.45410597324371338, -0.47396403551101685, 2.112546443939209, -1.1894419193267822, -0.050663962960243225, 1.688892126083374, -0.32670003175735474, -0.4418339729309082, 0.034906584769487381], [0.078192040324211121, 0.31289404630661011, 1.0631040334701538, 0.81612998247146606, -0.055265963077545166, -0.75937199592590332, -0.69639408588409424, 0.081260040402412415, -0.05833396315574646, 1.5233039855957031, -0.63350003957748413, -0.30990996956825256, -0.69639408588409424, -0.51853394508361816, -0.64423805475234985, 2.112546443939209, -1.1894419193267822, -0.10588795691728592, 1.6781541109085083, -0.32670003175735474, -0.44336795806884766, 0.034906584769487381], [0.081260040402412415, 0.31289404630661011, 1.0723079442977905, 0.80999398231506348, -0.053731963038444519, -0.76243996620178223, -0.60895603895187378, -0.14270396530628204, -0.1381019651889801, 1.7181220054626465, -1.101370096206665, -0.024585962295532227, -0.60895603895187378, -0.5952339768409729, -0.78996807336807251, 2.112546443939209, -1.1894419193267822, -0.099751964211463928, 1.6996300220489502, -0.5966840386390686, -0.44796997308731079, 0.034906584769487381], [0.081260040402412415, 0.31289404630661011, 1.0738420486450195, 0.80999398231506348, -0.053731963038444519, -0.76243996620178223, -0.31749606132507324, -0.37126997113227844, -0.50932997465133667, 1.8500460386276245, -1.1335840225219727, 0.12574604153633118, -0.31749606132507324, -0.45870798826217651, -0.81297802925109863, 2.112546443939209, -1.1894419193267822, -0.10435395687818527, 1.6996300220489502, -0.5966840386390686, -0.44796997308731079, 0.034906584769487381], [0.081260040402412415, 0.31289404630661011, 1.0738420486450195, 0.80999398231506348, -0.053731963038444519, -0.76243996620178223, -0.04291003942489624, -0.37126997113227844, -0.68573999404907227, 1.8193659782409668, -1.0768260955810547, 0.12267804145812988, -0.04291003942489624, -0.28536596894264221, -0.83752202987670898, 2.112546443939209, -1.1894419193267822, -0.092081964015960693, 1.6996300220489502, -0.5966840386390686, -0.44796997308731079, 0.034906584769487381] ] for i in range(len(anglelist[0])): # 1 2 3 4 5 6 7 8 9 10 11 12 13 times.append([0.5, 1, 1.5, 2 ,2.5, 3, 3.5, 4, 4.3, 4.6, 4.9, 5.1, 5.5]) oneangle = list() for j in range(len(anglelist)): oneangle.append(anglelist[j][i]) angles.append(oneangle) angleInterpolation('Body', angles, times, True) stance() # sidestep right (keeper) def footRight(): names = list() times = list() angles = list() names = list() times = list() angles = list() names.extend( ['LShoulderPitch','LShoulderRoll','LElbowYaw','LElbowRoll','RShoulderPitch','RShoulderRoll']) angles.extend([[1.2], [0.43], [-0.61], [-0.44], [1.9], [-0.5 ] ]) times.extend( [[0.75], [0.75], [0.75], [0.75], [0.4], [0.4 ] ]) names.extend( ['RElbowYaw','RElbowRoll','LHipYawPitch','LHipRoll','LHipPitch','LKneePitch','LAnklePitch']) angles.extend([[-0.3 ], [-0.05], [-0.98], [-0.1426], [-0.222], [2.11], [-0.9]]) times.extend( [[ 0.4], [0.4], [0.6], [0.75], [0.75 ] , [0.75], [0.75]]) names.extend( ['LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'] ) angles.extend([[-0.079], [-0.68], [-1.5,-1.1], [0.7, 0.0], [1.0, 2.0], [0.05]] ) times.extend( [[0.75], [0.75], [0.75, 1.2], [0.75,1.2], [0.75, 1.2], [0.75]] ) angleInterpolation(names, angles, times , True) # wait to stop the ball time.sleep(1) angles = list() times = list() anglelist = [[0.056716039776802063, 0.31442803144454956, 1.3023240566253662, 0.33437004685401917, -0.61364197731018066, -0.42641004920005798, -0.95717406272888184, -0.14568804204463959, -0.24233004450798035, 2.112546443939209, -0.88209199905395508, -0.0076280385255813599, -0.95717406272888184, -0.68565607070922852, -1.3116120100021362, 0.69034194946289062, 0.93200582265853882, 0.001575961709022522, 1.9497560262680054, -0.4939899742603302, -0.30070596933364868, 0.09361596405506134], [0.05825003981590271, 0.3159620463848114, 1.3406740427017212, 0.33590406179428101, -0.61364197731018066, -0.42487603425979614, -0.96484404802322388, -0.14568804204463959, -0.21165004372596741, 2.112546443939209, -0.90816998481750488, -0.0076280385255813599, -0.96484404802322388, -0.68565607070922852, -1.5647220611572266, 1.2747960090637207, 0.6535259485244751, 4.1961669921875e-05, 1.9497560262680054, -0.50012600421905518, -0.29917198419570923, 0.090547963976860046], [0.056716039776802063, 0.31442803144454956, 1.4465200901031494, 0.33437004685401917, -0.61364197731018066, -0.42487603425979614, -0.97251403331756592, -0.14568804204463959, -0.20858204364776611, 2.112546443939209, -1.1229300498962402, -0.018366038799285889, -0.97251403331756592, -0.68719005584716797, -1.5800620317459106, 1.4711480140686035, 0.45103797316551208, 0.18565596640110016, 1.9359500408172607, -0.50012600421905518, -0.30070596933364868, 0.085945963859558105], [0.056716039776802063, 0.3159620463848114, 1.4557241201400757, 0.33437004685401917, -0.61364197731018066, -0.42487603425979614, -0.97251403331756592, -0.14262004196643829, -0.20704804360866547, 2.112546443939209, -1.132133960723877, 0.016915962100028992, -0.97251403331756592, -0.68719005584716797, -1.5800620317459106, 1.831637978553772, 0.1381019651889801, 0.19332596659660339, 1.9359500408172607, -0.50012600421905518, -0.30070596933364868, 0.084411963820457458], [0.056716039776802063, 0.3159620463848114, 1.4833360910415649, 0.33437004685401917, -0.61057400703430176, -0.42487603425979614, -0.98325204849243164, -0.14415404200553894, -0.20858204364776611, 2.112546443939209, -1.1894419193267822, -0.0045600384473800659, -0.98325204849243164, -0.68872404098510742, -1.5754599571228027, 2.112546443939209, -0.22392204403877258, 0.27616196870803833, 1.9328820705413818, -0.50012600421905518, -0.30070596933364868, 0.084411963820457458], [0.064386039972305298, 0.3159620463848114, 1.6198620796203613, 0.33437004685401917, -0.61057400703430176, -0.42487603425979614, -0.77616202831268311, 0.1381019651889801, -0.3451080322265625, 2.112546443939209, -1.1894419193267822, 0.10588795691728592, -0.77616202831268311, -0.55066406726837158, -0.86521798372268677, 1.8837940692901611, -0.35431206226348877, 0.30837595462799072, 1.9313479661941528, -0.47711598873138428, -0.29917198419570923, 0.085945963859558105], [0.065920040011405945, 0.31442803144454956, 1.6290661096572876, 0.33437004685401917, -0.61057400703430176, -0.42487603425979614, -0.74855005741119385, 0.16571396589279175, -0.37272006273269653, 2.112546443939209, -1.1894419193267822, 0.10435395687818527, -0.74855005741119385, -0.41107004880905151, -0.53540796041488647, 2.0755441188812256, -0.88047409057617188, 0.22553996741771698, 1.9313479661941528, -0.47711598873138428, -0.29917198419570923, 0.085945963859558105], [0.059784039855003357, 0.30982604622840881, 1.6428720951080322, 0.32363206148147583, -0.60903996229171753, -0.42180806398391724, -0.75315207242965698, 0.17338396608829498, -0.38039004802703857, 2.112546443939209, -1.1894419193267822, 0.053731963038444519, -0.75315207242965698, -0.40953606367111206, -0.55381596088409424, 2.1016221046447754, -0.92496007680892944, 0.16264596581459045, 1.925212025642395, -0.4740479588508606, -0.29917198419570923, 0.085945963859558105], [0.078192040324211121, 0.31289404630661011, 1.688892126083374, 0.32670003175735474, -0.4418339729309082, -0.034906584769487381, -0.63656806945800781, 0.45410597324371338, -0.47396403551101685, 2.112546443939209, -1.1894419193267822, 0.050663962960243225, -0.63656806945800781, -0.25153404474258423, -0.22247196733951569, 1.7825499773025513, -0.77002608776092529, 0.27769595384597778, 1.0615699291229248, -0.81612998247146606, -0.055265963077545166, 0.75783801078796387], [0.078192040324211121, 0.31289404630661011, 1.6781541109085083, 0.32670003175735474, -0.44336795806884766, -0.034906584769487381, -0.69639408588409424, 0.51853394508361816, -0.64423805475234985, 2.112546443939209, -1.1894419193267822, 0.10588795691728592, -0.69639408588409424, -0.081260040402412415, -0.05833396315574646, 1.5233039855957031, -0.63350003957748413, 0.30990996956825256, 1.0631040334701538, -0.81612998247146606, -0.055265963077545166, 0.75937199592590332], [0.081260040402412415, 0.31289404630661011, 1.6996300220489502, 0.5966840386390686, -0.44796997308731079, -0.034906584769487381, -0.60895603895187378, 0.5952339768409729, -0.78996807336807251, 2.112546443939209, -1.1894419193267822, 0.099751964211463928, -0.60895603895187378, 0.14270396530628204, -0.1381019651889801, 1.7181220054626465, -1.101370096206665, 0.024585962295532227, 1.0723079442977905, -0.80999398231506348, -0.053731963038444519, 0.76243996620178223], [0.081260040402412415, 0.31289404630661011, 1.6996300220489502, 0.5966840386390686, -0.44796997308731079, -0.034906584769487381, -0.31749606132507324, 0.45870798826217651, -0.81297802925109863, 2.112546443939209, -1.1894419193267822, 0.10435395687818527, -0.31749606132507324, 0.37126997113227844, -0.50932997465133667, 1.8500460386276245, -1.1335840225219727, -0.12574604153633118, 1.0738420486450195, -0.80999398231506348, -0.053731963038444519, 0.76243996620178223], [0.081260040402412415, 0.31289404630661011, 1.6996300220489502, 0.5966840386390686, -0.44796997308731079, -0.034906584769487381, -0.04291003942489624, 0.28536596894264221, -0.83752202987670898, 2.112546443939209, -1.1894419193267822, 0.092081964015960693, -0.04291003942489624, 0.37126997113227844, -0.68573999404907227, 1.8193659782409668, -1.0768260955810547, -0.12267804145812988, 1.0738420486450195, -0.80999398231506348, -0.053731963038444519, 0.76243996620178223]] for i in range(len(anglelist[0])): # 1 2 3 4 5 6 7 8 9 10 11 12 13 times.append([0.5, 1, 1.5, 2 ,2.5, 3, 3.5, 4, 4.3, 4.6, 4.9, 5.1, 5.5]) oneangle = list() for j in range(len(anglelist)): oneangle.append(anglelist[j][i]) angles.append(oneangle) angleInterpolation('Body', angles, times, True) stance() # return headposition [yaw, pitch] def getHeadPos(): return motProxy.getAngles(['HeadYaw', 'HeadPitch'], True) # return nao's current pose def getPose(): return posProxy.getPostureFamily() # hakje fails def hak(): stiff() time.sleep(1) walkTo(0.001, 0, 0) names = list() angles = list() times = list() names.append('LHipPitch') angles.append([-0.4, -1.2, -1.0, -0.85, -0.25, -0.1]) times.append( [ 0.5, 1.0, 1.5, 2.0, 3.0, 4.0]) names.append('LKneePitch') angles.append([0.95, 1.2, 0.8, 0.7, 0.0]) times.append( [0.5, 1.0, 1.5, 2.0, 3.0]) names.append('LAnklePitch') angles.append([-0.55, 0.0, 0.0, -0.15, -0.05, 0.0]) times.append( [ 0.5 , 1.0, 1.5, 2.0, 3.0, 4.0]) names.append('LShoulderRoll') angles.append([0.1, 1]) times.append([1.3, 2.0]) names.append('RHipPitch') angles.append([-0.4, -0.4, -0.2, 0.1, 0.5, 0.5, -0.9]) times.append( [ 0.5, 1.0, 1.5, 2.0, 3.0, 4.0, 5.0]) names.append('RKneePitch') angles.append([0.95, 0.95, 0.6, 0.3, 2.0]) times.append( [0.5 , 1.0 , 1.5, 2.0, 4.0]) names.append('RAnklePitch') angles.append([-0.55, -0.55, -0.55, -0.3, -0.3, -1.2]) times.append( [ 0.5 , 1.0 , 1.5 , 3.0, 4.0, 4.5]) names.append('RAnkleRoll') angles.append([-0.12, -0.12, 0.1, 0.2]) times.append( [ 0.5 , 1.2, 2.0, 4.5]) names.append('LAnkleRoll') angles.append([-0.12, 0.0, 0.15, 0.2]) times.append( [ 0.5, 1.1, 2.0, 4.5]) angleInterpolation(names, angles, times, True) time.sleep(1) # check to see if nao has fallen if isLyingBack(): kill() return 0 elif isLyingBelly(): kill() return 0 # leg over ball, heel angleInterpolation(['LHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch'], \ [[0.0 ,0.0], [-0.5, 0], [-1.2, -0.2], [0.2, 0.5], [0.6, -0.4] ], \ [[0.2, 0.8], [0.3, 0.6], [0.7, 0.8], [0.7, 0.9], [0.7, 0.8] ], True) angleInterpolation(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'], \ [[0], [0], [-0.4], [0.95], [-0.55], [0], [0], [-0.4], [0.95], [-0.55], [0]], \ [[0.65], [0.85], [0.85], [0.85], [0.85], [0.85], [0.85], [0.85], [0.85], [0.65], [0.75]], True) # MAX ANGLE 0.3!!!! def hakje(angle): # motProxy.setAngles(['RAnkleRoll', 'LAnkleRoll'], [-0.2, -0.2], 0.2) # time.sleep(0.2) # step motProxy.setStiffnesses('Body', 1) motProxy.setStiffnesses('Head', 0.5) names = list() angles = list() times = list() names.extend(['RElbowRoll', 'RElbowYaw', 'LElbowRoll', 'LElbowYaw']) angles.extend([[0], [0], [0], [0]]) times.extend([[0.1], [0.1], [0.1], [0.1]]) names.append('RShoulderPitch') angles.append( [ 1.2, 0.8, 0.5, 0.8 ]) times.append([ 0.1, 1.3, 2 , 3.5]) names.append('RShoulderRoll') angles.append( [-0.2, -0.2]) times.append([0.1, 1.1]) names.append('LShoulderPitch') angles.append( [ 1.2, 0.8, 0.5, 0.8 ]) times.append([ 0.1, 1.3, 2 , 3.5]) names.append('LShoulderRoll') angles.append( [0.3, 0.4, 1]) times.append([0.1, 0.8, 1.1]) names.append('LHipPitch',) angles.append( [-0.5, -0.8, -0.4, 0]) times.append([0.4, 0.7, 1.1 , 1.7]) names.append('LKneePitch') angles.append( [0.95, 1.13, 0.5, 0 ]) times.append([0.4, 0.7, 1.1, 1.4 ]) names.append('LAnklePitch') angles.append( [-0.2, -0.4, -0.1, -0.025, 0]) times.append([0.4, 0.8, 1.2, 1.4, 2.5]) names.append('LAnkleRoll') angles.append([-0.11, 0.13, 0.2, ]) times.append([0.3, 0.95, 1.2, ]) names.append('RHipPitch') angles.append([-0.4, -0.22, 0.5, 0.5, -1]) times.append([0.4, 0.8, 1.2, 2, 3]) names.append('RKneePitch') angles.append( [ 0.95, 0.95, 0.65, 2 ]) times.append([ 0.4, 0.8, 1.2, 3]) names.append( 'RAnklePitch') angles.append( [-0.5, -0.67, -1.1, -1 ]) times.append([ 0.4, 0.8, 1.2, 3]) names.append('RAnkleRoll') angles.append( [-0.11, -0.12, 0.2,]) # 0.23, 0.225]) times.append([ 0.3, 0.7, 1.1, ]) # 1.5, 2.3]) # prepare to kick angleInterpolation(names, angles, times, True) time.sleep(1) # check to see if nao has fallen if isLyingBack(): kill() return 0 elif isLyingBelly(): kill() return 0 # leg over ball, heel angleInterpolation(['LHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch','RShoulderPitch','LShoulderPitch'], \ [[angle, angle], [-0.5, 0], [-1.2, -0.2], [0.2, 0.5], [0.6, -0.4], [1.5, 0.6], [1.5, 0.6]], \ [[0.2, 0.8], [0.3, 0.6], [0.7, 0.8], [0.7, 0.9], [0.7, 0.8], [0.3, 0.8], [0.3, 0.8]] , True) motProxy.setAngles(['LShoulderRoll', 'RShoulderRoll', 'LShoulderPitch', 'RShoulderPitch'], [0.1, -0.1, 1.5, 1.5], 0.2) angleInterpolation(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'], \ [ [0], [0], [-0.4], [0.95], [-0.55], [0], [0], [-0.4], [0.95], [-0.55], [0] ], \ [ [0.65], [0.85], [0.85], [0.85], [0.85], [0.85], [0.85], [0.85], [0.85], [0.65], [0.75] ], True) stiff() # return if nao is walking def isWalking(): return motProxy.walkIsActive() # a normal pose for the keeper which prevents overheating (hopefully) def keepNormalPose(): anglelist = motProxy.getAngles(['RHipPitch','RKneePitch', 'RAnklePitch'], True) if not( getPose() == 'Stand' and -0.01 <= anglelist[0] <= 0.01 and -0.01 <= anglelist[1] <= 0.01 and -0.01 <= anglelist[2] <= 0.01 ): names = list() times = list() angles = list() names.extend( ['LShoulderPitch','LShoulderRoll','LElbowYaw','LElbowRoll','RShoulderPitch','RShoulderRoll']) angles.extend([[1.2], [0.15], [0.0], [0.0], [1.2], [-0.15] ]) times.extend( [[1.0], [1.0], [1.0], [1.0], [1.0], [1.0 ] ]) names.extend( ['RElbowYaw','RElbowRoll','LHipYawPitch','LHipRoll','LHipPitch','LKneePitch','LAnklePitch']) angles.extend([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]]) times.extend( [[1.0], [1.0], [1.0], [1.0], [1.0], [1.0], [1.0]]) names.extend( ['LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'] ) angles.extend([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]] ) times.extend( [[1.0], [1.0], [1.0], [1.0], [1.0], [1.0]] ) angleInterpolation(names, angles, times, True) # kick towards front, right leg def kick(angle): # angle slightly positive, kick towards left with rightleg if 0 <= angle < 0.2: names = ['RShoulderRoll', 'RShoulderPitch', 'LShoulderRoll', 'LShoulderPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', \ 'RAnklePitch', 'RAnkleRoll', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll'] angles = [[-0.3], [0.4], [1.0], [1.0], [0.0], [-0.4, -0.2], [0.95, 1.5], [-0.55, -1], [0.2], [0.0], [-0.4], [0.95], [-0.55], [0.2]] times = [[ 0.5], [0.5], [0.5], [0.5], [0.5], [ 0.4, 0.8], [ 0.4, 0.8], [0.4, 0.8], [0.4], [0.5], [ 0.4], [ 0.4], [ 0.4], [0.4]] angleInterpolation(names, angles, times, True) angleInterpolation(['RShoulderPitch', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'LShoulderPitch'], [1.0, -0.7, 1.05, -0.5, 0.2], [[0.1], [0.1], [0.1], [0.1], [0.1]], True) angleInterpolation(['RHipPitch', 'RKneePitch', 'RAnklePitch'], \ [-0.5, 1.1, -0.65], [[0.25], [0.25], [0.25]], True) normalPose(True) # kick towards front, left leg elif -0.2 < angle < 0: names = ['LShoulderRoll', 'LShoulderPitch', 'RShoulderRoll', 'RShoulderPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', \ 'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'] angles = [[0.3], [0.4], [-0.5], [1.0], [0.0], [-0.4, -0.2], [0.95, 1.5], [-0.55, -1], [-0.2], [0.0], [-0.4], [0.95], [-0.55], [-0.2]] times = [[0.5], [0.5], [ 0.5], [0.5], [0.5], [ 0.4, 0.8], [ 0.4, 0.8], [ 0.4, 0.8], [ 0.4], [0.5], [ 0.4], [0.4] , [0.4], [0.4]] angleInterpolation(names, angles, times, True) angleInterpolation(['LShoulderPitch', 'LHipPitch', 'LKneePitch', 'LAnklePitch'], [1.0, -0.7, 1.05, -0.5], [[0.1], [0.1], [0.1], [0.1]], True) angleInterpolation(['LHipPitch', 'LKneePitch', 'LAnklePitch'], [-0.5, 1.1, -0.65], [[0.25], [0.25], [0.25]], True) normalPose(True) # remove stiffness from body def kill(): motProxy.setStiffnesses('Body', 0) # remove knee stiffness def killKnees(): if motProxy.getStiffnesses('RKneePitch')[0] > 0: motProxy.post.stiffnessInterpolation(['RKneePitch', 'LKneePitch'], [[0], [0]], [[0.15], [0.15]]) angleInterpolation(['RHipPitch', 'LHipPitch'], [[-1.1], [-1.1]], [[0.35],[0.35]], True) # stop walking if active def killWalk(): if motProxy.walkIsActive(): walkTo(0.00001, 0, 0) # left kick with inputangle def lKickAngled(angle): motProxy.setAngles(['RShoulderRoll', 'RShoulderPitch', 'LShoulderRoll', 'LShoulderPitch', 'RElbowRoll', 'RElbowYaw', 'LElbowRoll', 'LElbowYaw'], [-0.5 - 0.2*angle, 0.8, 0.3, 0.8, 0, 0, 0, 0], 0.3) # Turn on left foot motProxy.setAngles(['RAnkleRoll', 'LAnkleRoll'], [-0.185 - 0.025 * angle, -0.2], 0.2) time.sleep(0.3) # Left leg stretched, right leg goes backwards angleInterpolation(['LHipYawPitch', 'RHipRoll', 'RKneePitch', 'RAnklePitch', 'LHipRoll', 'RHipPitch', 'LHipPitch', 'LKneePitch', 'LAnklePitch'], [[0.65*angle], [-0.2*angle], [0.175 + 0.25*angle], [-0.125 -0.125*angle], [-0.075 +0.295*angle], [-0.3 - 0.05*angle], [0.4], [0.5 - 0.35*angle], [-0.6 + 0.5*angle]], [[0.8], [0.9], [0.6], [0.6], [1.1], [0.6], [1.1], [1.2], [1.2]], True) time.sleep(0.3) if isLyingBack(): kill() return 0 elif isLyingBelly(): kill() return 0 # kick motProxy.setAngles('LShoulderPitch', 2, 1) motProxy.setAngles(['LHipYawPitch', 'RHipRoll', 'RHipPitch', 'LHipPitch', 'LKneePitch', 'LAnklePitch'], [0.75*angle, -0.1, -0.1 -0.25*angle , -0.2 + -0.25 * angle, 0, 0.1 - 0.075*angle], 0.9) time.sleep(0.4 + 0.3 * angle) # return to start position angleInterpolation(['LHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll'], [[0], [0],[-0.4], [0.95], [-0.55],[-0.15], [0], [-0.4], [0.95], [-0.55], [-0.15]], [[0.65],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.65],[0.75]], True) time.sleep(0.2) motProxy.setAngles(['RShoulderRoll', 'RShoulderPitch', 'LShoulderRoll', 'LShoulderPitch'], [0, 1.2, 0, 1.2], 0.4) motProxy.setAngles(['RAnkleRoll', 'LAnkleRoll'], [0, 0], 0.05) # a normal pose from which walking is almost immediately possible def normalPose(force = False): anglelist = motProxy.getAngles(['RHipPitch','RKneePitch', 'RAnklePitch'],True) if force or not( getPose() == 'Stand' and -0.41 <= anglelist[0] <= -0.39 and 0.94 <= anglelist[1] <= 0.96 and -0.56 <= anglelist[2] <= -0.54): names = list() times = list() angles = list() names.extend( ['LShoulderPitch','LShoulderRoll','LElbowYaw','LElbowRoll','RShoulderPitch','RShoulderRoll']) angles.extend([[1.2], [0.15], [0.0], [0.0], [1.2], [-0.15] ]) times.extend( [[1.0], [1.0], [1.0], [1.0], [1.0], [1.0 ] ]) names.extend( ['RElbowYaw','RElbowRoll','LHipYawPitch','LHipRoll','LHipPitch','LKneePitch','LAnklePitch']) angles.extend([[0.0], [0.0], [0.0], [0.0], [-0.4], [0.95], [-0.55]]) times.extend( [[1.0], [1.0], [1.0], [1.0], [1.0], [1.0], [1.0]]) names.extend( ['LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'] ) angles.extend([[0.0], [0.0], [-0.4], [0.95], [-0.55], [0.0]] ) times.extend( [[1.0], [1.0], [1.0], [1.0], [1.0], [1.0]] ) angleInterpolation(names, angles, times, True) # non-blocking walk call def postWalkTo(x, y, angle): motProxy.post.walkTo(x, y, angle) # right kick with inputangle def rKickAngled(angle): motProxy.setAngles(['LShoulderRoll', 'LShoulderPitch', 'RShoulderRoll', 'RShoulderPitch', 'RElbowRoll', 'RElbowYaw', 'LElbowRoll', 'LElbowYaw'], [0.5 + 0.2*angle, 0.8, -0.3, 0.8, 0, 0, 0, 0], 0.3) # Turn on left foot motProxy.setAngles(['LAnkleRoll', 'RAnkleRoll'], [0.185 + 0.025 * angle, 0.2], 0.2) time.sleep(0.3) # Left leg stretched, right leg goes backwards angleInterpolation(['LHipYawPitch', 'LHipRoll', 'LKneePitch', 'LAnklePitch', 'RHipRoll', 'LHipPitch','RHipPitch', 'RKneePitch', 'RAnklePitch'], [[0.65*angle], [0.2*angle], [0.175 + 0.25*angle], [-0.125 -0.125*angle], [0.075 -0.295*angle], [-0.3 - 0.05*angle], [0.4], [0.5 - 0.35*angle], [-0.6 + 0.5*angle]], [[0.8], [0.9], [0.6], [0.6], [1.1], [0.6], [1.1], [1.2], [1.2]], True) time.sleep(0.3) if isLyingBack(): kill() return 0 elif isLyingBelly(): kill() return 0 # kick motProxy.setAngles('RShoulderPitch', 2, 1) motProxy.setAngles(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'RHipPitch', 'RKneePitch', 'RAnklePitch'], [0.75*angle, 0.1, -0.1 -0.25*angle , -0.2 + -0.25 * angle, 0, 0.1 - 0.075*angle], 0.9) time.sleep(0.4 + 0.3 * angle) # return to start position angleInterpolation(['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'], [[0], [0],[-0.4], [0.95], [-0.55],[0.15], [0], [-0.4], [0.95], [-0.55], [0.15]], [[0.65],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.85],[0.65],[0.75]], True) time.sleep(0.2) motProxy.setAngles(['LShoulderRoll', 'LShoulderPitch', 'RShoulderRoll', 'RShoulderPitch'], [0, 1.2, 0, 1.2], 0.4) motProxy.setAngles(['LAnkleRoll', 'RAnkleRoll'], [0, 0], 0.05) # non blocking call, absolute def setHead(yaw, pitch): motProxy.setAngles(['HeadYaw', 'HeadPitch'], [yaw, pitch], 0.5) # soft kick towards right, left leg def sideLeftKick(): angleInterpolation(['RAnkleRoll', 'LAnkleRoll'], [-0.2, -0.2], [[0.4], [0.4]], True) names = list() angles = list() times = list() namees = ['RShoulderRoll','LHipRoll', 'LHipPitch', 'LKneePitch','LAnklePitch','LAnkleRoll','RHipRoll','RHipPitch','RKneePitch','RAnklePitch','RAnkleRoll'] angles = [[-0.4], [0.0, 0.3], [-0.4, -0.8],[0.95, 0.2], [-0.55, 0.6], [-0.25,-0.3],[0.05 ], [-0.4], [0.95], [-0.5], [-0.25]] times = [[0.2], [0.3, 0.75],[0.5, 1.0],[0.5, 1.0], [ 0.5, 1.0], [0.5, 1.0],[ 1.0], [0.5], [0.5 ], [ 0.5], [ 0.5]] angleInterpolation(names, angles, times, True) angleInterpolation('LHipRoll', [-0.05], [0.1], True) time.sleep(0.1) angleInterpolation(['LHipRoll','LHipPitch','LKneePitch','LAnklePitch'], [[0.0], [-0.55], [1.2], [-0.6]], [[0.5], [0.6], [0.6], [0.6]], True) normalPose(True) # soft kick towards left, right leg # soft kick towards left, right leg def sideRightKick(): angleInterpolation(['RAnkleRoll', 'LAnkleRoll'], [0.2, 0.2], [[0.4], [0.4]], True) names = ['LShoulderRoll','RHipRoll','RHipPitch','RKneePitch','RAnklePitch','RAnkleRoll','LHipRoll','LHipPitch','LKneePitch','LAnklePitch','LAnkleRoll'] angles = [[0.4], [0, -0.3],[-0.4, -0.8],[0.95, 0.2],[-0.55, 0.6], [0.25, 0.3], [-0.05], [-0.4], [0.95], [-0.5], [0.25]] times = [[0.2], [0.3, 0.75 ],[0.5, 1], [0.5, 1], [0.5, 1], [0.5, 1], [1.0], [0.5], [0.5], [0.5], [0.5]] angleInterpolation(names, angles, times, True) angleInterpolation('RHipRoll', [0.05], [0.1], True) time.sleep(0.1) angleInterpolation(['RHipRoll','RHipPitch', 'RKneePitch', 'RAnklePitch'], [[0.0], [-0.55], [1.2], [-0.6]], [[0.5], [0.6], [0.6], [0.6]], True) normalPose(True) # keeper stance def stance(): anglelist = motProxy.getAngles(['RHipPitch','RKneePitch', 'RAnklePitch', 'LHipPitch', 'LKneePitch'],True) ### TODO 2015-11-23: Ugly hack: what does this if do? #if not( -0.91 <= anglelist[0] <= 0.89 and 2.11 <= anglelist[1] <= 2.13 and # -1.21 <= anglelist[2] <= 1.21 and -0.91 <= anglelist[3] <= 0.89 and 2.11 <= anglelist[4] <= 2.13): if True: names = list() angles = list() times = list() names.append('LLeg') angles.extend([0, 0, -0.9, 2.12, -1.2, 0]) times.extend([0.7, 0.7, 0.7, 0.7, 0.7, 0.7]) names.append('RLeg') angles.extend([0, 0, -0.9, 2.12, -1.2, 0]) times.extend([0.7, 0.7, 0.7, 0.7, 0.7, 0.7]) names.append('RArm') angles.extend([1.2, -0.1, 0, 0]) times.extend([0.7, 0.7, 0.7, 0.7]) names.append('LArm') angles.extend([1.2, 0.1, 0, 0]) times.extend([0.7, 0.7, 0.7, 0.7]) angleInterpolation(names, angles, times, True) # stand up if fallen down, return fallen==True def standUp(): fallen = False if isLyingBack(): stiff() backToStand() fallen = True elif isLyingBelly(): stiff() bellyToStand() fallen = True return fallen # activate stiffness def stiff(): if motProxy.getStiffnesses('HeadPitch') != 0: if BODY_VERSION[0] > 3: motProxy.setStiffnesses(['RArm', 'LArm', 'LLeg', 'RLeg', 'Head'], [0.6, 0.6, 0.6, 0.6, 0.6, 0.6, #RArm 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, #LArm 0.95, 0.95, 0.95, 0.95, 0.95, 0.95, #LLeg 0.95, 0.95, 0.95, 0.95, 0.95, 0.95, #RLeg 0.6, 0.6]) #Head else: motProxy.setStiffnesses(['RArm', 'LArm', 'LLeg', 'RLeg', 'Head'], [0.6, 0.6, 0.6, 0.6, #RArm 0.6, 0.6, 0.6, 0.6, #LArm 0.95, 0.95, 0.95, 0.95, 0.95, 0.95, #LLeg 0.95, 0.95, 0.95, 0.95, 0.95, 0.95, #RLeg 0.6, 0.6]) #Head # set stiffness for knees again def stiffKnees(): if motProxy.getStiffnesses('RKneePitch')[0] < 0.95: motProxy.post.stiffnessInterpolation(['RKneePitch', 'LKneePitch'], [[0.95], [0.95]], [[0.15], [0.15]]) angleInterpolation(['RHipPitch', 'LHipPitch'], [[-1.0], [-1.0]], [[0.35],[0.35]], True) motProxy.setStiffnesses(['RKneePitch','LKneePitch','RAnklePitch','LAnklePitch'], 0.95) while motProxy.getStiffnesses ('RKneePitch') < 0.95: pass if motProxy.getStiffnesses('RKneePitch')[0] < 0.95: motProxy.post.stiffnessInterpolation(['RKneePitch', 'LKneePitch'], [[0.95], [0.95]], [[0.05], [0.05]]) angleInterpolation(['RHipPitch', 'LHipPitch'], [[-1.0], [-1.0]], [[0.15],[0.15]], True) # setWalkTargetVelocity function as specified in documentation (with check for valid parameters) def SWTV(x,y,t,f): if x > 1: x = 1 if y > 1: y = 1 if y < -1: y = -1 if x >= -1: motProxy.setWalkTargetVelocity(x,y,t,f) # blocking walk call def walkTo(x,y,angle): motProxy.walkTo(x,y,angle) if __name__ == '__main__': footLeft()