Init gym

behaviors/Behavior.py

@@ -0,0 +1,176 @@
+import numpy as np
+
+
+class Behavior():
+
+    def __init__(self, base_agent) -> None:
+        from agent.Base_Agent import Base_Agent  # for type hinting
+        self.base_agent: Base_Agent = base_agent
+        self.world = self.base_agent.world
+        self.state_behavior_name = None
+        self.state_behavior_init_ms = 0
+        self.previous_behavior = None
+        self.previous_behavior_duration = None
+
+        # Initialize standard behaviors
+        from behaviors.Poses import Poses
+        from behaviors.Slot_Engine import Slot_Engine
+        from behaviors.Head import Head
+
+        self.poses = Poses(self.world)
+        self.slot_engine = Slot_Engine(self.world)
+        self.head = Head(self.world)
+
+    def create_behaviors(self):
+        '''
+        Behaviors dictionary:
+            creation:   key: ( description, auto_head, lambda reset[,a,b,c,..]: self.execute(...), lambda: self.is_ready(...) )
+            usage:      key: ( description, auto_head, execute_func(reset, *args), is_ready_func )
+        '''
+        self.behaviors = self.poses.get_behaviors_callbacks()
+        self.behaviors.update(self.slot_engine.get_behaviors_callbacks())
+        self.behaviors.update(self.get_custom_callbacks())
+
+    def get_custom_callbacks(self):
+        '''
+        Searching custom behaviors could be implemented automatically
+        However, for code distribution, loading code dynamically is not ideal (unless we load byte code or some other import solution)
+        Currently, adding custom behaviors is a manual process:
+            1. Add import statement below
+            2. Add class to 'classes' list
+        '''
+
+        # Declaration of behaviors
+        from behaviors.custom.Basic_Kick.Basic_Kick import Basic_Kick
+        from behaviors.custom.Dribble.Dribble import Dribble
+        from behaviors.custom.Fall.Fall import Fall
+        from behaviors.custom.Get_Up.Get_Up import Get_Up
+        from behaviors.custom.Step.Step import Step
+        from behaviors.custom.Walk.Walk import Walk
+        from behaviors.custom.Nerul_Kick.Nerul_Kick import Nerul_Kick
+        from behaviors.custom.Kick_RL.Kick_RL import Kick_RL
+
+        classes = [Basic_Kick, Dribble, Fall, Get_Up, Step, Walk, Nerul_Kick, Kick_RL]
+
+        '''---- End of manual declarations ----'''
+
+        # Prepare callbacks
+        self.objects = {cls.__name__: cls(self.base_agent) for cls in classes}
+
+        return {name: (o.description, o.auto_head,
+                       lambda reset, *args, o=o: o.execute(reset, *args), lambda *args, o=o: o.is_ready(*args)) for
+                name, o in self.objects.items()}
+
+    def get_custom_behavior_object(self, name):
+        ''' Get unique object from class "name" ("name" must represent a custom behavior) '''
+        assert name in self.objects, f"There is no custom behavior called {name}"
+        return self.objects[name]
+
+    def get_all_behaviors(self):
+        ''' Get name and description of all behaviors '''
+        return [key for key in self.behaviors], [val[0] for val in self.behaviors.values()]
+
+    def get_current(self):
+        ''' Get name and duration (in seconds) of current behavior '''
+        duration = (self.world.time_local_ms - self.state_behavior_init_ms) / 1000.0
+        return self.state_behavior_name, duration
+
+    def get_previous(self):
+        ''' Get name and duration (in seconds) of previous behavior '''
+        return self.previous_behavior, self.previous_behavior_duration
+
+    def force_reset(self):
+        ''' Force reset next executed behavior '''
+        self.state_behavior_name = None
+
+    def execute(self, name, *args) -> bool:
+        ''' 
+        Execute one step of behavior `name` with arguments `*args`
+        - Automatically resets behavior on first call
+        - Call get_current() to get the current behavior (and its duration)
+
+        Returns
+        -------
+        finished : bool
+            True if behavior has finished
+        '''
+
+        assert name in self.behaviors, f"Behavior {name} does not exist!"
+
+        # Check if transitioning from other behavior
+        reset = bool(self.state_behavior_name != name)
+        if reset:
+            if self.state_behavior_name is not None:
+                self.previous_behavior = self.state_behavior_name  # Previous behavior was interrupted (did not finish)
+            self.previous_behavior_duration = (self.world.time_local_ms - self.state_behavior_init_ms) / 1000.0
+            self.state_behavior_name = name
+            self.state_behavior_init_ms = self.world.time_local_ms
+
+        # Control head orientation if behavior allows it
+        if self.behaviors[name][1]:
+            self.head.execute()
+
+        # Execute behavior
+        if not self.behaviors[name][2](reset, *args):
+            return False
+
+        # The behavior has finished
+        self.previous_behavior = self.state_behavior_name  # Store current behavior name
+        self.state_behavior_name = None
+        return True
+
+    def execute_sub_behavior(self, name, reset, *args):
+        '''
+        Execute one step of behavior `name` with arguments `*args`
+        Useful for custom behaviors that call other behaviors
+        - Behavior reset is performed manually
+        - Calling get_current() will return the main behavior (not the sub behavior)
+        - Poses ignore the reset argument
+
+        Returns
+        -------
+        finished : bool
+            True if behavior has finished
+        '''
+
+        assert name in self.behaviors, f"Behavior {name} does not exist!"
+
+        # Control head orientation if behavior allows it
+        if self.behaviors[name][1]:
+            self.head.execute()
+
+        # Execute behavior
+        return self.behaviors[name][2](reset, *args)
+
+    def execute_to_completion(self, name, *args):
+        ''' 
+        Execute steps and communicate with server until completion 
+        - Slot behaviors indicate that the behavior has finished when sending the last command (which is promptly sent)
+        - Poses are finished when the server returns the desired robot state (so the last command is irrelevant)
+        - For custom behaviors, we assume the same logic, and so, the last command is ignored
+
+        Notes
+        -----
+        - Before exiting, the `Robot.joints_target_speed` array is reset to avoid polluting the next command
+        - For poses and custom behaviors that indicate a finished behavior on the 1st call, nothing is committed or sent
+        - Warning: this function may get stuck in an infinite loop if the behavior never ends
+        '''
+
+        r = self.world.robot
+        skip_last = name not in self.slot_engine.behaviors
+
+        while True:
+            done = self.execute(name, *args)
+            if done and skip_last: break  # Exit here if last command is irrelevant
+            self.base_agent.scom.commit_and_send(r.get_command())
+            self.base_agent.scom.receive()
+            if done: break  # Exit here if last command is part of the behavior
+
+        # reset to avoid polluting the next command
+        r.joints_target_speed = np.zeros_like(r.joints_target_speed)
+
+    def is_ready(self, name, *args) -> bool:
+        ''' Checks if behavior is ready to start under current game/robot conditions '''
+
+        assert name in self.behaviors, f"Behavior {name} does not exist!"
+        return self.behaviors[name][3](*args)

behaviors/Head.py

@@ -0,0 +1,105 @@
+from math_ops.Math_Ops import Math_Ops as M
+from world.World import World
+import numpy as np
+
+class Head():
+    FIELD_FLAGS = World.FLAGS_CORNERS_POS + World.FLAGS_POSTS_POS
+    HEAD_PITCH = -35
+
+    def __init__(self, world : World) -> None:
+        self.world = world
+        self.look_left = True
+        self.state = 0
+
+    
+    def execute(self):
+        '''
+        Try to compute best head orientation if possible, otherwise look around
+
+        state:
+        0            - Adjust position - ball is in FOV and robot can self-locate
+        1..TIMEOUT-1 - Guided search - attempt to use recent visual/radio information to guide the search
+        TIMEOUT      - Random search - look around (default mode after guided search fails by timeout)
+        '''
+        TIMEOUT = 30
+        w = self.world
+        r = w.robot
+        can_self_locate = r.loc_last_update > w.time_local_ms - w.VISUALSTEP_MS
+        
+        #--------------------------------------- A. Ball is in FOV and robot can self-locate
+
+        if w.ball_last_seen > w.time_local_ms - w.VISUALSTEP_MS: # ball is in FOV
+            if can_self_locate:
+                best_dir = self.compute_best_direction(can_self_locate, use_ball_from_vision=True)
+                self.state = 0
+            elif self.state < TIMEOUT:
+        #--------------------------------------- B. Ball is in FOV but robot cannot currently self-locate
+                best_dir = self.compute_best_direction(can_self_locate, use_ball_from_vision=True)
+                self.state += 1 # change to guided search and increment time
+        elif self.state < TIMEOUT:
+        #--------------------------------------- C. Ball is not in FOV
+            best_dir = self.compute_best_direction(can_self_locate)
+            self.state += 1 # change to guided search and increment time
+
+
+        if self.state == TIMEOUT: # Random search
+
+            if w.ball_last_seen > w.time_local_ms - w.VISUALSTEP_MS: # Ball is in FOV (search 45 deg to both sides of the ball)
+                ball_dir = M.vector_angle(w.ball_rel_torso_cart_pos[:2])
+                targ = np.clip(ball_dir + (45 if self.look_left else -45), -119, 119)
+            else: # Ball is not in FOV (search 119 deg to both sides)
+                targ = 119 if self.look_left else -119
+
+            if r.set_joints_target_position_direct([0,1], np.array([targ,Head.HEAD_PITCH]), False) <= 0:
+                self.look_left = not self.look_left
+
+        else: # Adjust position or guided search
+            r.set_joints_target_position_direct([0,1], np.array([best_dir,Head.HEAD_PITCH]), False)
+
+
+    def compute_best_direction(self, can_self_locate, use_ball_from_vision=False):
+        FOV_MARGIN = 15 # safety margin, avoid margin horizontally
+        SAFE_RANGE = 120 - FOV_MARGIN*2
+        HALF_RANGE = SAFE_RANGE / 2
+
+        w = self.world
+        r = w.robot
+        
+        if use_ball_from_vision:
+            ball_2d_dist = np.linalg.norm(w.ball_rel_torso_cart_pos[:2])
+        else:
+            ball_2d_dist = np.linalg.norm(w.ball_abs_pos[:2]-r.loc_head_position[:2])
+                
+        if ball_2d_dist > 0.12:
+            if use_ball_from_vision:
+                ball_dir = M.vector_angle(w.ball_rel_torso_cart_pos[:2])
+            else:
+                ball_dir = M.target_rel_angle(r.loc_head_position, r.imu_torso_orientation, w.ball_abs_pos)
+        else: # ball is very close to robot
+            ball_dir = 0
+
+        flags_diff = dict()
+            
+        # iterate flags
+        for f in Head.FIELD_FLAGS:
+            flag_dir = M.target_rel_angle(r.loc_head_position, r.imu_torso_orientation, f)
+            diff = M.normalize_deg( flag_dir - ball_dir )
+            if abs(diff) < HALF_RANGE and can_self_locate:
+                return ball_dir # return ball direction if robot can self-locate
+            flags_diff[f] = diff
+
+        
+        closest_flag = min( flags_diff, key=lambda k: abs(flags_diff[k]) )
+        closest_diff = flags_diff[closest_flag]
+
+        if can_self_locate: # at this point, if it can self-locate, then  abs(closest_diff) > HALF_RANGE
+            # return position that centers the ball as much as possible in the FOV, including the nearest flag if possible
+            final_diff = min( abs(closest_diff) - HALF_RANGE, SAFE_RANGE ) * np.sign(closest_diff)
+        else:
+            # position that centers the flag as much as possible, until it is seen, while keeping the ball in the FOV
+            final_diff = np.clip( closest_diff, -SAFE_RANGE, SAFE_RANGE )
+            # saturate instead of normalizing angle to avoid a complete neck rotation
+            return np.clip(ball_dir + final_diff, -119, 119)
+
+
+        return M.normalize_deg( ball_dir + final_diff )

behaviors/Poses.py

@@ -0,0 +1,97 @@
+''' 
+Pose - angles in degrees for the specified joints
+Note: toes positions are ignored by robots that have no toes 
+
+Poses may control all joints or just a subgroup defined by the "indices" variable
+'''
+
+import numpy as np
+from world.World import World
+
+
+class Poses():
+    def __init__(self, world : World) -> None:
+        self.world = world
+        self.tolerance = 0.05 # angle error tolerance to consider that behavior is finished
+
+        '''
+        Instruction to add new pose:
+        1. add new entry to the following dictionary, using a unique behavior name
+        2. that's it
+        '''
+        self.poses = {
+            "Zero":(
+                "Neutral pose, including head", # description
+                False, # disable automatic head orientation
+                np.array([0,1,2,3,4,5,6,7,8,9,10,11,12,13, 14, 15,16,17,18,19,20,21,22,23]), # indices
+                np.array([0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0,-90,-90, 0, 0,90,90, 0, 0, 0, 0])  # values
+            ),
+            "Zero_Legacy":(
+                "Neutral pose, including head, elbows cause collision (legacy)", # description
+                False, # disable automatic head orientation
+                np.array([0,1,2,3,4,5,6,7,8,9,10,11,12,13, 14, 15,16,17,18,19,20,21,22,23]), # indices
+                np.array([0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0,-90,-90, 0, 0, 0, 0, 0, 0, 0, 0])  # values
+            ),
+            "Zero_Bent_Knees":(
+                "Neutral pose, including head, bent knees", # description
+                False, # disable automatic head orientation
+                np.array([0,1,2,3,4,5,6,  7,  8,  9,10,11,12,13, 14, 15,16,17,18,19,20,21,22,23]), # indices
+                np.array([0,0,0,0,0,0,30,30,-60,-60,30,30, 0, 0,-90,-90, 0, 0,90,90, 0, 0, 0, 0])  # values
+            ),
+            "Zero_Bent_Knees_Auto_Head":(
+                "Neutral pose, automatic head, bent knees", # description
+                True, # enable automatic head orientation
+                np.array([2,3,4,5,6,  7,  8,  9,10,11,12,13, 14, 15,16,17,18,19,20,21,22,23]), # indices
+                np.array([0,0,0,0,30,30,-60,-60,30,30, 0, 0,-90,-90, 0, 0,90,90, 0, 0, 0, 0])  # values
+            ),
+            "Fall_Back":(
+                "Incline feet to fall back", # description
+                True, # enable automatic head orientation
+                np.array([ 10, 11]), # indices
+                np.array([-20,-20])  # values
+            ),
+            "Fall_Front":(
+                "Incline feet to fall forward", # description
+                True, # enable automatic head orientation
+                np.array([10,11]), # indices
+                np.array([45,45])  # values
+            ),
+            "Fall_Left":(
+                "Incline legs to fall to left", # description
+                True, # enable automatic head orientation
+                np.array([  4, 5]), # indices
+                np.array([-20,20])  # values
+            ),
+            "Fall_Right":(
+                "Incline legs to fall to right", # description
+                True, # enable automatic head orientation
+                np.array([ 4,  5]), # indices
+                np.array([20,-20])  # values
+            ),
+        }
+
+        # Remove toes if not robot 4
+        if world.robot.type != 4:
+            for key, val in self.poses.items():
+                idxs = np.where(val[2] >= 22)[0] # search for joint 22 & 23
+                if len(idxs) > 0:
+                    self.poses[key] = (val[0], val[1], np.delete(val[2],idxs), np.delete(val[3],idxs)) # remove those joints
+
+
+    def get_behaviors_callbacks(self):
+        ''' 
+        Returns callbacks for each pose behavior (used internally)
+        
+        Implementation note:
+        --------------------
+        Using dummy default parameters because lambda expression will remember the scope and var name.
+        In the loop, the scope does not change, nor does the var name.
+        However, default parameters are evaluated when the lambda is defined.
+        '''
+        return {key: (val[0], val[1], lambda reset, key=key: self.execute(key), lambda: True) for key, val in self.poses.items()}
+
+    def execute(self,name) -> bool:
+        _, _, indices, values = self.poses[name]
+        remaining_steps = self.world.robot.set_joints_target_position_direct(indices,values,True,tolerance=self.tolerance)
+        return bool(remaining_steps == -1)
+

behaviors/Slot_Engine.py

@@ -0,0 +1,110 @@
+from math_ops.Math_Ops import Math_Ops as M
+from os import listdir
+from os.path import isfile, join
+from world.World import World
+import numpy as np
+import xml.etree.ElementTree as xmlp
+
+class Slot_Engine():
+
+    def __init__(self, world : World) -> None:
+        self.world = world
+        self.state_slot_number = 0
+        self.state_slot_start_time = 0
+        self.state_slot_start_angles = None
+        self.state_init_zero = True
+
+        # ------------- Parse slot behaviors
+
+        dir = M.get_active_directory("/behaviors/slot/")
+
+        common_dir = f"{dir}common/"
+        files =  [(f,join(common_dir, f)) for f in listdir(common_dir) if isfile(join(common_dir, f)) and f.endswith(".xml")]
+        robot_dir = f"{dir}r{world.robot.type}"
+        files += [(f,join(robot_dir, f)) for f in listdir(robot_dir) if isfile(join(robot_dir, f)) and f.endswith(".xml")]
+
+        self.behaviors = dict()
+        self.descriptions = dict()
+        self.auto_head_flags = dict()
+
+        for fname, file in files:
+            robot_xml_root = xmlp.parse(file).getroot()
+            slots = []
+            bname = fname[:-4] # remove extension ".xml"
+
+            for xml_slot in robot_xml_root:
+                assert xml_slot.tag == 'slot', f"Unexpected XML element in slot behavior {fname}: '{xml_slot.tag}'"
+                indices, angles = [],[]
+                
+                for action in xml_slot:
+                    indices.append(  int(action.attrib['id'])    )
+                    angles.append( float(action.attrib['angle']) )
+
+                delta_ms = float(xml_slot.attrib['delta']) * 1000
+                assert delta_ms > 0, f"Invalid delta <=0 found in Slot Behavior {fname}"
+                slots.append((delta_ms, indices, angles))
+
+            assert bname not in self.behaviors, f"Found at least 2 slot behaviors with same name: {fname}"
+
+            self.descriptions[bname] = robot_xml_root.attrib["description"] if "description" in robot_xml_root.attrib else bname
+            self.auto_head_flags[bname] = (robot_xml_root.attrib["auto_head"] == "1")
+            self.behaviors[bname] = slots
+
+
+    def get_behaviors_callbacks(self):
+        ''' 
+        Returns callbacks for each slot behavior (used internally) 
+
+        Implementation note:
+        --------------------
+        Using dummy default parameters because lambda expression will remember the scope and var name.
+        In the loop, the scope does not change, nor does the var name.
+        However, default parameters are evaluated when the lambda is defined.
+        '''
+        return {key: (self.descriptions[key],self.auto_head_flags[key],
+                lambda reset,key=key: self.execute(key,reset), lambda key=key: self.is_ready(key)) for key in self.behaviors}
+
+
+    def is_ready(self,name) -> bool:
+        return True
+
+
+    def reset(self, name):
+        ''' Initialize/Reset slot behavior '''
+
+        self.state_slot_number = 0
+        self.state_slot_start_time_ms = self.world.time_local_ms
+        self.state_slot_start_angles = np.copy(self.world.robot.joints_position)
+        assert name in self.behaviors, f"Requested slot behavior does not exist: {name}"
+
+
+    def execute(self,name,reset) -> bool:
+        ''' Execute one step '''
+
+        if reset: self.reset(name)
+
+        elapsed_ms = self.world.time_local_ms - self.state_slot_start_time_ms
+        delta_ms, indices, angles = self.behaviors[name][self.state_slot_number]
+
+        # Check slot progression
+        if elapsed_ms >= delta_ms:
+            self.state_slot_start_angles[indices] = angles #update start angles based on last target
+             
+            # Prevent 2 rare scenarios:
+            # 1 - this function is called after the behavior is finished & reset==False
+            # 2 - we are in the last slot, syncmode is not active, and we lost the last step
+            if self.state_slot_number+1 == len(self.behaviors[name]):
+                return True # So, the return indicates a finished behavior until a reset is sent via the arguments
+
+            self.state_slot_number += 1
+            elapsed_ms = 0
+            self.state_slot_start_time_ms = self.world.time_local_ms
+            delta_ms, indices, angles = self.behaviors[name][self.state_slot_number]
+
+        # Execute 
+        progress = (elapsed_ms+20) / delta_ms
+        target = (angles - self.state_slot_start_angles[indices]) * progress + self.state_slot_start_angles[indices]
+        self.world.robot.set_joints_target_position_direct(indices,target,False)
+
+        # Return True if finished (this is the last step)
+        return bool(elapsed_ms+20 >= delta_ms and self.state_slot_number + 1 == len(self.behaviors[name])) # true if next step (now+20ms) is out of bounds

behaviors/custom/Basic_Kick/Basic_Kick.py

@@ -0,0 +1,73 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Step.Step_Generator import Step_Generator
+from math_ops.Math_Ops import Math_Ops as M
+
+
+class Basic_Kick():
+
+    def __init__(self, base_agent : Base_Agent) -> None:
+        self.behavior = base_agent.behavior
+        self.path_manager = base_agent.path_manager
+        self.world = base_agent.world
+        self.description = "Walk to ball and perform a basic kick"
+        self.auto_head = True
+
+        r_type = self.world.robot.type
+        self.bias_dir = [22,29,26,29,22][self.world.robot.type]
+        self.ball_x_limits = ((0.19,0.215), (0.2,0.22), (0.19,0.22), (0.2,0.215), (0.2,0.215))[r_type]
+        self.ball_y_limits = ((-0.115,-0.1), (-0.125,-0.095), (-0.12,-0.1), (-0.13,-0.105), (-0.09,-0.06))[r_type]
+        self.ball_x_center = (self.ball_x_limits[0] + self.ball_x_limits[1])/2
+        self.ball_y_center = (self.ball_y_limits[0] + self.ball_y_limits[1])/2
+      
+    def execute(self,reset, direction, abort=False) -> bool: # You can add more arguments 
+        '''
+        Parameters
+        ----------
+        direction : float
+            kick direction relative to field, in degrees
+        abort : bool
+            True to abort.
+            The method returns True upon successful abortion, which is immediate while the robot is aligning itself. 
+            However, if the abortion is requested during the kick, it is delayed until the kick is completed.
+        '''
+
+        w = self.world
+        r = self.world.robot
+        b = w.ball_rel_torso_cart_pos
+        t = w.time_local_ms
+        gait : Step_Generator = self.behavior.get_custom_behavior_object("Walk").env.step_generator
+
+        if reset:
+            self.phase = 0
+            self.reset_time = t
+
+        if self.phase == 0: 
+            biased_dir = M.normalize_deg(direction + self.bias_dir) # add bias to rectify direction
+            ang_diff = abs(M.normalize_deg( biased_dir - r.loc_torso_orientation )) # the reset was learned with loc, not IMU
+
+            next_pos, next_ori, dist_to_final_target = self.path_manager.get_path_to_ball(
+                x_ori=biased_dir, x_dev=-self.ball_x_center, y_dev=-self.ball_y_center, torso_ori=biased_dir)
+            
+            if (w.ball_last_seen > t - w.VISUALSTEP_MS and ang_diff < 5 and       # ball is visible & aligned
+                self.ball_x_limits[0] < b[0] < self.ball_x_limits[1] and          # ball is in kick area (x)
+                self.ball_y_limits[0] < b[1] < self.ball_y_limits[1] and          # ball is in kick area (y)
+                t - w.ball_abs_pos_last_update < 100 and                          # ball absolute location is recent
+                dist_to_final_target < 0.03 and                                   # if absolute ball position is updated
+                not gait.state_is_left_active and gait.state_current_ts == 2 and  # walk gait phase is adequate
+                t - self.reset_time > 500): # to avoid kicking immediately without preparation & stability
+
+                self.phase += 1
+                return self.behavior.execute_sub_behavior("Kick_Motion", True)
+            else:
+                dist = max(0.07, dist_to_final_target)
+                reset_walk = reset and self.behavior.previous_behavior != "Walk" # reset walk if it wasn't the previous behavior
+                self.behavior.execute_sub_behavior("Walk", reset_walk, next_pos, True, next_ori, True, dist) # target, is_target_abs, ori, is_ori_abs, distance
+                return abort # abort only if self.phase == 0
+
+        else: # define kick parameters and execute 
+            return self.behavior.execute_sub_behavior("Kick_Motion", False)
+
+      
+    def is_ready(self) -> any: # You can add more arguments 
+        ''' Returns True if this behavior is ready to start/continue under current game/robot conditions '''
+        return True

behaviors/custom/Basic_Kick/Env.py

@@ -0,0 +1,201 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Step.Step_Generator import Step_Generator
+from math_ops.Math_Ops import Math_Ops as M
+import math
+import numpy as np
+
+
+class Env():
+    def __init__(self, base_agent : Base_Agent) -> None:
+
+        self.world = base_agent.world
+        self.ik = base_agent.inv_kinematics
+        
+        # State space  
+        self.obs = np.zeros(63, np.float32)
+        
+        # Step behavior defaults
+        self.STEP_DUR = 8
+        self.STEP_Z_SPAN = 0.02
+        self.STEP_Z_MAX = 0.70
+
+        # IK 
+        nao_specs = self.ik.NAO_SPECS
+        self.leg_length = nao_specs[1] + nao_specs[3] # upper leg height + lower leg height
+        feet_y_dev = nao_specs[0] * 1.12 # wider step
+        sample_time = self.world.robot.STEPTIME
+        max_ankle_z = nao_specs[5]
+
+        self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z)
+        self.DEFAULT_ARMS = np.array([-90,-90,8,8,90,90,70,70],np.float32)
+
+        self.walk_rel_orientation = None
+        self.walk_rel_target = None
+        self.walk_distance = None
+
+
+    def observe(self, init=False):
+
+        r = self.world.robot
+
+        if init: # reset variables
+            self.act = np.zeros(16, np.float32) # memory variable
+            self.step_counter = 0
+
+        # index       observation              naive normalization
+        self.obs[0] = min(self.step_counter,15*8) /100  # simple counter: 0,1,2,3...
+        self.obs[1] = r.loc_head_z                *3   # z coordinate (torso)
+        self.obs[2] = r.loc_head_z_vel            /2   # z velocity (torso)  
+        self.obs[3] = r.imu_torso_roll            /15   # absolute torso roll  in deg
+        self.obs[4] = r.imu_torso_pitch           /15   # absolute torso pitch in deg
+        self.obs[5:8] = r.gyro                    /100  # gyroscope
+        self.obs[8:11] = r.acc                    /10   # accelerometer
+
+        self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10,10,10,0.01,0.01,0.01) #  left foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10,10,10,0.01,0.01,0.01) # right foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        # *if foot is not touching the ground, then (px=0,py=0,pz=0,fx=0,fy=0,fz=0)
+
+        # Joints: Forward kinematics for ankles + feet rotation + arms (pitch + roll)
+        rel_lankle = self.ik.get_body_part_pos_relative_to_hip("lankle") # ankle position relative to center of both hip joints
+        rel_rankle = self.ik.get_body_part_pos_relative_to_hip("rankle") # ankle position relative to center of both hip joints
+        lf = r.head_to_body_part_transform("torso", r.body_parts['lfoot'].transform ) # foot transform relative to torso
+        rf = r.head_to_body_part_transform("torso", r.body_parts['rfoot'].transform ) # foot transform relative to torso
+        lf_rot_rel_torso = np.array( [lf.get_roll_deg(), lf.get_pitch_deg(), lf.get_yaw_deg()] ) # foot rotation relative to torso
+        rf_rot_rel_torso = np.array( [rf.get_roll_deg(), rf.get_pitch_deg(), rf.get_yaw_deg()] ) # foot rotation relative to torso
+
+        # pose
+        self.obs[23:26] = rel_lankle * (8,8,5)
+        self.obs[26:29] = rel_rankle * (8,8,5)
+        self.obs[29:32] = lf_rot_rel_torso / 20
+        self.obs[32:35] = rf_rot_rel_torso / 20
+        self.obs[35:39] = r.joints_position[14:18] /100 # arms (pitch + roll)
+
+        # velocity
+        self.obs[39:55] = r.joints_target_last_speed[2:18] # predictions == last action
+
+        '''
+        Expected observations for walking state:
+        Time step        R  0   1   2   3   4   5   6   7   0
+        Progress         1  0 .14 .28 .43 .57 .71 .86   1   0
+        Left leg active  T  F   F   F   F   F   F   F   F   T
+        '''
+
+        if init: # the walking parameters refer to the last parameters in effect (after a reset, they are pointless)
+            self.obs[55] = 1 # step progress
+            self.obs[56] = 1 # 1 if left  leg is active
+            self.obs[57] = 0 # 1 if right leg is active
+        else:
+            self.obs[55] = self.step_generator.external_progress # step progress
+            self.obs[56] = float(self.step_generator.state_is_left_active)     # 1 if left  leg is active
+            self.obs[57] = float(not self.step_generator.state_is_left_active) # 1 if right leg is active
+
+        '''
+        Create internal target with a smoother variation
+        '''
+
+        MAX_LINEAR_DIST = 0.5
+        MAX_LINEAR_DIFF = 0.014 # max difference (meters) per step
+        MAX_ROTATION_DIFF = 1.6 # max difference (degrees) per step
+        MAX_ROTATION_DIST = 45
+
+
+        if init:      
+            self.internal_rel_orientation = 0
+            self.internal_target = np.zeros(2)
+
+        previous_internal_target = np.copy(self.internal_target)
+       
+        #---------------------------------------------------------------- compute internal linear target
+        
+        rel_raw_target_size = np.linalg.norm(self.walk_rel_target)
+
+        if rel_raw_target_size == 0:
+            rel_target = self.walk_rel_target
+        else:
+            rel_target = self.walk_rel_target / rel_raw_target_size * min(self.walk_distance, MAX_LINEAR_DIST)
+       
+        internal_diff = rel_target - self.internal_target
+        internal_diff_size = np.linalg.norm(internal_diff)
+
+        if internal_diff_size > MAX_LINEAR_DIFF:
+            self.internal_target += internal_diff * (MAX_LINEAR_DIFF / internal_diff_size)
+        else:
+            self.internal_target[:] = rel_target
+
+        #---------------------------------------------------------------- compute internal rotation target
+
+        internal_ori_diff =  np.clip( M.normalize_deg( self.walk_rel_orientation - self.internal_rel_orientation ), -MAX_ROTATION_DIFF, MAX_ROTATION_DIFF)
+        self.internal_rel_orientation = np.clip(M.normalize_deg( self.internal_rel_orientation + internal_ori_diff ), -MAX_ROTATION_DIST, MAX_ROTATION_DIST)
+
+        #----------------------------------------------------------------- observations
+        
+        internal_target_vel = self.internal_target - previous_internal_target
+
+        self.obs[58] = self.internal_target[0] / MAX_LINEAR_DIST
+        self.obs[59] = self.internal_target[1] / MAX_LINEAR_DIST
+        self.obs[60] = self.internal_rel_orientation / MAX_ROTATION_DIST
+        self.obs[61] = internal_target_vel[0] / MAX_LINEAR_DIFF
+        self.obs[62] = internal_target_vel[0] / MAX_LINEAR_DIFF
+
+        return self.obs
+
+
+    def execute_ik(self, l_pos, l_rot, r_pos, r_rot):
+        r = self.world.robot
+        # Apply IK to each leg + Set joint targets
+          
+        # Left leg 
+        indices, self.values_l, error_codes = self.ik.leg(l_pos, l_rot, True, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_l, harmonize=False)
+
+        # Right leg
+        indices, self.values_r, error_codes = self.ik.leg(r_pos, r_rot, False, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_r, harmonize=False)
+
+
+    def execute(self, action):
+        
+        r = self.world.robot
+
+        # Actions:
+        # 0,1,2    left ankle pos
+        # 3,4,5    right ankle pos
+        # 6,7,8    left foot rotation
+        # 9,10,11  right foot rotation
+        # 12,13    left/right arm pitch
+        # 14,15    left/right arm roll
+
+        internal_dist = np.linalg.norm( self.internal_target )
+        action_mult = 1 if internal_dist > 0.2 else (0.7/0.2) * internal_dist + 0.3
+
+        # exponential moving average
+        self.act = 0.8 * self.act + 0.2 * action * action_mult * 0.7
+        
+        # execute Step behavior to extract the target positions of each leg (we will override these targets)
+        lfy,lfz,rfy,rfz = self.step_generator.get_target_positions(self.step_counter == 0, self.STEP_DUR, self.STEP_Z_SPAN, self.leg_length * self.STEP_Z_MAX)
+
+
+        # Leg IK
+        a = self.act
+        l_ankle_pos = (a[0]*0.02, max(0.01,  a[1]*0.02 + lfy), a[2]*0.01 + lfz) # limit y to avoid self collision
+        r_ankle_pos = (a[3]*0.02, min(a[4]*0.02 + rfy, -0.01), a[5]*0.01 + rfz) # limit y to avoid self collision
+        l_foot_rot = a[6:9]  * (3,3,5)
+        r_foot_rot = a[9:12] * (3,3,5)
+
+        # Limit leg yaw/pitch
+        l_foot_rot[2] = max(0,l_foot_rot[2] + 7)
+        r_foot_rot[2] = min(0,r_foot_rot[2] - 7)
+
+        # Arms actions
+        arms = np.copy(self.DEFAULT_ARMS) # default arms pose
+        arm_swing = math.sin(self.step_generator.state_current_ts / self.STEP_DUR * math.pi) * 6
+        inv = 1 if self.step_generator.state_is_left_active else -1
+        arms[0:4] += a[12:16]*4 + (-arm_swing*inv,arm_swing*inv,0,0) # arms pitch+roll
+
+        # Set target positions
+        self.execute_ik(l_ankle_pos, l_foot_rot, r_ankle_pos, r_foot_rot)           # legs 
+        r.set_joints_target_position_direct( slice(14,22), arms, harmonize=False )  # arms
+
+        self.step_counter += 1

behaviors/custom/Dribble/Dribble.py

@@ -0,0 +1,210 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Dribble.Env import Env
+from math_ops.Math_Ops import Math_Ops as M
+from math_ops.Neural_Network import run_mlp
+import numpy as np
+import pickle
+
+
+class Dribble:
+
+    def __init__(self, base_agent: Base_Agent) -> None:
+        self.behavior = base_agent.behavior
+        self.path_manager = base_agent.path_manager
+        self.world = base_agent.world
+        self.description = "RL dribble"
+        self.auto_head = True
+        self.env = Env(base_agent, 0.9 if self.world.robot.type == 3 else 1.2)
+
+        with open(M.get_active_directory([
+                                             "/behaviors/custom/Dribble/dribble_R0.pkl",
+                                             "/behaviors/custom/Dribble/dribble_R1.pkl",
+                                             "/behaviors/custom/Dribble/dribble_R2.pkl",
+                                             "/behaviors/custom/Dribble/dribble_R3.pkl",
+                                             "/behaviors/custom/Dribble/dribble_R4.pkl"
+                                         ][self.world.robot.type]), 'rb') as f:
+            self.model = pickle.load(f)
+
+    def define_approach_orientation(self):
+
+        w = self.world
+        b = w.ball_abs_pos[:2]
+        me = w.robot.loc_head_position[:2]
+
+        self.approach_orientation = None
+
+        MARGIN = 0.8  # safety margin (if ball is near the field limits by this amount, the approach orientation is considered)
+        M90 = 90 / MARGIN  # auxiliary variable
+        DEV = 25  # when standing on top of sidelines or endlines, the approach direction deviates from that line by this amount
+        MDEV = (90 + DEV) / MARGIN  # auxiliary variable
+
+        a1 = -180  # angle range start (counterclockwise rotation)
+        a2 = 180  # angle range end   (counterclockwise rotation)
+
+        if b[1] < -10 + MARGIN:
+            if b[0] < -15 + MARGIN:
+                a1 = DEV - M90 * (b[1] + 10)
+                a2 = 90 - DEV + M90 * (b[0] + 15)
+            elif b[0] > 15 - MARGIN:
+                a1 = 90 + DEV - M90 * (15 - b[0])
+                a2 = 180 - DEV + M90 * (b[1] + 10)
+            else:
+                a1 = DEV - MDEV * (b[1] + 10)
+                a2 = 180 - DEV + MDEV * (b[1] + 10)
+        elif b[1] > 10 - MARGIN:
+            if b[0] < -15 + MARGIN:
+                a1 = -90 + DEV - M90 * (b[0] + 15)
+                a2 = -DEV + M90 * (10 - b[1])
+            elif b[0] > 15 - MARGIN:
+                a1 = 180 + DEV - M90 * (10 - b[1])
+                a2 = 270 - DEV + M90 * (15 - b[0])
+            else:
+                a1 = -180 + DEV - MDEV * (10 - b[1])
+                a2 = -DEV + MDEV * (10 - b[1])
+        elif b[0] < -15 + MARGIN:
+            a1 = -90 + DEV - MDEV * (b[0] + 15)
+            a2 = 90 - DEV + MDEV * (b[0] + 15)
+        elif b[0] > 15 - MARGIN and abs(b[1]) > 1.2:
+            a1 = 90 + DEV - MDEV * (15 - b[0])
+            a2 = 270 - DEV + MDEV * (15 - b[0])
+
+        cad = M.vector_angle(b - me)  # current approach direction
+
+        a1 = M.normalize_deg(a1)
+        a2 = M.normalize_deg(a2)
+
+        if a1 < a2:
+            if a1 <= cad <= a2:
+                return  # current approach orientation is within accepted range
+        else:
+            if a1 <= cad or cad <= a2:
+                return  # current approach orientation is within accepted range
+
+        a1_diff = abs(M.normalize_deg(a1 - cad))
+        a2_diff = abs(M.normalize_deg(a2 - cad))
+
+        self.approach_orientation = a1 if a1_diff < a2_diff else a2  # fixed normalized orientation
+
+    def execute(self, reset, orientation, is_orientation_absolute, speed=1, stop=False):
+        '''
+        Parameters
+        ----------
+        orientation : float
+            absolute or relative orientation of torso (relative to imu_torso_orientation), in degrees
+            set to None to dribble towards the opponent's goal (is_orientation_absolute is ignored)
+        is_orientation_absolute : bool
+            True if orientation is relative to the field, False if relative to the robot's torso
+        speed : float
+            speed from 0 to 1 (scale is not linear)
+        stop : bool
+            return True immediately if walking, wind down if dribbling, and return True when possible
+        '''
+        w = self.world
+        r = self.world.robot
+        me = r.loc_head_position[:2]
+        b = w.ball_abs_pos[:2]
+        b_rel = w.ball_rel_torso_cart_pos[:2]
+        b_dist = np.linalg.norm(b - me)
+        behavior = self.behavior
+        reset_dribble = False
+        lost_ball = (w.ball_last_seen <= w.time_local_ms - w.VISUALSTEP_MS) or np.linalg.norm(b_rel) > 0.4
+
+        if reset:
+            self.phase = 0
+            if behavior.previous_behavior == "Push_RL" and 0 < b_rel[0] < 0.25 and abs(b_rel[1]) < 0.07:
+                self.phase = 1
+                reset_dribble = True
+
+        if self.phase == 0:  # walk to ball
+            reset_walk = reset and behavior.previous_behavior != "Walk" and behavior.previous_behavior != "Push_RL"  # reset walk if it wasn't the previous behavior
+
+            # ------------------------ 1. Decide if a better approach orientation is needed (when ball is nearly out of bounds)
+            if reset or b_dist > 0.4:  # stop defining orientation after getting near the ball to avoid noise
+                self.define_approach_orientation()
+
+            # ------------------------ 2A. A better approach orientation is needed (ball is almost out of bounds)
+            if self.approach_orientation is not None:
+                next_pos, next_ori, dist_to_final_target = self.path_manager.get_path_to_ball(
+                    x_ori=self.approach_orientation, x_dev=-0.24, torso_ori=self.approach_orientation,
+                    safety_margin=0.4)
+
+                if b_rel[0] < 0.26 and b_rel[0] > 0.18 and abs(
+                        b_rel[1]) < 0.04 and w.ball_is_visible:  # ready to start dribble
+                    self.phase += 1
+                    reset_dribble = True
+                else:
+                    dist = max(0.08, dist_to_final_target * 0.7)
+                    behavior.execute_sub_behavior("Walk", reset_walk, next_pos, True, next_ori, True,
+                                                  dist)  # target, is_target_abs, ori, is_ori_abs, distance
+
+            # ------------------------ 2B. A better approach orientation is not needed but the robot cannot see the ball
+            elif w.time_local_ms - w.ball_last_seen > 200:  # walk to absolute target if ball was not seen
+                abs_ori = M.vector_angle(b - me)
+                behavior.execute_sub_behavior("Walk", reset_walk, b, True, abs_ori, True,
+                                              None)  # target, is_target_abs, ori, is_ori_abs, distance
+
+            # ------------------------ 2C. A better approach orientation is not needed and the robot can see the ball
+            else:  # walk to relative target
+                if 0.18 < b_rel[0] < 0.25 and abs(b_rel[1]) < 0.05 and w.ball_is_visible:  # ready to start dribble
+                    self.phase += 1
+                    reset_dribble = True
+                else:
+                    rel_target = b_rel + (-0.23, 0)  # relative target is a circle (center: ball, radius:0.23m)
+                    rel_ori = M.vector_angle(b_rel)  # relative orientation of ball, NOT the target!
+                    dist = max(0.08, np.linalg.norm(rel_target) * 0.7)  # slow approach
+                    behavior.execute_sub_behavior("Walk", reset_walk, rel_target, False, rel_ori, False,
+                                                  dist)  # target, is_target_abs, ori, is_ori_abs, distance
+
+            if stop:
+                return True
+
+        if self.phase == 1 and (stop or (b_dist > 0.5 and lost_ball)):  # go back to walking
+            self.phase += 1
+        elif self.phase == 1:  # dribble
+            # ------------------------ 1. Define dribble parameters
+            self.env.dribble_speed = speed
+
+            # Relative orientation values are decreased to avoid overshoot
+            if orientation is None:
+                if b[0] < 0:  # dribble to the sides
+                    if b[1] > 0:
+                        dribble_target = (15, 5)
+                    else:
+                        dribble_target = (15, -5)
+                else:
+                    dribble_target = None  # go to goal
+                self.env.dribble_rel_orientation = \
+                self.path_manager.get_dribble_path(optional_2d_target=dribble_target)[1]
+            elif is_orientation_absolute:
+                self.env.dribble_rel_orientation = M.normalize_deg(orientation - r.imu_torso_orientation)
+            else:
+                self.env.dribble_rel_orientation = float(orientation)  # copy if numpy float
+
+            # ------------------------ 2. Execute behavior
+            obs = self.env.observe(reset_dribble)
+            action = run_mlp(obs, self.model)
+            self.env.execute(action)
+
+        # wind down dribbling, and then reset phase
+        if self.phase > 1:
+            WIND_DOWN_STEPS = 60
+            # ------------------------ 1. Define dribble wind down parameters
+            self.env.dribble_speed = 1 - self.phase / WIND_DOWN_STEPS
+            self.env.dribble_rel_orientation = 0
+
+            # ------------------------ 2. Execute behavior
+            obs = self.env.observe(reset_dribble, virtual_ball=True)
+            action = run_mlp(obs, self.model)
+            self.env.execute(action)
+
+            # ------------------------ 3. Reset behavior
+            self.phase += 1
+            if self.phase >= WIND_DOWN_STEPS - 5:
+                self.phase = 0
+                return True
+
+        return False
+
+    def is_ready(self):
+        ''' Returns True if this behavior is ready to start/continue under current game/robot conditions '''
+        return True

behaviors/custom/Dribble/Env.py

@@ -0,0 +1,184 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Step.Step_Generator import Step_Generator
+from math_ops.Math_Ops import Math_Ops as M
+import math
+import numpy as np
+
+
+class Env:
+    def __init__(self, base_agent: Base_Agent, step_width) -> None:
+
+        self.gym_last_internal_abs_ori = None
+        self.world = base_agent.world
+        self.ik = base_agent.inv_kinematics
+
+        # State space  
+        self.obs = np.zeros(76, np.float32)
+
+        # Step behavior defaults
+        self.STEP_DUR = 8
+        self.STEP_Z_SPAN = 0.02
+        self.STEP_Z_MAX = 0.70
+
+        # IK 
+        r = self.world.robot
+        nao_specs = self.ik.NAO_SPECS
+        self.leg_length = nao_specs[1] + nao_specs[3]  # upper leg height + lower leg height
+        feet_y_dev = nao_specs[0] * step_width  # wider step
+        sample_time = r.STEPTIME
+        max_ankle_z = nao_specs[5]
+
+        self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z)
+        self.DEFAULT_ARMS = np.array([-90, -90, 8, 8, 90, 90, 70, 70], np.float32)
+
+        self.dribble_rel_orientation = None  # relative to imu_torso_orientation (in degrees)
+        self.dribble_speed = 1
+
+    def observe(self, init=False, virtual_ball=False):
+
+        w = self.world
+        r = self.world.robot
+
+        if init:  # reset variables
+            self.step_counter = 0
+            self.act = np.zeros(16, np.float32)  # memory variable
+
+        # index       observation              naive normalization
+        self.obs[0] = min(self.step_counter, 12 * 8) / 100  # simple counter: 0,1,2,3...
+        self.obs[1] = r.loc_head_z * 3  # z coordinate (torso)
+        self.obs[2] = r.loc_head_z_vel / 2  # z velocity (torso)
+        self.obs[3] = r.imu_torso_roll / 15  # absolute torso roll  in deg
+        self.obs[4] = r.imu_torso_pitch / 15  # absolute torso pitch in deg
+        self.obs[5:8] = r.gyro / 100  # gyroscope
+        self.obs[8:11] = r.acc / 10  # accelerometer
+
+        self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01,
+                                                          0.01)  # left foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01,
+                                                          0.01)  # right foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        # *if foot is not touching the ground, then (px=0,py=0,pz=0,fx=0,fy=0,fz=0)
+
+        self.obs[23:43] = r.joints_position[2:22] / 100  # position of all joints except head & toes (for robot type 4)
+        self.obs[43:63] = r.joints_speed[2:22] / 6.1395  # speed of    all joints except head & toes (for robot type 4)
+
+        '''
+        Expected observations for walking state:
+        Time step        R  0   1   2   3   4   5   6   7   0
+        Progress         1  0 .14 .28 .43 .57 .71 .86   1   0
+        Left leg active  T  F   F   F   F   F   F   F   F   T
+        '''
+
+        if init:  # the walking parameters refer to the last parameters in effect (after a reset, they are pointless)
+            self.obs[63] = 1  # step progress
+            self.obs[64] = 1  # 1 if left  leg is active
+            self.obs[65] = 0  # 1 if right leg is active
+            self.obs[66] = 0
+        else:
+            self.obs[63] = self.step_generator.external_progress  # step progress
+            self.obs[64] = float(self.step_generator.state_is_left_active)  # 1 if left  leg is active
+            self.obs[65] = float(not self.step_generator.state_is_left_active)  # 1 if right leg is active
+            self.obs[66] = math.sin(self.step_generator.state_current_ts / self.step_generator.ts_per_step * math.pi)
+
+        # Ball
+        ball_rel_hip_center = self.ik.torso_to_hip_transform(w.ball_rel_torso_cart_pos)
+        ball_dist_hip_center = np.linalg.norm(ball_rel_hip_center)
+
+        if init:
+            self.obs[67:70] = (0, 0, 0)  # Initial velocity is 0
+        elif w.ball_is_visible:
+            self.obs[67:70] = (ball_rel_hip_center - self.obs[70:73]) * 10  # Ball velocity, relative to ankle's midpoint
+
+        self.obs[70:73] = ball_rel_hip_center  # Ball position, relative to hip
+        self.obs[73] = ball_dist_hip_center * 2
+
+        if virtual_ball:  # simulate the ball between the robot's feet
+            self.obs[67:74] = (0, 0, 0, 0.05, 0, -0.175, 0.36)
+
+        '''
+        Create internal target with a smoother variation
+        '''
+
+        MAX_ROTATION_DIFF = 20  # max difference (degrees) per visual step
+        MAX_ROTATION_DIST = 80
+
+        if init:
+            self.internal_rel_orientation = 0
+            self.internal_target_vel = 0
+            self.gym_last_internal_abs_ori = r.imu_torso_orientation  # for training purposes (reward)
+
+        # ---------------------------------------------------------------- compute internal target
+
+        if w.vision_is_up_to_date:
+            previous_internal_rel_orientation = np.copy(self.internal_rel_orientation)
+
+            internal_ori_diff = np.clip(M.normalize_deg(self.dribble_rel_orientation - self.internal_rel_orientation),
+                                        -MAX_ROTATION_DIFF, MAX_ROTATION_DIFF)
+            self.internal_rel_orientation = np.clip(M.normalize_deg(self.internal_rel_orientation + internal_ori_diff),
+                                                    -MAX_ROTATION_DIST, MAX_ROTATION_DIST)
+
+            # Observations
+            self.internal_target_vel = self.internal_rel_orientation - previous_internal_rel_orientation
+
+            self.gym_last_internal_abs_ori = self.internal_rel_orientation + r.imu_torso_orientation
+
+        # ----------------------------------------------------------------- observations
+
+        self.obs[74] = self.internal_rel_orientation / MAX_ROTATION_DIST
+        self.obs[75] = self.internal_target_vel / MAX_ROTATION_DIFF
+
+        return self.obs
+
+    def execute_ik(self, l_pos, l_rot, r_pos, r_rot):
+        r = self.world.robot
+        # Apply IK to each leg + Set joint targets
+
+        # Left leg 
+        indices, self.values_l, error_codes = self.ik.leg(l_pos, l_rot, True, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_l, harmonize=False)
+
+        # Right leg
+        indices, self.values_r, error_codes = self.ik.leg(r_pos, r_rot, False, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_r, harmonize=False)
+
+    def execute(self, action):
+
+        r = self.world.robot
+
+        # Actions:
+        # 0,1,2    left ankle pos
+        # 3,4,5    right ankle pos
+        # 6,7,8    left foot rotation
+        # 9,10,11  right foot rotation
+        # 12,13    left/right arm pitch
+        # 14,15    left/right arm roll
+
+        # exponential moving average
+        self.act = 0.85 * self.act + 0.15 * action * 0.7 * 0.95 * self.dribble_speed
+
+        # execute Step behavior to extract the target positions of each leg (we will override these targets)
+        lfy, lfz, rfy, rfz = self.step_generator.get_target_positions(self.step_counter == 0, self.STEP_DUR,
+                                                                      self.STEP_Z_SPAN,
+                                                                      self.leg_length * self.STEP_Z_MAX)
+
+        # Leg IK
+        a = self.act
+        l_ankle_pos = (a[0] * 0.025 - 0.01, a[1] * 0.01 + lfy, a[2] * 0.01 + lfz)
+        r_ankle_pos = (a[3] * 0.025 - 0.01, a[4] * 0.01 + rfy, a[5] * 0.01 + rfz)
+        l_foot_rot = a[6:9] * (2, 2, 3)
+        r_foot_rot = a[9:12] * (2, 2, 3)
+
+        # Limit leg yaw/pitch (and add bias)
+        l_foot_rot[2] = max(0, l_foot_rot[2] + 18.3)
+        r_foot_rot[2] = min(0, r_foot_rot[2] - 18.3)
+
+        # Arms actions
+        arms = np.copy(self.DEFAULT_ARMS)  # default arms pose
+        arms[0:4] += a[12:16] * 4  # arms pitch+roll
+
+        # Set target positions
+        self.execute_ik(l_ankle_pos, l_foot_rot, r_ankle_pos, r_foot_rot)  # legs
+        r.set_joints_target_position_direct(slice(14, 22), arms, harmonize=False)  # arms
+
+        self.step_counter += 1

behaviors/custom/Fall/Fall.py

@@ -0,0 +1,43 @@
+from agent.Base_Agent import Base_Agent
+from math_ops.Math_Ops import Math_Ops as M
+from math_ops.Neural_Network import run_mlp
+import pickle, numpy as np
+
+class Fall():
+
+    def __init__(self, base_agent : Base_Agent) -> None:
+        self.world = base_agent.world
+        self.description = "Fall example"
+        self.auto_head = False
+
+        with open(M.get_active_directory("/behaviors/custom/Fall/fall.pkl"), 'rb') as f:
+            self.model = pickle.load(f)
+
+        self.action_size = len(self.model[-1][0]) # extracted from size of Neural Network's last layer bias
+        self.obs = np.zeros(self.action_size+1, np.float32)
+
+        self.controllable_joints = min(self.world.robot.no_of_joints, self.action_size) # compatibility between different robot types
+
+    def observe(self):
+        r = self.world.robot
+        
+        for i in range(self.action_size):
+            self.obs[i] = r.joints_position[i] / 100 # naive scale normalization
+
+        self.obs[self.action_size] = r.cheat_abs_pos[2] # head.z (alternative: r.loc_head_z)
+      
+    def execute(self,reset) -> bool:
+        self.observe()
+        action = run_mlp(self.obs, self.model) 
+        
+        self.world.robot.set_joints_target_position_direct( # commit actions:
+            slice(self.controllable_joints), # act on trained joints
+            action*10,                       # scale actions up to motivate early exploration
+            harmonize=False                  # there is no point in harmonizing actions if the targets change at every step  
+        )
+
+        return self.world.robot.loc_head_z < 0.15 # finished when head height < 0.15 m
+
+    def is_ready(self) -> any:
+        ''' Returns True if this behavior is ready to start/continue under current game/robot conditions '''
+        return True

behaviors/custom/Get_Up/Get_Up.py

@@ -0,0 +1,68 @@
+from agent.Base_Agent import Base_Agent
+from collections import deque
+import numpy as np
+
+class Get_Up():
+
+    def __init__(self, base_agent : Base_Agent) -> None:
+        self.behavior = base_agent.behavior
+        self.world = base_agent.world
+        self.description = "Get Up using the most appropriate skills"
+        self.auto_head = False
+        self.MIN_HEIGHT = 0.3 # minimum value for the head's height
+        self.MAX_INCLIN = 50  # maximum torso inclination in degrees
+        self.STABILITY_THRESHOLD = 4
+
+    def reset(self):
+        self.state = 0
+        self.gyro_queue = deque(maxlen=self.STABILITY_THRESHOLD)
+        self.watchdog = 0 # when player has the shaking bug, it is never stable enough to get up
+
+    def execute(self,reset):
+
+        r = self.world.robot
+        execute_sub_behavior = self.behavior.execute_sub_behavior
+        
+        if reset:
+            self.reset()
+
+        if self.state == 0: # State 0: go to pose "Zero"
+
+            self.watchdog += 1
+            self.gyro_queue.append( max(abs(r.gyro)) ) # log last STABILITY_THRESHOLD values
+
+            # advance to next state if behavior is complete & robot is stable
+            if (execute_sub_behavior("Zero",None) and len(self.gyro_queue) == self.STABILITY_THRESHOLD 
+                and all(g < 10 for g in self.gyro_queue)) or self.watchdog > 100:
+
+                # determine how to get up
+                if r.acc[0] < -4 and abs(r.acc[1]) < 2 and abs(r.acc[2]) < 3:
+                    execute_sub_behavior("Get_Up_Front", True) # reset behavior
+                    self.state = 1
+                elif r.acc[0] > 4 and abs(r.acc[1]) < 2 and abs(r.acc[2]) < 3:
+                    execute_sub_behavior("Get_Up_Back", True) # reset behavior
+                    self.state = 2
+                elif r.acc[2] > 8: # fail-safe if vision is not up to date: if pose is 'Zero' and torso is upright, the robot is already up
+                    return True
+                else:
+                    execute_sub_behavior("Flip", True) # reset behavior
+                    self.state = 3
+
+        elif self.state == 1:
+            if execute_sub_behavior("Get_Up_Front", False):
+                return True
+        elif self.state == 2:
+            if execute_sub_behavior("Get_Up_Back", False):
+                return True
+        elif self.state == 3:
+            if execute_sub_behavior("Flip", False):
+                self.reset()
+
+        return False
+        
+
+    def is_ready(self):
+        ''' Returns True if the Get Up behavior is ready (= robot is down) '''
+        r = self.world.robot
+        # check if z < 5 and acc magnitude > 8 and any visual indicator says we fell
+        return r.acc[2] < 5 and np.dot(r.acc,r.acc) > 64 and (r.loc_head_z < self.MIN_HEIGHT or r.imu_torso_inclination > self.MAX_INCLIN)

behaviors/custom/Kick_RL/Env.py

@@ -0,0 +1,89 @@
+import numpy as np
+from math_ops.Math_Ops import Math_Ops as M
+from world.World import World
+from agent.Base_Agent import Base_Agent
+
+
+class Env():
+
+    def __init__(self, base_agent: Base_Agent, world=None) -> None:
+        self.world = world
+        self.obs = np.zeros(63, np.float32)
+        self.step_counter = 0
+        self.kick_dir = None
+        self.ik = base_agent.inv_kinematics
+        self.base_agent = base_agent
+        self.DEFAULT_ARMS = np.array([
+            -90,
+            -90,
+            10,
+            10,
+            90,
+            90,
+            80,
+            80], np.float32)
+
+    def observe(self, init=(False,)):
+        w = self.world
+        r = self.world.robot
+        if init:
+            self.step_counter = 0
+            self.act = np.zeros(16, np.float32)
+        self.obs[0] = self.step_counter / 100
+        self.obs[1] = r.loc_head_z * 3
+        self.obs[2] = r.loc_head_z_vel / 2
+        self.obs[3] = r.imu_torso_roll / 15
+        self.obs[4] = r.imu_torso_pitch / 15
+        self.obs[5:8] = r.gyro / 100
+        self.obs[8:11] = r.acc / 10
+        self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01, 0.01)
+        self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01, 0.01)
+        self.obs[23:39] = r.joints_position[2:18] / 100
+        self.obs[39:55] = r.joints_speed[2:18] / 6.1395
+        ball_rel_hip_center = self.base_agent.inv_kinematics.torso_to_hip_transform(w.ball_rel_torso_cart_pos)
+        if init:
+            self.obs[55:58] = (0, 0, 0)
+        elif w.ball_is_visible:
+            self.obs[55:58] = (ball_rel_hip_center - self.obs[58:61]) * 10
+        self.obs[58:61] = ball_rel_hip_center
+        self.obs[61] = np.linalg.norm(ball_rel_hip_center) * 2
+        self.obs[62] = M.normalize_deg(self.kick_dir - r.imu_torso_orientation) / 30
+        return self.obs
+
+    def execute(self, action, max_speed=(7.03,)):
+        w = self.world
+        r = self.world.robot
+        self.act = 0.7 * self.act + 0.3 * action
+        a = self.act
+        leg_actions = a[:12] * [
+            5,
+            5,
+            2,
+            2,
+            5,
+            5,
+            5,
+            5,
+            5,
+            5,
+            1,
+            1] + [
+                          -7,
+                          -7,
+                          1,
+                          1,
+                          30,
+                          30,
+                          -60,
+                          -60,
+                          30,
+                          30,
+                          0,
+                          0]
+        arms = np.copy(self.DEFAULT_ARMS)
+        arms[0:4] += a[12:16] * 4
+        r.set_joints_target_position_direct(slice(2, 14), leg_actions, False)
+        r.set_joints_target_position_direct([0, 1], np.array([0, -44], float), False)
+        r.set_joints_target_position_direct(slice(14, 22), arms, False)
+        self.step_counter += 1
+        return self.step_counter > 3

behaviors/custom/Kick_RL/Kick_RL.py

@@ -0,0 +1,111 @@
+import pickle
+from math_ops.Math_Ops import Math_Ops as M
+import numpy as np
+from behaviors.custom.Kick_RL.Env import Env
+from math_ops.Neural_Network import run_mlp
+import math
+from agent.Base_Agent import Base_Agent
+
+
+class Kick_RL:
+
+    def __init__(self, base_agent: Base_Agent) -> None:
+        self.base_agent = base_agent
+        self.world = base_agent.world
+        self.description = 'Walk to ball and kick in a given direction'
+        self.auto_head = True
+        self.env = Env(self.base_agent,self.world )
+        self.ik = base_agent.inv_kinematics
+        self.behavior = base_agent.behavior
+
+        if self.world.robot.type == 1:
+            model_file = "/behaviors/custom/Kick_RL/kick_all.pkl"
+            self.kick_time = (8, 8)
+            self.kick_target_bias = (14, 7)
+            self.kick_cap_spd = 4
+            self.kick_dist = 7
+            self.kick_init_pos = 0.3
+            with open(M.get_active_directory(model_file), 'rb') as f:
+                self.model = pickle.load(f)
+        else:
+            model_file = "/behaviors/custom/Kick_RL/kick_r2.pkl"
+            self.kick_time = (11, 9)
+            self.kick_target_bias = (7, 0)
+            self.kick_cap_spd = 7.03
+            self.kick_dist = 11
+            self.kick_init_pos = 0.26 if self.world.robot.type == 0 else 0.26
+            with open(M.get_active_directory(model_file), 'rb') as f:
+                self.model = pickle.load(f)
+
+    # WARNING: Decompyle incomplete
+
+    def get_kick_pos_rot(self, direction, ball_2d, cap_power):
+        ''' Returns kick position and desired robot orientation (with correction bias) '''
+        direction += self.kick_target_bias[int(cap_power)]
+        dir_rad = direction * math.pi / 180
+        target_unit_vec = np.array([
+            math.cos(dir_rad),
+            math.sin(dir_rad)])
+        left_unit_vec = np.array([
+            -target_unit_vec[1],
+            target_unit_vec[0]])
+        return (ball_2d - self.kick_init_pos * target_unit_vec) + 0.045 * left_unit_vec, direction
+
+    def execute(self, reset, direction, cap_power=(False,)):
+        r = self.world.robot
+        b = self.world.ball_abs_pos[:2]
+        me = r.loc_torso_position[:2]
+        if reset:
+            self.phase = 0
+        kick_dir = direction + self.kick_target_bias[int(cap_power)] if self.phase == 0 else self.env.kick_dir
+        dir_rad = kick_dir * math.pi / 180
+        target_unit_vec = np.array([
+            math.cos(dir_rad),
+            math.sin(dir_rad)])
+        left_unit_vec = np.array([
+            -target_unit_vec[1],
+            target_unit_vec[0]])
+        ball_rel_hip_center = self.ik.torso_to_hip_transform(self.world.ball_rel_torso_cart_pos)
+        ball_dist = np.linalg.norm(ball_rel_hip_center)
+        rot_with_bias = kick_dir + 8
+        if self.phase == 0:
+            target = (b - self.kick_init_pos * target_unit_vec) + 0.045 * left_unit_vec
+            if np.linalg.norm(target - me) < 0.1:
+                self.phase = 1
+                self.env.kick_dir = kick_dir
+            if reset:
+                pass
+            reset_walk = self.behavior.previous_behavior != 'Walk'
+            self.behavior.execute_sub_behavior('Walk', reset_walk, target, True, rot_with_bias, True, None)
+        elif self.phase == 1:
+            target = (b - 0.1 * target_unit_vec) + 0.045 * left_unit_vec
+            if ball_dist < 0.27:
+                self.phase = 2
+            else:
+                self.behavior.execute_sub_behavior('Walk', False, target, True, rot_with_bias, True, None)
+        if self.phase == 2:
+            walk_rl = self.behavior.get_custom_behavior_object('Walk').env
+            if walk_rl.step_generator.state_is_left_active and walk_rl.step_generator.state_current_ts == 1:
+                self.phase = 3
+            else:
+                self.behavior.execute_sub_behavior('Walk', False, (0, 0), True, 0, True, None)
+        if self.phase == 3:
+            obs = self.env.observe(True)
+            action = run_mlp(obs, self.model)
+            self.env.execute(action)
+            self.phase = 4
+            return False
+        if self.phase > 3:
+            obs = self.env.observe(False)
+            action = run_mlp(obs, self.model)
+            self.env.execute(action)
+            self.phase += 1
+            print(self.phase)
+            if self.phase >= self.kick_time[int(cap_power)]:
+                print("True")
+                return True
+            return False
+
+    def is_ready(self):
+        ''' Returns True if Walk Behavior is ready to start under current game/robot conditions '''
+        return True

behaviors/custom/Long_Kick/Env.py

@@ -0,0 +1,110 @@
+# Source Generated with Decompyle++
+# File: Env.pyc (Python 3.6)
+from agent.Base_Agent import Base_Agent
+
+import numpy as np
+from math_ops.Math_Ops import Math_Ops as U
+from world.World import World
+
+
+class Env:
+
+    def __init__(self,base_agent: Base_Agent, world=None):
+        self.base_agent = base_agent
+        self.world = base_agent.world
+        self.obs = np.zeros(64, np.float32)
+        self.DEFAULT_ARMS = np.array([
+            -90,
+            -90,
+            8,
+            8,
+            90,
+            90,
+            70,
+            70], np.float32)
+        self.kick_ori = None
+        self.kick_dist = None
+
+    def observe(self, init=(False,)):
+        w = self.world
+        r = self.world.robot
+        if init:
+            self.step_counter = 0
+            self.act = np.zeros(16, np.float32)
+        self.obs[0] = self.step_counter / 20
+        self.obs[1] = r.loc_head_z * 3
+        self.obs[2] = r.loc_head_z_vel / 2
+        self.obs[3] = r.imu_torso_roll / 15
+        self.obs[4] = r.imu_torso_pitch / 15
+        self.obs[5:8] = r.gyro / 100
+        self.obs[8:11] = r.acc / 10
+        self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01, 0.01)
+        self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01, 0.01)
+        self.obs[23:39] = r.joints_position[2:18] / 100
+        self.obs[39:55] = r.joints_speed[2:18] / 6.1395
+        ball_rel_hip_center = self.base_agent.inv_kinematics.torso_to_hip_transform(w.ball_rel_torso_cart_pos)
+        if init:
+            self.obs[55:58] = (0, 0, 0)
+        elif w.ball_is_visible:
+            self.obs[55:58] = (ball_rel_hip_center - self.obs[58:61]) * 10
+        self.obs[58:61] = ball_rel_hip_center
+        self.obs[61] = np.linalg.norm(ball_rel_hip_center) * 2
+        self.obs[62] = U.normalize_deg(self.kick_ori - r.imu_torso_orientation) / 30
+        return self.obs
+
+    def execute(self, action):
+        w = self.world
+        r = self.world.robot
+        if self.step_counter < 6:
+            r.joints_target_speed[2:18] = action * [
+                2,
+                2,
+                1,
+                1,
+                0.2,
+                0.2,
+                0.2,
+                0.2,
+                2,
+                2,
+                1,
+                1,
+                1,
+                1,
+                1,
+                1]
+            r.joints_target_speed[12] += 1
+            r.joints_target_speed[6] += 3
+            r.joints_target_speed[5] -= 1
+            r.joints_target_speed[8] += 3
+            r.joints_target_speed[9] -= 5
+        else:
+            r.joints_target_speed[2:18] = action * [
+                6,
+                6,
+                3,
+                3,
+                6,
+                6,
+                6,
+                6,
+                6,
+                6,
+                2,
+                2,
+                1,
+                1,
+                1,
+                1]
+            r.joints_target_speed[6] -= 3
+            r.joints_target_speed[7] += 3
+            r.joints_target_speed[9] += 3
+            r.joints_target_speed[11] -= 1
+        r.set_joints_target_position_direct([
+            0,
+            1], np.array([
+            0,
+            -44], float), False)
+        self.step_counter += 1
+        return self.step_counter >= 18
+

behaviors/custom/Long_Kick/Long_Kick.py

@@ -0,0 +1,101 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Nerul_Kick.Env import Env
+from math_ops.Math_Ops import Math_Ops as M
+from math_ops.Neural_Network import run_mlp
+import numpy as np
+import pickle
+from behaviors.custom.Step.Step_Generator import Step_Generator
+
+
+class Long_Kick():
+
+    def __init__(self, base_agent: Base_Agent) -> None:
+        self.phase = None
+        self.reset_time = None
+        self.behavior = base_agent.behavior
+        self.path_manager = base_agent.path_manager
+        self.world = base_agent.world
+        self.description = "RL dribble"
+        self.auto_head = True
+        self.env = Env(base_agent)
+        self.kick_flag = 0
+        r_type = self.world.robot.type
+        self.bias_dir = [0.09, 0.1, 0.14, 0.08, 0.05][self.world.robot.type]
+        self.ball_x_limits = ((0.19,0.215), (0.22, 0.245), (0.20, 0.22),(0.2,0.215), (0.20, 0.22))[r_type]
+        self.ball_y_limits = ((-0.115,-0.1), (-0.045,-0.025), (-0.11, -0.07), (-0.06,-0.03), (-0.055, -0.035))[r_type]
+        self.ball_x_center = 0.21
+        self.ball_y_center = -0.045
+        with open(M.get_active_directory([
+                                             "/behaviors/custom/Long_Kick/T4_best_model.zip.pkl",
+                                             "/behaviors/custom/Long_Kick/T4_best_model.zip.pkl",
+                                             "/behaviors/custom/Long_Kick/T4_best_model.zip.pkl",
+                                             "/behaviors/custom/Long_Kick/T4_best_model.zip.pkl",
+                                             "/behaviors/custom/Long_Kick/T4_best_model.zip.pkl"
+                                         ][self.world.robot.type]), 'rb') as f:
+            self.model = pickle.load(f)
+
+    def execute(self, reset, direction, abort=False):
+        '''
+        Parameters
+        ----------
+        orientation : float
+            absolute or relative orientation of torso (relative to imu_torso_orientation), in degrees
+            set to None to dribble towards the opponent's goal (is_orientation_absolute is ignored)
+        is_orientation_absolute : bool
+            True if orientation is relative to the field, False if relative to the robot's torso
+        speed : float
+            speed from 0 to 1 (scale is not linear)
+        stop : bool
+            return True immediately if walking, wind down if dribbling, and return True when possible
+        '''
+
+        w = self.world
+        r = self.world.robot
+        b = w.ball_rel_torso_cart_pos
+        t = w.time_local_ms
+        gait: Step_Generator = self.behavior.get_custom_behavior_object("Walk").env.step_generator
+
+        if reset:
+            self.kick_flag = 0
+            self.phase = 0
+            self.reset_time = t
+
+
+        if self.phase == 0:
+            biased_dir = M.normalize_deg(direction + self.bias_dir)  # add bias to rectify direction
+            ang_diff = abs(
+                M.normalize_deg(biased_dir - r.loc_torso_orientation))  # the reset was learned with loc, not IMU
+
+            next_pos, next_ori, dist_to_final_target = self.path_manager.get_path_to_ball(
+                x_ori=biased_dir, x_dev=-self.ball_x_center, y_dev=-self.ball_y_center, torso_ori=biased_dir)
+
+            if (w.ball_last_seen > t - w.VISUALSTEP_MS and ang_diff < 5 and
+                    t - w.ball_abs_pos_last_update < 100 and  # ball absolute location is recent
+                    dist_to_final_target < 0.025 and  # if absolute ball position is updated
+                    not gait.state_is_left_active and gait.state_current_ts == 2 and  # walk gait phase is adequate
+                    t - self.reset_time > 500):  # to avoid kicking immediately without preparation & stability
+                self.phase = 1
+                self.env.kick_ori = direction
+                obs = self.env.observe(True)
+                action = run_mlp(obs, self.model)
+                self.env.execute(action)
+            else:
+                dist = max(0.07, dist_to_final_target)
+                reset_walk = reset and self.behavior.previous_behavior != "Walk"  # reset walk if it wasn't the previous behavior
+                self.behavior.execute_sub_behavior("Walk", reset_walk, next_pos, True, next_ori, True,
+                                                   dist)  # target, is_target_abs, ori, is_ori_abs, distance
+                self.phase = 0
+                return abort  # abort only if self.phase == 0
+        else:
+            if (self.phase > 20):
+                return abort
+            print("kick")
+            self.env.kick_ori = direction
+            obs = self.env.observe(False)
+            action = run_mlp(obs, self.model)
+            self.env.execute(action)
+            self.phase += 1
+
+    def is_ready(self):
+        ''' Returns True if this behavior is ready to start/continue under current game/robot conditions '''
+        return True

behaviors/custom/Nerul_Kick/Env.py

@@ -0,0 +1,111 @@
+# Source Generated with Decompyle++
+# File: Env.pyc (Python 3.6)
+from agent.Base_Agent import Base_Agent
+
+import numpy as np
+from math_ops.Math_Ops import Math_Ops as U
+from world.World import World
+
+
+class Env:
+
+    def __init__(self,base_agent: Base_Agent, world=None):
+        self.base_agent = base_agent
+        self.world = base_agent.world
+        self.obs = np.zeros(64, np.float32)
+        self.DEFAULT_ARMS = np.array([
+            -90,
+            -90,
+            8,
+            8,
+            90,
+            90,
+            70,
+            70], np.float32)
+        self.kick_ori = None
+        self.kick_dist = None
+
+    def observe(self, init=(False,)):
+        w = self.world
+        r = self.world.robot
+        if init:
+            self.step_counter = 0
+            self.act = np.zeros(16, np.float32)
+        self.obs[0] = self.step_counter / 20
+        self.obs[1] = r.loc_head_z * 3
+        self.obs[2] = r.loc_head_z_vel / 2
+        self.obs[3] = r.imu_torso_roll / 15
+        self.obs[4] = r.imu_torso_pitch / 15
+        self.obs[5:8] = r.gyro / 100
+        self.obs[8:11] = r.acc / 10
+        self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01, 0.01)
+        self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01, 0.01)
+        self.obs[23:39] = r.joints_position[2:18] / 100
+        self.obs[39:55] = r.joints_speed[2:18] / 6.1395
+        ball_rel_hip_center = self.base_agent.inv_kinematics.torso_to_hip_transform(w.ball_rel_torso_cart_pos)
+        if init:
+            self.obs[55:58] = (0, 0, 0)
+        elif w.ball_is_visible:
+            self.obs[55:58] = (ball_rel_hip_center - self.obs[58:61]) * 10
+        self.obs[58:61] = ball_rel_hip_center
+        self.obs[61] = np.linalg.norm(ball_rel_hip_center) * 2
+        self.obs[62] = U.normalize_deg(self.kick_ori - r.imu_torso_orientation) / 30
+        self.obs[63] = 1
+        return self.obs
+
+    def execute(self, action):
+        w = self.world
+        r = self.world.robot
+        if self.step_counter < 2:
+            r.joints_target_speed[2:18] = action * [
+                2,
+                2,
+                1,
+                1,
+                0.2,
+                0.2,
+                0.2,
+                0.2,
+                2,
+                2,
+                1,
+                1,
+                1,
+                1,
+                1,
+                1]
+            r.joints_target_speed[12] += 1
+            r.joints_target_speed[6] += 3
+            r.joints_target_speed[5] -= 1
+            r.joints_target_speed[8] += 3
+            r.joints_target_speed[9] -= 5
+        else:
+            r.joints_target_speed[2:18] = action * [
+                6,
+                6,
+                3,
+                3,
+                6,
+                6,
+                6,
+                6,
+                6,
+                6,
+                2,
+                2,
+                1,
+                1,
+                1,
+                1]
+            r.joints_target_speed[6] -= 3
+            r.joints_target_speed[7] += 3
+            r.joints_target_speed[9] += 3
+            r.joints_target_speed[11] -= 1
+        r.set_joints_target_position_direct([
+            0,
+            1], np.array([
+            0,
+            -44], float), False)
+        self.step_counter += 1
+        return self.step_counter >= 14
+

behaviors/custom/Nerul_Kick/Nerul_Kick.py

@@ -0,0 +1,106 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Nerul_Kick.Env import Env
+from math_ops.Math_Ops import Math_Ops as M
+from math_ops.Neural_Network import run_mlp
+import numpy as np
+import pickle
+from behaviors.custom.Step.Step_Generator import Step_Generator
+
+
+class Nerul_Kick():
+
+    def __init__(self, base_agent: Base_Agent) -> None:
+        self.phase = None
+        self.reset_time = None
+        self.behavior = base_agent.behavior
+        self.path_manager = base_agent.path_manager
+        self.world = base_agent.world
+        self.description = "RL dribble"
+        self.auto_head = True
+        self.env = Env(base_agent)
+        self.kick_flag = 0
+        r_type = self.world.robot.type
+        self.bias_dir = [0.09, 0.1, 0.14, 0.08, 0.05][self.world.robot.type]
+        self.ball_x_limits = ((0.19,0.215), (0.22, 0.245), (0.20, 0.22),(0.2,0.215), (0.20, 0.22))[r_type]
+        self.ball_y_limits = ((-0.115,-0.1), (-0.045,-0.025), (-0.11, -0.07), (-0.06,-0.03), (-0.055, -0.035))[r_type]
+        self.ball_x_center = 0.21
+        self.ball_y_center = -0.045
+        with open(M.get_active_directory([
+                                             "/behaviors/custom/Nerul_Kick/short_kick_R0_31_14.3M.pkl",
+                                             "/behaviors/custom/Nerul_Kick/model_14336000_steps.zip.pkl",
+                                             "/behaviors/custom/Nerul_Kick/short_kick_R2_01_14976000.pkl",
+                                             "/behaviors/custom/Nerul_Kick/short_kick_R3_14976000.pkl",
+                                             "/behaviors/custom/Nerul_Kick/short_kick_R4_14400000.pkl"
+                                         ][self.world.robot.type]), 'rb') as f:
+            self.model = pickle.load(f)
+
+    def execute(self, reset, direction, abort=False):
+        '''
+        Parameters
+        ----------
+        orientation : float
+            absolute or relative orientation of torso (relative to imu_torso_orientation), in degrees
+            set to None to dribble towards the opponent's goal (is_orientation_absolute is ignored)
+        is_orientation_absolute : bool
+            True if orientation is relative to the field, False if relative to the robot's torso
+        speed : float
+            speed from 0 to 1 (scale is not linear)
+        stop : bool
+            return True immediately if walking, wind down if dribbling, and return True when possible
+        '''
+
+        w = self.world
+        r = self.world.robot
+        b = w.ball_rel_torso_cart_pos
+        t = w.time_local_ms
+        gait: Step_Generator = self.behavior.get_custom_behavior_object("Walk").env.step_generator
+
+        if reset:
+            self.kick_flag = 0
+            self.phase = 0
+            self.reset_time = t
+
+
+        if self.phase == 0:
+            biased_dir = M.normalize_deg(direction + self.bias_dir)  # add bias to rectify direction
+            ang_diff = abs(
+                M.normalize_deg(biased_dir - r.loc_torso_orientation))  # the reset was learned with loc, not IMU
+
+            next_pos, next_ori, dist_to_final_target = self.path_manager.get_path_to_ball(
+                x_ori=biased_dir, x_dev=-self.ball_x_center, y_dev=-self.ball_y_center, torso_ori=biased_dir)
+            print(w.ball_last_seen > t - w.VISUALSTEP_MS and ang_diff < 5 and
+                    t - w.ball_abs_pos_last_update < 100 and  # ball absolute location is recent
+                    dist_to_final_target < 0.025 and  # if absolute ball position is updated
+                    not gait.state_is_left_active and gait.state_current_ts == 2 and  # walk gait phase is adequate
+                    t - self.reset_time > 50)
+            if (w.ball_last_seen > t - w.VISUALSTEP_MS and ang_diff < 5 and
+                    t - w.ball_abs_pos_last_update < 100 and  # ball absolute location is recent
+                    dist_to_final_target < 0.025 and  # if absolute ball position is updated
+                    not gait.state_is_left_active and gait.state_current_ts == 2 and  # walk gait phase is adequate
+                    t - self.reset_time > 500):  # to avoid kicking immediately without preparation & stability
+                self.phase = 1
+                print("kick")
+                self.env.kick_ori = direction
+                obs = self.env.observe(True)
+                action = run_mlp(obs, self.model)
+                self.env.execute(action)
+            else:
+                dist = max(0.07, dist_to_final_target)
+                reset_walk = reset and self.behavior.previous_behavior != "Walk"  # reset walk if it wasn't the previous behavior
+                self.behavior.execute_sub_behavior("Walk", reset_walk, next_pos, True, next_ori, True,
+                                                   dist)  # target, is_target_abs, ori, is_ori_abs, distance
+                self.phase = 0
+                return abort  # abort only if self.phase == 0
+        else:
+            if (self.phase > 20):
+                return abort
+            print("kick")
+            self.env.kick_ori = direction
+            obs = self.env.observe(False)
+            action = run_mlp(obs, self.model)
+            self.env.execute(action)
+            self.phase += 1
+
+    def is_ready(self):
+        ''' Returns True if this behavior is ready to start/continue under current game/robot conditions '''
+        return True

behaviors/custom/Sprint/Env.py

@@ -0,0 +1,183 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Step.Step_Generator import Step_Generator
+from math_ops.Math_Ops import Math_Ops as M
+import math
+import numpy as np
+
+
+class Env:
+    def __init__(self, base_agent: Base_Agent, step_width) -> None:
+
+        self.gym_last_internal_abs_ori = None
+        self.world = base_agent.world
+        self.ik = base_agent.inv_kinematics
+
+        # State space  
+        self.obs = np.zeros(76, np.float32)
+
+        # Step behavior defaults
+        self.STEP_DUR = 8
+        self.STEP_Z_SPAN = 0.02
+        self.STEP_Z_MAX = 0.70
+
+        # IK 
+        r = self.world.robot
+        nao_specs = self.ik.NAO_SPECS
+        self.leg_length = nao_specs[1] + nao_specs[3]  # upper leg height + lower leg height
+        feet_y_dev = nao_specs[0] * step_width  # wider step
+        sample_time = r.STEPTIME
+        max_ankle_z = nao_specs[5]
+
+        self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z)
+        self.DEFAULT_ARMS = np.array([-90, -90, 8, 8, 90, 90, 70, 70], np.float32)
+
+        self.dribble_rel_orientation = None  # relative to imu_torso_orientation (in degrees)
+        self.dribble_speed = 1
+
+    def observe(self, init=False, virtual_ball=False):
+
+        w = self.world
+        r = self.world.robot
+
+        if init:  # reset variables
+            self.step_counter = 0
+            self.act = np.zeros(16, np.float32)  # memory variable
+
+        # index       observation              naive normalization
+        self.obs[0] = min(self.step_counter, 12 * 8) / 100  # simple counter: 0,1,2,3...
+        self.obs[1] = r.loc_head_z * 3  # z coordinate (torso)
+        self.obs[2] = r.loc_head_z_vel / 2  # z velocity (torso)
+        self.obs[3] = r.imu_torso_roll / 15  # absolute torso roll  in deg
+        self.obs[4] = r.imu_torso_pitch / 15  # absolute torso pitch in deg
+        self.obs[5:8] = r.gyro / 100  # gyroscope
+        self.obs[8:11] = r.acc / 10  # accelerometer
+
+        self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01,
+                                                          0.01)  # left foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01,
+                                                          0.01)  # right foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        # *if foot is not touching the ground, then (px=0,py=0,pz=0,fx=0,fy=0,fz=0)
+
+        self.obs[23:43] = r.joints_position[2:22] / 100  # position of all joints except head & toes (for robot type 4)
+        self.obs[43:63] = r.joints_speed[2:22] / 6.1395  # speed of    all joints except head & toes (for robot type 4)
+
+        '''
+        Expected observations for walking state:
+        Time step        R  0   1   2   3   4   5   6   7   0
+        Progress         1  0 .14 .28 .43 .57 .71 .86   1   0
+        Left leg active  T  F   F   F   F   F   F   F   F   T
+        '''
+
+        if init:  # the walking parameters refer to the last parameters in effect (after a reset, they are pointless)
+            self.obs[63] = 1  # step progress
+            self.obs[64] = 1  # 1 if left  leg is active
+            self.obs[65] = 0  # 1 if right leg is active
+            self.obs[66] = 0
+        else:
+            self.obs[63] = self.step_generator.external_progress  # step progress
+            self.obs[64] = float(self.step_generator.state_is_left_active)  # 1 if left  leg is active
+            self.obs[65] = float(not self.step_generator.state_is_left_active)  # 1 if right leg is active
+            self.obs[66] = math.sin(self.step_generator.state_current_ts / self.step_generator.ts_per_step * math.pi)
+
+        # Ball
+        ball_rel_hip_center = self.ik.torso_to_hip_transform(w.ball_rel_torso_cart_pos)
+        ball_dist_hip_center = np.linalg.norm(ball_rel_hip_center)
+
+        if init:
+            self.obs[67:70] = (0, 0, 0)  # Initial velocity is 0
+        elif w.ball_is_visible:
+            self.obs[67:70] = (ball_rel_hip_center - self.obs[70:73]) * 10  # Ball velocity, relative to ankle's midpoint
+
+        self.obs[70:73] = ball_rel_hip_center  # Ball position, relative to hip
+        self.obs[73] = ball_dist_hip_center * 2
+
+        if virtual_ball:  # simulate the ball between the robot's feet
+            self.obs[67:74] = (0, 0, 0, 0.05, 0, -0.175, 0.36)
+
+        '''
+        Create internal target with a smoother variation
+        '''
+
+        MAX_ROTATION_DIFF = 20  # max difference (degrees) per visual step
+        MAX_ROTATION_DIST = 80
+
+        if init:
+            self.internal_rel_orientation = 0
+            self.internal_target_vel = 0
+            self.gym_last_internal_abs_ori = r.imu_torso_orientation  # for training purposes (reward)
+
+        # ---------------------------------------------------------------- compute internal target
+
+        if w.vision_is_up_to_date:
+            previous_internal_rel_orientation = np.copy(self.internal_rel_orientation)
+
+            internal_ori_diff = np.clip(M.normalize_deg(self.dribble_rel_orientation - self.internal_rel_orientation),
+                                        -MAX_ROTATION_DIFF, MAX_ROTATION_DIFF)
+            self.internal_rel_orientation = np.clip(M.normalize_deg(self.internal_rel_orientation + internal_ori_diff),
+                                                    -MAX_ROTATION_DIST, MAX_ROTATION_DIST)
+
+            # Observations
+            self.internal_target_vel = self.internal_rel_orientation - previous_internal_rel_orientation
+
+            self.gym_last_internal_abs_ori = self.internal_rel_orientation + r.imu_torso_orientation
+
+        # ----------------------------------------------------------------- observations
+
+        self.obs[74] = self.internal_rel_orientation / MAX_ROTATION_DIST
+        self.obs[75] = self.internal_target_vel / MAX_ROTATION_DIFF
+
+        return self.obs
+
+    def execute_ik(self, l_pos, l_rot, r_pos, r_rot):
+        r = self.world.robot
+        # Apply IK to each leg + Set joint targets
+
+        # Left leg 
+        indices, self.values_l, error_codes = self.ik.leg(l_pos, l_rot, True, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_l, harmonize=False)
+
+        # Right leg
+        indices, self.values_r, error_codes = self.ik.leg(r_pos, r_rot, False, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_r, harmonize=False)
+
+    def execute(self, action):
+
+        r = self.world.robot
+        # Actions:
+        # 0,1,2    left ankle pos
+        # 3,4,5    right ankle pos
+        # 6,7,8    left foot rotation
+        # 9,10,11  right foot rotation
+        # 12,13    left/right arm pitch
+        # 14,15    left/right arm roll
+
+        # exponential moving average
+        self.act = 0.85 * self.act + 0.15 * action * 0.7 * 0.95 * self.dribble_speed
+
+        # execute Step behavior to extract the target positions of each leg (we will override these targets)
+        lfy, lfz, rfy, rfz = self.step_generator.get_target_positions(self.step_counter == 0, self.STEP_DUR,
+                                                                      self.STEP_Z_SPAN,
+                                                                      self.leg_length * self.STEP_Z_MAX)
+
+        # Leg IK
+        a = self.act
+        l_ankle_pos = (a[0] * 0.025 - 0.01, a[1] * 0.01 + lfy, a[2] * 0.01 + lfz)
+        r_ankle_pos = (a[3] * 0.025 - 0.01, a[4] * 0.01 + rfy, a[5] * 0.01 + rfz)
+        l_foot_rot = a[6:9] * (2, 2, 3)
+        r_foot_rot = a[9:12] * (2, 2, 3)
+
+        # Limit leg yaw/pitch (and add bias)
+        l_foot_rot[2] = max(0, l_foot_rot[2] + 18.3)
+        r_foot_rot[2] = min(0, r_foot_rot[2] - 18.3)
+
+        # Arms actions
+        arms = np.copy(self.DEFAULT_ARMS)  # default arms pose
+        arms[0:4] += a[12:16] * 4  # arms pitch+roll
+
+        # Set target positions
+        self.execute_ik(l_ankle_pos, l_foot_rot, r_ankle_pos, r_foot_rot)  # legs
+        r.set_joints_target_position_direct(slice(14, 22), arms, harmonize=False)  # arms
+
+        self.step_counter += 1

behaviors/custom/Sprint/Sprint.py

@@ -0,0 +1,178 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Sprint.Env import Env
+from math_ops.Math_Ops import Math_Ops as M
+from math_ops.Neural_Network import run_mlp
+import numpy as np
+import pickle
+
+
+class Sprint:
+
+    def __init__(self, base_agent: Base_Agent) -> None:
+        self.behavior = base_agent.behavior
+        self.path_manager = base_agent.path_manager
+        self.world = base_agent.world
+        self.description = "RL dribble"
+        self.auto_head = True
+        self.env = Env(base_agent, 0.9 if self.world.robot.type == 3 else 1.2)
+
+        with open(M.get_active_directory([
+                                             "/behaviors/custom/Sprint/Sprint_R0.pkl",
+                                             "/behaviors/custom/Sprint/Sprint_R1.pkl",
+                                             "/behaviors/custom/Sprint/Sprint_R2.pkl",
+                                             "/behaviors/custom/Sprint/Sprint_R3.pkl",
+                                             "/behaviors/custom/Sprint/Sprint_R4.pkl"
+                                         ][self.world.robot.type]), 'rb') as f:
+            self.model = pickle.load(f)
+
+    def define_approach_orientation(self):
+
+        w = self.world
+        b = w.ball_abs_pos[:2]
+        me = w.robot.loc_head_position[:2]
+
+        self.approach_orientation = None
+
+        MARGIN = 0.8  # safety margin (if ball is near the field limits by this amount, the approach orientation is considered)
+        M90 = 90 / MARGIN  # auxiliary variable
+        DEV = 25  # when standing on top of sidelines or endlines, the approach direction deviates from that line by this amount
+        MDEV = (90 + DEV) / MARGIN  # auxiliary variable
+
+        a1 = -180  # angle range start (counterclockwise rotation)
+        a2 = 180  # angle range end   (counterclockwise rotation)
+
+        if b[1] < -10 + MARGIN:
+            if b[0] < -15 + MARGIN:
+                a1 = DEV - M90 * (b[1] + 10)
+                a2 = 90 - DEV + M90 * (b[0] + 15)
+            elif b[0] > 15 - MARGIN:
+                a1 = 90 + DEV - M90 * (15 - b[0])
+                a2 = 180 - DEV + M90 * (b[1] + 10)
+            else:
+                a1 = DEV - MDEV * (b[1] + 10)
+                a2 = 180 - DEV + MDEV * (b[1] + 10)
+        elif b[1] > 10 - MARGIN:
+            if b[0] < -15 + MARGIN:
+                a1 = -90 + DEV - M90 * (b[0] + 15)
+                a2 = -DEV + M90 * (10 - b[1])
+            elif b[0] > 15 - MARGIN:
+                a1 = 180 + DEV - M90 * (10 - b[1])
+                a2 = 270 - DEV + M90 * (15 - b[0])
+            else:
+                a1 = -180 + DEV - MDEV * (10 - b[1])
+                a2 = -DEV + MDEV * (10 - b[1])
+        elif b[0] < -15 + MARGIN:
+            a1 = -90 + DEV - MDEV * (b[0] + 15)
+            a2 = 90 - DEV + MDEV * (b[0] + 15)
+        elif b[0] > 15 - MARGIN and abs(b[1]) > 1.2:
+            a1 = 90 + DEV - MDEV * (15 - b[0])
+            a2 = 270 - DEV + MDEV * (15 - b[0])
+
+        cad = M.vector_angle(b - me)  # current approach direction
+
+        a1 = M.normalize_deg(a1)
+        a2 = M.normalize_deg(a2)
+
+        if a1 < a2:
+            if a1 <= cad <= a2:
+                return  # current approach orientation is within accepted range
+        else:
+            if a1 <= cad or cad <= a2:
+                return  # current approach orientation is within accepted range
+
+        a1_diff = abs(M.normalize_deg(a1 - cad))
+        a2_diff = abs(M.normalize_deg(a2 - cad))
+
+        self.approach_orientation = a1 if a1_diff < a2_diff else a2  # fixed normalized orientation
+
+    def execute(self, reset, target, orientation, is_orientation_absolute, speed=1, stop=False):
+        '''
+        Parameters
+        ----------
+        target
+        orientation : float
+            absolute or relative orientation of torso (relative to imu_torso_orientation), in degrees
+            set to None to dribble towards the opponent's goal (is_orientation_absolute is ignored)
+        is_orientation_absolute : bool
+            True if orientation is relative to the field, False if relative to the robot's torso
+        speed : float
+            speed from 0 to 1 (scale is not linear)
+        stop : bool
+            return True immediately if walking, wind down if dribbling, and return True when possible
+        '''
+        w = self.world
+        r = self.world.robot
+        me = r.loc_head_position[:2]
+        b = w.ball_abs_pos[:2]
+        b_rel = w.ball_rel_torso_cart_pos[:2]
+        b_dist = np.linalg.norm(b - me)
+        behavior = self.behavior
+        reset_dribble = False
+        lost_ball = (w.ball_last_seen <= w.time_local_ms - w.VISUALSTEP_MS) or np.linalg.norm(b_rel) > 0.4
+
+        if reset:
+            self.phase = 0
+            if behavior.previous_behavior == "Push_RL" and 0 < b_rel[0] < 0.25 and abs(b_rel[1]) < 0.07:
+                self.phase = 1
+                reset_dribble = True
+
+        if self.phase == 0:  # prepare to sprint
+            self.phase = 1
+            reset_dribble = True
+
+            if stop:
+                return True
+
+        if self.phase == 1 and (stop):  # go back to walking
+            self.phase += 1
+        elif self.phase == 1:  # dribble
+            # ------------------------ 1. Define dribble parameters
+            self.env.dribble_speed = speed
+            if target is not None:
+                raw_target = target - r.loc_head_position[:2]
+                walk_rel_target = M.rotate_2d_vec(raw_target, -r.imu_torso_orientation)
+                orientation = M.vector_angle(walk_rel_target)
+            # Relative orientation values are decreased to avoid overshoot
+            if orientation is None:
+                if b[0] < 0:  # dribble to the sides
+                    if b[1] > 0:
+                        dribble_target = (15, 5)
+                    else:
+                        dribble_target = (15, -5)
+                else:
+                    dribble_target = None  # go to goal
+                self.env.dribble_rel_orientation = \
+                    self.path_manager.get_dribble_path(optional_2d_target=dribble_target)[1]
+            elif is_orientation_absolute:
+                self.env.dribble_rel_orientation = M.normalize_deg(orientation - r.imu_torso_orientation)
+            else:  # sprint use this
+                self.env.dribble_rel_orientation = float(orientation)  # copy if numpy float
+
+            # ------------------------ 2. Execute behavior
+            obs = self.env.observe(reset_dribble, virtual_ball=True)
+            action = run_mlp(obs, self.model)
+            self.env.execute(action)
+
+        # wind down dribbling, and then reset phase
+        if self.phase > 1:
+            WIND_DOWN_STEPS = 60
+            # ------------------------ 1. Define dribble wind down parameters
+            self.env.dribble_speed = 1 - self.phase / WIND_DOWN_STEPS
+            self.env.dribble_rel_orientation = 0
+
+            # ------------------------ 2. Execute behavior
+            obs = self.env.observe(reset_dribble, virtual_ball=True)
+            action = run_mlp(obs, self.model)
+            self.env.execute(action)
+
+            # ------------------------ 3. Reset behavior
+            self.phase += 1
+            if self.phase >= WIND_DOWN_STEPS - 5:
+                self.phase = 0
+                return True
+
+        return False
+
+    def is_ready(self):
+        ''' Returns True if this behavior is ready to start/continue under current game/robot conditions '''
+        return True

behaviors/custom/Step/Step.py

@@ -0,0 +1,59 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Step.Step_Generator import Step_Generator
+import numpy as np
+
+class Step():
+
+    def __init__(self, base_agent : Base_Agent) -> None:
+        self.world = base_agent.world
+        self.ik = base_agent.inv_kinematics
+        self.description = "Step (Skill-Set-Primitive)"
+        self.auto_head = True
+
+        nao_specs = self.ik.NAO_SPECS
+        self.leg_length = nao_specs[1] + nao_specs[3] # upper leg height + lower leg height
+
+        feet_y_dev = nao_specs[0] * 1.2 # wider step
+        sample_time = self.world.robot.STEPTIME
+        max_ankle_z = nao_specs[5]
+
+        # Initialize step generator with constants
+        self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z)
+
+
+    def execute(self,reset, ts_per_step=7, z_span=0.03, z_max=0.8):
+
+        lfy,lfz,rfy,rfz = self.step_generator.get_target_positions(reset, ts_per_step, z_span, self.leg_length * z_max)
+ 
+        #----------------- Apply IK to each leg + Set joint targets
+          
+        # Left leg 
+        indices, self.values_l, error_codes = self.ik.leg((0,lfy,lfz), (0,0,0), True, dynamic_pose=False)
+        for i in error_codes:
+            print(f"Joint {i} is out of range!" if i!=-1 else "Position is out of reach!")
+
+        self.world.robot.set_joints_target_position_direct(indices, self.values_l)
+
+        # Right leg
+        indices, self.values_r, error_codes = self.ik.leg((0,rfy,rfz), (0,0,0), False, dynamic_pose=False)
+        for i in error_codes:
+            print(f"Joint {i} is out of range!" if i!=-1 else "Position is out of reach!")
+
+        self.world.robot.set_joints_target_position_direct(indices, self.values_r)
+
+        # ----------------- Fixed arms
+
+        indices = [14,16,18,20]
+        values  = np.array([-80,20,90,0])
+        self.world.robot.set_joints_target_position_direct(indices,values)
+
+        indices = [15,17,19,21]
+        values  = np.array([-80,20,90,0])
+        self.world.robot.set_joints_target_position_direct(indices,values)
+
+        return False
+        
+
+    def is_ready(self):
+        ''' Returns True if Step Behavior is ready to start under current game/robot conditions '''
+        return True

behaviors/custom/Step/Step_Generator.py

@@ -0,0 +1,75 @@
+import math
+
+
+class Step_Generator():
+    GRAVITY = 9.81
+    Z0 = 0.2
+    
+    def __init__(self, feet_y_dev, sample_time, max_ankle_z) -> None:
+        self.feet_y_dev = feet_y_dev
+        self.sample_time = sample_time
+        self.state_is_left_active = False
+        self.state_current_ts = 0
+        self.switch = False # switch legs
+        self.external_progress = 0 # non-overlaped progress
+        self.max_ankle_z = max_ankle_z
+
+
+    def get_target_positions(self, reset, ts_per_step, z_span, z_extension):
+        '''
+        Get target positions for each foot
+
+        Returns
+        -------
+        target : `tuple`
+            (Left leg y, Left leg z, Right leg y, Right leg z)
+        '''
+
+        assert type(ts_per_step)==int and ts_per_step > 0, "ts_per_step must be a positive integer!"
+
+        #-------------------------- Advance 1ts
+        if reset:
+            self.ts_per_step = ts_per_step        # step duration in time steps
+            self.swing_height = z_span
+            self.max_leg_extension = z_extension  # maximum distance between ankle to center of both hip joints
+            self.state_current_ts = 0
+            self.state_is_left_active = False 
+            self.switch = False
+        elif self.switch:
+            self.state_current_ts = 0
+            self.state_is_left_active = not self.state_is_left_active # switch leg
+            self.switch = False
+        else:
+            self.state_current_ts += 1
+
+        #-------------------------- Compute COM.y
+        W = math.sqrt(self.Z0/self.GRAVITY)
+
+        step_time = self.ts_per_step * self.sample_time
+        time_delta = self.state_current_ts * self.sample_time
+ 
+        y0 = self.feet_y_dev # absolute initial y value
+        y_swing = y0 + y0 * (  math.sinh((step_time - time_delta)/W) + math.sinh(time_delta/W)  ) / math.sinh(-step_time/W)
+
+        #-------------------------- Cap maximum extension and swing height
+        z0 = min(-self.max_leg_extension, self.max_ankle_z) #  capped initial z value
+        zh = min(self.swing_height, self.max_ankle_z - z0) # capped swing height
+
+        #-------------------------- Compute Z Swing
+        progress = self.state_current_ts / self.ts_per_step
+        self.external_progress = self.state_current_ts / (self.ts_per_step-1)
+        active_z_swing = zh * math.sin(math.pi * progress)
+
+        #-------------------------- Accept new parameters after final step
+        if self.state_current_ts + 1 >= self.ts_per_step:
+            self.ts_per_step = ts_per_step        # step duration in time steps
+            self.swing_height = z_span
+            self.max_leg_extension = z_extension  # maximum distance between ankle to center of both hip joints
+            self.switch = True
+
+        #-------------------------- Distinguish active leg
+        if self.state_is_left_active:
+            return y0+y_swing, active_z_swing+z0, -y0+y_swing, z0
+        else:
+            return y0-y_swing, z0, -y0-y_swing, active_z_swing+z0
+

behaviors/custom/Stop/Env.py

@@ -0,0 +1,197 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Step.Step_Generator import Step_Generator
+from math_ops.Math_Ops import Math_Ops as M
+import math
+import numpy as np
+
+
+class Env:
+    def __init__(self, base_agent: Base_Agent, step_width) -> None:
+
+        self.walk_rel_orientation = None
+        self.walk_distance = None
+        self.walk_rel_target = None
+        self.gym_last_internal_abs_ori = None
+        self.world = base_agent.world
+        self.ik = base_agent.inv_kinematics
+
+        # State space
+        self.obs = np.zeros(63, np.float32)
+
+        # Step behavior defaults
+        self.STEP_DUR = 8
+        self.STEP_Z_SPAN = 0.02
+        self.STEP_Z_MAX = 0.70
+
+        # IK
+        r = self.world.robot
+        nao_specs = self.ik.NAO_SPECS
+        self.leg_length = nao_specs[1] + nao_specs[3]  # upper leg height + lower leg height
+        feet_y_dev = nao_specs[0] * step_width  # wider step
+        sample_time = r.STEPTIME
+        max_ankle_z = nao_specs[5]
+
+        self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z)
+        self.DEFAULT_ARMS = np.array([-90, -90, 8, 8, 90, 90, 70, 70], np.float32)
+
+        self.dribble_rel_orientation = None  # relative to imu_torso_orientation (in degrees)
+        self.dribble_speed = 1
+
+    def observe(self, init=False):
+
+        r = self.world.robot
+
+        if init:  # reset variables
+            self.act = np.zeros(16, np.float32)  # memory variable
+            self.step_counter = 0
+
+        # index       observation              naive normalization
+        self.obs[0] = min(self.step_counter, 15 * 8) / 100  # simple counter: 0,1,2,3...
+        self.obs[1] = r.loc_head_z * 3  # z coordinate (torso)
+        self.obs[2] = r.loc_head_z_vel / 2  # z velocity (torso)  
+        self.obs[3] = r.imu_torso_roll / 15  # absolute torso roll  in deg
+        self.obs[4] = r.imu_torso_pitch / 15  # absolute torso pitch in deg
+        self.obs[5:8] = r.gyro / 100  # gyroscope
+        self.obs[8:11] = r.acc / 10  # accelerometer
+
+        self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01,
+                                                          0.01)  # left foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01,
+                                                          0.01)  # right foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        # *if foot is not touching the ground, then (px=0,py=0,pz=0,fx=0,fy=0,fz=0)
+
+        # Joints: Forward kinematics for ankles + feet rotation + arms (pitch + roll)
+        rel_lankle = self.ik.get_body_part_pos_relative_to_hip(
+            "lankle")  # ankle position relative to center of both hip joints
+        rel_rankle = self.ik.get_body_part_pos_relative_to_hip(
+            "rankle")  # ankle position relative to center of both hip joints
+        lf = r.head_to_body_part_transform("torso", r.body_parts['lfoot'].transform)  # foot transform relative to torso
+        rf = r.head_to_body_part_transform("torso", r.body_parts['rfoot'].transform)  # foot transform relative to torso
+        lf_rot_rel_torso = np.array(
+            [lf.get_roll_deg(), lf.get_pitch_deg(), lf.get_yaw_deg()])  # foot rotation relative to torso
+        rf_rot_rel_torso = np.array(
+            [rf.get_roll_deg(), rf.get_pitch_deg(), rf.get_yaw_deg()])  # foot rotation relative to torso
+
+        # pose
+        self.obs[23:26] = rel_lankle * (8, 8, 5)
+        self.obs[26:29] = rel_rankle * (8, 8, 5)
+        self.obs[29:32] = lf_rot_rel_torso / 20
+        self.obs[32:35] = rf_rot_rel_torso / 20
+        self.obs[35:39] = r.joints_position[14:18] / 100  # arms (pitch + roll)
+
+        # velocity
+        self.obs[39:55] = r.joints_target_last_speed[2:18]  # predictions == last action
+
+        '''
+        Expected observations for walking state:
+        Time step        R  0   1   2   3   4   5   6   7   0
+        Progress         1  0 .14 .28 .43 .57 .71 .86   1   0
+        Left leg active  T  F   F   F   F   F   F   F   F   T
+        '''
+
+        if init:  # the walking parameters refer to the last parameters in effect (after a reset, they are pointless)
+            self.obs[55] = 1  # step progress
+            self.obs[56] = 1  # 1 if left  leg is active
+            self.obs[57] = 0  # 1 if right leg is active
+        else:
+            self.obs[55] = self.step_generator.external_progress  # step progress
+            self.obs[56] = float(self.step_generator.state_is_left_active)  # 1 if left  leg is active
+            self.obs[57] = float(not self.step_generator.state_is_left_active)  # 1 if right leg is active
+
+        '''
+        Create internal target with a smoother variation
+        '''
+
+        MAX_LINEAR_DIST = 0.5
+        MAX_LINEAR_DIFF = 0.014  # max difference (meters) per step
+        MAX_ROTATION_DIFF = 1.6  # max difference (degrees) per step
+        MAX_ROTATION_DIST = 45
+
+        if init:
+            self.internal_rel_orientation = 0
+            self.internal_target = np.zeros(2)
+
+        previous_internal_target = np.copy(self.internal_target)
+
+        # ---------------------------------------------------------------- compute internal linear target
+
+        rel_raw_target_size = np.linalg.norm(self.walk_rel_target)
+
+        if rel_raw_target_size == 0:
+            rel_target = self.walk_rel_target
+        else:
+            rel_target = self.walk_rel_target / rel_raw_target_size * min(self.walk_distance, MAX_LINEAR_DIST)
+
+        internal_diff = rel_target - self.internal_target
+        internal_diff_size = np.linalg.norm(internal_diff)
+
+        if internal_diff_size > MAX_LINEAR_DIFF:
+            self.internal_target += internal_diff * (MAX_LINEAR_DIFF / internal_diff_size)
+        else:
+            self.internal_target[:] = rel_target
+
+        # ---------------------------------------------------------------- compute internal rotation target
+
+        internal_ori_diff = np.clip(M.normalize_deg(self.walk_rel_orientation - self.internal_rel_orientation),
+                                    -MAX_ROTATION_DIFF, MAX_ROTATION_DIFF)
+        self.internal_rel_orientation = np.clip(M.normalize_deg(self.internal_rel_orientation + internal_ori_diff),
+                                                -MAX_ROTATION_DIST, MAX_ROTATION_DIST)
+
+        # ----------------------------------------------------------------- observations
+
+        internal_target_vel = self.internal_target - previous_internal_target
+
+        self.obs[58] = self.internal_target[0] / MAX_LINEAR_DIST
+        self.obs[59] = self.internal_target[1] / MAX_LINEAR_DIST
+        self.obs[60] = self.internal_rel_orientation / MAX_ROTATION_DIST
+        self.obs[61] = internal_target_vel[0] / MAX_LINEAR_DIFF
+        self.obs[62] = internal_target_vel[0] / MAX_LINEAR_DIFF
+
+        return self.obs
+
+    def execute_ik(self, l_pos, l_rot, r_pos, r_rot):
+        r = self.world.robot
+        # Apply IK to each leg + Set joint targets
+
+        # Left leg
+        indices, self.values_l, error_codes = self.ik.leg(l_pos, l_rot, True, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_l, harmonize=False)
+
+        # Right leg
+        indices, self.values_r, error_codes = self.ik.leg(r_pos, r_rot, False, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_r, harmonize=False)
+
+    def execute(self, action):
+
+        r = self.world.robot
+        # exponential moving average
+        self.act = 0.5 * self.act + 0.5 * action
+
+        # execute Step behavior to extract the target positions of each leg (we will override these targets)
+        lfy, lfz, rfy, rfz = self.step_generator.get_target_positions(self.step_counter == 0, self.STEP_DUR,
+                                                                      self.STEP_Z_SPAN,
+                                                                      self.leg_length * self.STEP_Z_MAX)
+
+        # Leg IK
+        a = self.act
+        l_ankle_pos = (a[0] * 0.025 - 0.01, a[1] * 0.01 + lfy, a[2] * 0.01 + lfz)
+        r_ankle_pos = (a[3] * 0.025 - 0.01, a[4] * 0.01 + rfy, a[5] * 0.01 + rfz)
+        l_foot_rot = a[6:9] * (2, 2, 3)
+        r_foot_rot = a[9:12] * (2, 2, 3)
+
+        # Limit leg yaw/pitch (and add bias)
+        l_foot_rot[2] = max(0, l_foot_rot[2] + 18.3)
+        r_foot_rot[2] = min(0, r_foot_rot[2] - 18.3)
+
+        # Arms actions
+        arms = np.copy(self.DEFAULT_ARMS)  # default arms pose
+        arms[0:4] += a[12:16] * 4  # arms pitch+roll
+
+        # Set target positions
+        self.execute_ik(l_ankle_pos, l_foot_rot, r_ankle_pos, r_foot_rot)  # legs
+        r.set_joints_target_position_direct(slice(14, 22), arms, harmonize=False)  # arms
+
+        self.step_counter += 1
+        return self.step_counter > 33

behaviors/custom/Stop/Stop.py

@@ -0,0 +1,69 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Stop.Env import Env
+from math_ops.Math_Ops import Math_Ops as M
+from math_ops.Neural_Network import run_mlp
+import numpy as np
+import pickle
+
+
+class Stop:
+
+    def __init__(self, base_agent: Base_Agent) -> None:
+        self.last_executed = 0
+        self.behavior = base_agent.behavior
+        self.path_manager = base_agent.path_manager
+        self.world = base_agent.world
+        self.description = "RL dribble"
+        self.auto_head = True
+        self.env = Env(base_agent, 0.9 if self.world.robot.type == 3 else 1.2)
+
+        with open(M.get_active_directory([
+                                             "/behaviors/custom/Stop/best_model.zip.pkl",
+                                             "/behaviors/custom/Stop/best_model.zip.pkl",
+                                             "/behaviors/custom/Stop/best_model.zip.pkl",
+                                             "/behaviors/custom/Stop/best_model.zip.pkl",
+                                             "/behaviors/custom/Stop/best_model.zip.pkl"
+                                         ][self.world.robot.type]), 'rb') as f:
+            self.model = pickle.load(f)
+
+    def execute(self, reset):
+        '''
+        Parameters
+        ----------
+        target_2d : array_like
+            2D target in absolute or relative coordinates (use is_target_absolute to specify)
+        is_target_absolute : bool
+            True if target_2d is in absolute coordinates, False if relative to robot's torso
+        orientation : float
+            absolute or relative orientation of torso, in degrees
+            set to None to go towards the target (is_orientation_absolute is ignored)
+        is_orientation_absolute : bool
+            True if orientation is relative to the field, False if relative to the robot's torso
+        distance : float
+            distance to final target [0,0.5] (influences walk speed when approaching the final target)
+            set to None to consider target_2d the final target
+        '''
+        r = self.world.robot
+
+        # ------------------------ 0. Override reset (since some behaviors use this as a sub-behavior)
+        if reset and self.world.time_local_ms - self.last_executed == 20:
+            reset = False
+        self.last_executed = self.world.time_local_ms
+
+        # ------------------------ 1. Define walk parameters
+
+        self.env.walk_rel_target = (0, 0)
+        self.env.walk_distance = 0  # MAX_LINEAR_DIST = 0.5
+        self.env.walk_rel_orientation = 0
+
+        # ------------------------ 2. Execute behavior
+
+        obs = self.env.observe(reset)
+        action = run_mlp(obs, self.model)
+        if self.env.execute(action):
+            return True
+        return False
+
+    def is_ready(self):
+        ''' Returns True if this behavior is ready to start/continue under current game/robot conditions '''
+        return True

behaviors/custom/Walk/Env.py

@@ -0,0 +1,200 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Step.Step_Generator import Step_Generator
+from math_ops.Math_Ops import Math_Ops as M
+import math
+import numpy as np
+
+
+class Env():
+    def __init__(self, base_agent : Base_Agent) -> None:
+
+        self.world = base_agent.world
+        self.ik = base_agent.inv_kinematics
+        
+        # State space  
+        self.obs = np.zeros(63, np.float32)
+        
+        # Step behavior defaults
+        self.STEP_DUR = 8
+        self.STEP_Z_SPAN = 0.02
+        self.STEP_Z_MAX = 0.70
+
+        # IK 
+        nao_specs = self.ik.NAO_SPECS
+        self.leg_length = nao_specs[1] + nao_specs[3] # upper leg height + lower leg height
+        feet_y_dev = nao_specs[0] * 1.12 # wider step
+        sample_time = self.world.robot.STEPTIME
+        max_ankle_z = nao_specs[5]
+
+        self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z)
+        self.DEFAULT_ARMS = np.array([-90,-90,8,8,90,90,70,70],np.float32)
+
+        self.walk_rel_orientation = None
+        self.walk_rel_target = None
+        self.walk_distance = None
+
+
+    def observe(self, init=False):
+
+        r = self.world.robot
+
+        if init: # reset variables
+            self.act = np.zeros(16, np.float32) # memory variable
+            self.step_counter = 0
+
+        # index       observation              naive normalization
+        self.obs[0] = min(self.step_counter,15*8) /100  # simple counter: 0,1,2,3...
+        self.obs[1] = r.loc_head_z                *3   # z coordinate (torso)
+        self.obs[2] = r.loc_head_z_vel            /2   # z velocity (torso)  
+        self.obs[3] = r.imu_torso_roll            /15   # absolute torso roll  in deg
+        self.obs[4] = r.imu_torso_pitch           /15   # absolute torso pitch in deg
+        self.obs[5:8] = r.gyro                    /100  # gyroscope
+        self.obs[8:11] = r.acc                    /10   # accelerometer
+
+        self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10,10,10,0.01,0.01,0.01) #  left foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10,10,10,0.01,0.01,0.01) # right foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)*
+        # *if foot is not touching the ground, then (px=0,py=0,pz=0,fx=0,fy=0,fz=0)
+
+        # Joints: Forward kinematics for ankles + feet rotation + arms (pitch + roll)
+        rel_lankle = self.ik.get_body_part_pos_relative_to_hip("lankle") # ankle position relative to center of both hip joints
+        rel_rankle = self.ik.get_body_part_pos_relative_to_hip("rankle") # ankle position relative to center of both hip joints
+        lf = r.head_to_body_part_transform("torso", r.body_parts['lfoot'].transform ) # foot transform relative to torso
+        rf = r.head_to_body_part_transform("torso", r.body_parts['rfoot'].transform ) # foot transform relative to torso
+        lf_rot_rel_torso = np.array( [lf.get_roll_deg(), lf.get_pitch_deg(), lf.get_yaw_deg()] ) # foot rotation relative to torso
+        rf_rot_rel_torso = np.array( [rf.get_roll_deg(), rf.get_pitch_deg(), rf.get_yaw_deg()] ) # foot rotation relative to torso
+
+        # pose
+        self.obs[23:26] = rel_lankle * (8,8,5)
+        self.obs[26:29] = rel_rankle * (8,8,5)
+        self.obs[29:32] = lf_rot_rel_torso / 20
+        self.obs[32:35] = rf_rot_rel_torso / 20
+        self.obs[35:39] = r.joints_position[14:18] /100 # arms (pitch + roll)
+
+        # velocity
+        self.obs[39:55] = r.joints_target_last_speed[2:18] # predictions == last action
+
+        '''
+        Expected observations for walking state:
+        Time step        R  0   1   2   3   4   5   6   7   0
+        Progress         1  0 .14 .28 .43 .57 .71 .86   1   0
+        Left leg active  T  F   F   F   F   F   F   F   F   T
+        '''
+
+        if init: # the walking parameters refer to the last parameters in effect (after a reset, they are pointless)
+            self.obs[55] = 1 # step progress
+            self.obs[56] = 1 # 1 if left  leg is active
+            self.obs[57] = 0 # 1 if right leg is active
+        else:
+            self.obs[55] = self.step_generator.external_progress # step progress
+            self.obs[56] = float(self.step_generator.state_is_left_active)     # 1 if left  leg is active
+            self.obs[57] = float(not self.step_generator.state_is_left_active) # 1 if right leg is active
+
+        '''
+        Create internal target with a smoother variation
+        '''
+
+        MAX_LINEAR_DIST = 0.5
+        MAX_LINEAR_DIFF = 0.014 # max difference (meters) per step
+        MAX_ROTATION_DIFF = 1.6 # max difference (degrees) per step
+        MAX_ROTATION_DIST = 45
+
+
+        if init:      
+            self.internal_rel_orientation = 0
+            self.internal_target = np.zeros(2)
+
+        previous_internal_target = np.copy(self.internal_target)
+       
+        #---------------------------------------------------------------- compute internal linear target
+        
+        rel_raw_target_size = np.linalg.norm(self.walk_rel_target)
+
+        if rel_raw_target_size == 0:
+            rel_target = self.walk_rel_target
+        else:
+            rel_target = self.walk_rel_target / rel_raw_target_size * min(self.walk_distance, MAX_LINEAR_DIST)
+       
+        internal_diff = rel_target - self.internal_target
+        internal_diff_size = np.linalg.norm(internal_diff)
+
+        if internal_diff_size > MAX_LINEAR_DIFF:
+            self.internal_target += internal_diff * (MAX_LINEAR_DIFF / internal_diff_size)
+        else:
+            self.internal_target[:] = rel_target
+
+        #---------------------------------------------------------------- compute internal rotation target
+
+        internal_ori_diff =  np.clip( M.normalize_deg( self.walk_rel_orientation - self.internal_rel_orientation ), -MAX_ROTATION_DIFF, MAX_ROTATION_DIFF)
+        self.internal_rel_orientation = np.clip(M.normalize_deg( self.internal_rel_orientation + internal_ori_diff ), -MAX_ROTATION_DIST, MAX_ROTATION_DIST)
+
+        #----------------------------------------------------------------- observations
+        
+        internal_target_vel = self.internal_target - previous_internal_target
+
+        self.obs[58] = self.internal_target[0] / MAX_LINEAR_DIST
+        self.obs[59] = self.internal_target[1] / MAX_LINEAR_DIST
+        self.obs[60] = self.internal_rel_orientation / MAX_ROTATION_DIST
+        self.obs[61] = internal_target_vel[0] / MAX_LINEAR_DIFF
+        self.obs[62] = internal_target_vel[0] / MAX_LINEAR_DIFF
+
+        return self.obs
+
+
+    def execute_ik(self, l_pos, l_rot, r_pos, r_rot):
+        r = self.world.robot
+        # Apply IK to each leg + Set joint targets
+          
+        # Left leg 
+        indices, self.values_l, error_codes = self.ik.leg(l_pos, l_rot, True, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_l, harmonize=False)
+
+        # Right leg
+        indices, self.values_r, error_codes = self.ik.leg(r_pos, r_rot, False, dynamic_pose=False)
+
+        r.set_joints_target_position_direct(indices, self.values_r, harmonize=False)
+
+
+    def execute(self, action):
+        
+        r = self.world.robot
+
+        # Actions:
+        # 0,1,2    left ankle pos
+        # 3,4,5    right ankle pos
+        # 6,7,8    left foot rotation
+        # 9,10,11  right foot rotation
+        # 12,13    left/right arm pitch
+        # 14,15    left/right arm roll
+
+        internal_dist = np.linalg.norm( self.internal_target )
+        action_mult = 1 if internal_dist > 0.2 else (0.7/0.2) * internal_dist + 0.3
+        # exponential moving average
+        self.act = 0.8 * self.act + 0.2 * action * action_mult * 0.7
+        
+        # execute Step behavior to extract the target positions of each leg (we will override these targets)
+        lfy,lfz,rfy,rfz = self.step_generator.get_target_positions(self.step_counter == 0, self.STEP_DUR, self.STEP_Z_SPAN, self.leg_length * self.STEP_Z_MAX)
+
+
+        # Leg IK
+        a = self.act
+        l_ankle_pos = (a[0]*0.02, max(0.01,  a[1]*0.02 + lfy), a[2]*0.01 + lfz) # limit y to avoid self collision
+        r_ankle_pos = (a[3]*0.02, min(a[4]*0.02 + rfy, -0.01), a[5]*0.01 + rfz) # limit y to avoid self collision
+        l_foot_rot = a[6:9]  * (3,3,5)
+        r_foot_rot = a[9:12] * (3,3,5)
+
+        # Limit leg yaw/pitch
+        l_foot_rot[2] = max(0,l_foot_rot[2] + 7)
+        r_foot_rot[2] = min(0,r_foot_rot[2] - 7)
+
+        # Arms actions
+        arms = np.copy(self.DEFAULT_ARMS) # default arms pose
+        arm_swing = math.sin(self.step_generator.state_current_ts / self.STEP_DUR * math.pi) * 6
+        inv = 1 if self.step_generator.state_is_left_active else -1
+        arms[0:4] += a[12:16]*4 + (-arm_swing*inv,arm_swing*inv,0,0) # arms pitch+roll
+
+        # Set target positions
+        self.execute_ik(l_ankle_pos, l_foot_rot, r_ankle_pos, r_foot_rot)           # legs 
+        r.set_joints_target_position_direct( slice(14,22), arms, harmonize=False )  # arms
+
+        self.step_counter += 1

behaviors/custom/Walk/Walk.py

@@ -0,0 +1,83 @@
+from agent.Base_Agent import Base_Agent
+from behaviors.custom.Walk.Env import Env
+from math_ops.Math_Ops import Math_Ops as M
+from math_ops.Neural_Network import run_mlp
+import numpy as np
+import pickle
+
+class Walk():
+
+    def __init__(self, base_agent : Base_Agent) -> None:
+        self.world = base_agent.world
+        self.description = "Omnidirectional RL walk"
+        self.auto_head = True
+        self.env = Env(base_agent)
+        self.last_executed = 0
+
+        with open(M.get_active_directory([
+            "/behaviors/custom/Walk/walk_R0.pkl",
+            "/behaviors/custom/Walk/walk_R1_R3.pkl",
+            "/behaviors/custom/Walk/walk_R2.pkl",
+            "/behaviors/custom/Walk/walk_R1_R3.pkl",
+            "/behaviors/custom/Walk/walk_R4.pkl"
+            ][self.world.robot.type]), 'rb') as f:
+            self.model = pickle.load(f)
+
+
+    def execute(self, reset, target_2d, is_target_absolute, orientation, is_orientation_absolute, distance):
+        '''
+        Parameters
+        ----------
+        target_2d : array_like
+            2D target in absolute or relative coordinates (use is_target_absolute to specify)
+        is_target_absolute : bool
+            True if target_2d is in absolute coordinates, False if relative to robot's torso
+        orientation : float
+            absolute or relative orientation of torso, in degrees
+            set to None to go towards the target (is_orientation_absolute is ignored)
+        is_orientation_absolute : bool
+            True if orientation is relative to the field, False if relative to the robot's torso
+        distance : float
+            distance to final target [0,0.5] (influences walk speed when approaching the final target)
+            set to None to consider target_2d the final target
+        '''
+        r = self.world.robot
+
+        #------------------------ 0. Override reset (since some behaviors use this as a sub-behavior)
+        if reset and self.world.time_local_ms - self.last_executed == 20:
+            reset = False
+        self.last_executed = self.world.time_local_ms
+
+        #------------------------ 1. Define walk parameters 
+
+        if is_target_absolute: # convert to target relative to (head position + torso orientation)
+            raw_target = target_2d - r.loc_head_position[:2]
+            self.env.walk_rel_target = M.rotate_2d_vec(raw_target, -r.imu_torso_orientation)
+        else:
+            self.env.walk_rel_target = target_2d
+
+        if distance is None:
+            self.env.walk_distance = np.linalg.norm(self.env.walk_rel_target)
+        else:
+            self.env.walk_distance = distance # MAX_LINEAR_DIST = 0.5
+
+        # Relative orientation values are decreased to avoid overshoot
+        if orientation is None:
+            self.env.walk_rel_orientation = M.vector_angle(self.env.walk_rel_target) * 0.3
+        elif is_orientation_absolute:
+            self.env.walk_rel_orientation = M.normalize_deg( orientation - r.imu_torso_orientation )
+        else:
+            self.env.walk_rel_orientation = orientation * 0.3
+
+        #------------------------ 2. Execute behavior
+
+        obs = self.env.observe(reset)
+        action = run_mlp(obs, self.model)
+        self.env.execute(action)
+
+        return False
+
+
+    def is_ready(self):
+        ''' Returns True if Walk Behavior is ready to start under current game/robot conditions '''
+        return True

behaviors/slot/common/Dive_Left.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Dive left (goalkeeper)" auto_head="1">
+    <slot delta="0.28">
+        <move id="2" angle="0"/>
+        <move id="3" angle="0"/>
+        <move id="4" angle="-25"/>
+        <move id="5" angle="45"/>
+        <move id="6" angle="60"/>
+        <move id="7" angle="0"/>
+        <move id="8" angle="-120"/>
+        <move id="9" angle="0"/>
+        <move id="10" angle="60"/>
+        <move id="11" angle="0"/>
+        <move id="12" angle="25"/>
+        <move id="13" angle="-45"/>
+        <move id="14" angle="90"/>
+        <move id="15" angle="90"/>
+        <move id="16" angle="0"/>
+        <move id="17" angle="0"/>
+        <move id="18" angle="90"/>
+        <move id="19" angle="90"/>
+        <move id="20" angle="0"/>
+        <move id="21" angle="0"/>
+    </slot>
+    <slot delta="0.18">
+        <move id="2" angle="0"/>
+        <move id="3" angle="0"/>
+        <move id="4" angle="-25"/>
+        <move id="5" angle="45"/>
+        <move id="6" angle="60"/>
+        <move id="7" angle="0"/>
+        <move id="8" angle="-120"/>
+        <move id="9" angle="0"/>
+        <move id="10" angle="60"/>
+        <move id="11" angle="0"/>
+        <move id="12" angle="25"/>
+        <move id="13" angle="-45"/>
+        <move id="14" angle="90"/>
+        <move id="15" angle="90"/>
+        <move id="16" angle="0"/>
+        <move id="17" angle="0"/>
+        <move id="18" angle="90"/>
+        <move id="19" angle="90"/>
+        <move id="20" angle="0"/>
+        <move id="21" angle="0"/>
+    </slot>
+    <slot delta="0.32">
+        <move id="2" angle="0"/>
+        <move id="3" angle="0"/>
+        <move id="4" angle="0"/>
+        <move id="5" angle="0"/>
+        <move id="6" angle="0"/>
+        <move id="7" angle="0"/>
+        <move id="8" angle="0"/>
+        <move id="9" angle="0"/>
+        <move id="10" angle="0"/>
+        <move id="11" angle="0"/>
+        <move id="12" angle="0"/>
+        <move id="13" angle="0"/>
+        <move id="14" angle="90"/>
+        <move id="15" angle="90"/>
+        <move id="16" angle="0"/>
+        <move id="17" angle="0"/>
+        <move id="18" angle="90"/>
+        <move id="19" angle="90"/>
+        <move id="20" angle="0"/>
+        <move id="21" angle="0"/>
+    </slot>
+</behavior>
+

behaviors/slot/common/Dive_Right.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Dive right (goalkeeper)" auto_head="1">
+    <slot delta="0.28">
+        <move id="2" angle="0"/>
+        <move id="3" angle="0"/>
+        <move id="4" angle="45"/>
+        <move id="5" angle="-25"/>
+        <move id="6" angle="0"/>
+        <move id="7" angle="60"/>
+        <move id="8" angle="0"/>
+        <move id="9" angle="-120"/>
+        <move id="10" angle="0"/>
+        <move id="11" angle="60"/>
+        <move id="12" angle="-45"/>
+        <move id="13" angle="25"/>
+        <move id="14" angle="90"/>
+        <move id="15" angle="90"/>
+        <move id="16" angle="0"/>
+        <move id="17" angle="0"/>
+        <move id="18" angle="90"/>
+        <move id="19" angle="90"/>
+        <move id="20" angle="0"/>
+        <move id="21" angle="0"/>
+    </slot>
+    <slot delta="0.18">
+        <move id="2" angle="0"/>
+        <move id="3" angle="0"/>
+        <move id="4" angle="45"/>
+        <move id="5" angle="-25"/>
+        <move id="6" angle="0"/>
+        <move id="7" angle="60"/>
+        <move id="8" angle="0"/>
+        <move id="9" angle="-120"/>
+        <move id="10" angle="0"/>
+        <move id="11" angle="60"/>
+        <move id="12" angle="-45"/>
+        <move id="13" angle="25"/>
+        <move id="14" angle="90"/>
+        <move id="15" angle="90"/>
+        <move id="16" angle="0"/>
+        <move id="17" angle="0"/>
+        <move id="18" angle="90"/>
+        <move id="19" angle="90"/>
+        <move id="20" angle="0"/>
+        <move id="21" angle="0"/>
+    </slot>
+    <slot delta="0.32">
+        <move id="2" angle="0"/>
+        <move id="3" angle="0"/>
+        <move id="4" angle="0"/>
+        <move id="5" angle="0"/>
+        <move id="6" angle="0"/>
+        <move id="7" angle="0"/>
+        <move id="8" angle="0"/>
+        <move id="9" angle="0"/>
+        <move id="10" angle="0"/>
+        <move id="11" angle="0"/>
+        <move id="12" angle="0"/>
+        <move id="13" angle="0"/>
+        <move id="14" angle="90"/>
+        <move id="15" angle="90"/>
+        <move id="16" angle="0"/>
+        <move id="17" angle="0"/>
+        <move id="18" angle="90"/>
+        <move id="19" angle="90"/>
+        <move id="20" angle="0"/>
+        <move id="21" angle="0"/>
+    </slot>
+</behavior>
+

behaviors/slot/common/Flip.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Flip from sides before getting up" auto_head="0">
+    <slot delta="0.30">
+        <move id="14" angle="-120"/>
+        <move id="15" angle="-120"/>
+    </slot>
+    <slot delta="0.30">
+        <move id="16" angle="80"/>
+        <move id="17" angle="80"/>
+    </slot>
+</behavior>

behaviors/slot/common/Squat.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Squat for demonstrations" auto_head="0">
+    <slot delta="1.0">    <!-- GO DOWN -->
+        <move id="6" angle="40"/>  <!-- Left  leg pitch -->
+        <move id="7" angle="40"/>  <!-- Right leg pitch -->
+        <move id="8" angle="-80"/> <!-- Left  knee -->
+        <move id="9" angle="-80"/> <!-- Right knee -->
+        <move id="10" angle="40"/> <!-- Left  foot pitch -->
+        <move id="11" angle="40"/> <!-- Right foot pitch -->
+        <move id="16" angle="30"/> <!-- Left  arm roll -->
+        <move id="17" angle="30"/> <!-- Right arm roll -->
+    </slot>
+    <slot delta="1.0">    <!-- GO UP -->
+        <move id="6" angle="0"/>   <!-- Left  leg pitch -->
+        <move id="7" angle="0"/>   <!-- Right leg pitch -->
+        <move id="8" angle="0"/>   <!-- Left  knee -->
+        <move id="9" angle="0"/>   <!-- Right knee -->
+        <move id="10" angle="0"/>  <!-- Left  foot pitch -->
+        <move id="11" angle="0"/>  <!-- Right foot pitch -->
+        <move id="16" angle="0"/>  <!-- Left  arm roll -->
+        <move id="17" angle="0"/>  <!-- Right arm roll -->
+    </slot>
+</behavior>

behaviors/slot/r0/Get_Up_Back.xml

@@ -0,0 +1,148 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-35.454876"/>
+        <move id="3" angle="-35.454876"/>
+        <move id="4" angle="9.4003105"/>
+        <move id="5" angle="9.4003105"/>
+        <move id="6" angle="106.73567"/>
+        <move id="7" angle="106.73567"/>
+        <move id="8" angle="23.08675"/>
+        <move id="9" angle="23.08675"/>
+        <move id="10" angle="-19.941034"/>
+        <move id="11" angle="-19.941034"/>
+        <move id="12" angle="8.216015"/>
+        <move id="13" angle="8.216015"/>
+        <move id="14" angle="57.021896"/>
+        <move id="15" angle="57.021896"/>
+        <move id="16" angle="-64.761406"/>
+        <move id="17" angle="-64.761406"/>
+        <move id="18" angle="-120.64462"/>
+        <move id="19" angle="-120.64462"/>
+        <move id="20" angle="85.76495"/>
+        <move id="21" angle="85.76495"/>
+    </slot>
+    <slot delta="0.23555405990408101">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-62.22426"/>
+        <move id="3" angle="-62.22426"/>
+        <move id="4" angle="-11.9137745"/>
+        <move id="5" angle="-11.9137745"/>
+        <move id="6" angle="24.511059"/>
+        <move id="7" angle="24.511059"/>
+        <move id="8" angle="14.4840355"/>
+        <move id="9" angle="14.4840355"/>
+        <move id="10" angle="53.158936"/>
+        <move id="11" angle="53.158936"/>
+        <move id="12" angle="-14.684006"/>
+        <move id="13" angle="-14.684006"/>
+        <move id="14" angle="-7.403747"/>
+        <move id="15" angle="-7.403747"/>
+        <move id="16" angle="60.993023"/>
+        <move id="17" angle="60.993023"/>
+        <move id="18" angle="106.169014"/>
+        <move id="19" angle="106.169014"/>
+        <move id="20" angle="47.9693"/>
+        <move id="21" angle="47.9693"/>
+    </slot>
+    <slot delta="0.32551713739126387">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-47.479836"/>
+        <move id="3" angle="-47.479836"/>
+        <move id="4" angle="24.034796"/>
+        <move id="5" angle="24.034796"/>
+        <move id="6" angle="143.53075"/>
+        <move id="7" angle="143.53075"/>
+        <move id="8" angle="-44.24652"/>
+        <move id="9" angle="-44.24652"/>
+        <move id="10" angle="-65.46661"/>
+        <move id="11" angle="-65.46661"/>
+        <move id="12" angle="-34.403675"/>
+        <move id="13" angle="-34.403675"/>
+        <move id="14" angle="63.59447"/>
+        <move id="15" angle="63.59447"/>
+        <move id="16" angle="-49.916084"/>
+        <move id="17" angle="-49.916084"/>
+        <move id="18" angle="-98.035194"/>
+        <move id="19" angle="-98.035194"/>
+        <move id="20" angle="-17.168068"/>
+        <move id="21" angle="-17.168068"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-66.561325"/>
+        <move id="3" angle="-66.561325"/>
+        <move id="4" angle="37.05487"/>
+        <move id="5" angle="37.05487"/>
+        <move id="6" angle="29.69468"/>
+        <move id="7" angle="29.69468"/>
+        <move id="8" angle="-54.39078"/>
+        <move id="9" angle="-54.39078"/>
+        <move id="10" angle="-60.952106"/>
+        <move id="11" angle="-60.952106"/>
+        <move id="12" angle="-29.30997"/>
+        <move id="13" angle="-29.30997"/>
+        <move id="14" angle="46.348843"/>
+        <move id="15" angle="46.348843"/>
+        <move id="16" angle="127.48256"/>
+        <move id="17" angle="127.48256"/>
+        <move id="18" angle="126.34747"/>
+        <move id="19" angle="126.34747"/>
+        <move id="20" angle="32.387558"/>
+        <move id="21" angle="32.387558"/>
+    </slot>
+    <slot delta="0.04259131836792499">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-20.94111"/>
+        <move id="3" angle="-20.94111"/>
+        <move id="4" angle="45.91111"/>
+        <move id="5" angle="45.91111"/>
+        <move id="6" angle="112.662"/>
+        <move id="7" angle="112.662"/>
+        <move id="8" angle="-134.07819"/>
+        <move id="9" angle="-134.07819"/>
+        <move id="10" angle="39.1108"/>
+        <move id="11" angle="39.1108"/>
+        <move id="12" angle="-41.35012"/>
+        <move id="13" angle="-41.35012"/>
+        <move id="14" angle="-27.910278"/>
+        <move id="15" angle="-27.910278"/>
+        <move id="16" angle="16.615406"/>
+        <move id="17" angle="16.615406"/>
+        <move id="18" angle="-107.39476"/>
+        <move id="19" angle="-107.39476"/>
+        <move id="20" angle="75.266754"/>
+        <move id="21" angle="75.266754"/>
+    </slot>
+    <slot delta="0.14964306950470477">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="2.0762901"/>
+        <move id="3" angle="2.0762901"/>
+        <move id="4" angle="53.06344"/>
+        <move id="5" angle="53.06344"/>
+        <move id="6" angle="138.88753"/>
+        <move id="7" angle="138.88753"/>
+        <move id="8" angle="-131.63484"/>
+        <move id="9" angle="-131.63484"/>
+        <move id="10" angle="31.228016"/>
+        <move id="11" angle="31.228016"/>
+        <move id="12" angle="-13.720424"/>
+        <move id="13" angle="-13.720424"/>
+        <move id="14" angle="43.981934"/>
+        <move id="15" angle="43.981934"/>
+        <move id="16" angle="-0.4473343"/>
+        <move id="17" angle="-0.4473343"/>
+        <move id="18" angle="13.684681"/>
+        <move id="19" angle="13.684681"/>
+        <move id="20" angle="-15.960043"/>
+        <move id="21" angle="-15.960043"/>
+    </slot>
+</behavior>

behaviors/slot/r0/Get_Up_Front.xml

@@ -0,0 +1,220 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
+    <slot delta="0.2">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-4.976922"/>
+        <move id="3" angle="-4.976922"/>
+        <move id="4" angle="14.641327"/>
+        <move id="5" angle="14.641327"/>
+        <move id="6" angle="6.7912245"/>
+        <move id="7" angle="6.7912245"/>
+        <move id="8" angle="-0.386917"/>
+        <move id="9" angle="-0.386917"/>
+        <move id="10" angle="0.23039937"/>
+        <move id="11" angle="0.23039937"/>
+        <move id="12" angle="-5.228052"/>
+        <move id="13" angle="-5.228052"/>
+        <move id="14" angle="23.919197"/>
+        <move id="15" angle="23.919197"/>
+        <move id="16" angle="9.847563"/>
+        <move id="17" angle="9.847563"/>
+        <move id="18" angle="-6.9379196"/>
+        <move id="19" angle="-6.9379196"/>
+        <move id="20" angle="-16.700315"/>
+        <move id="21" angle="-16.700315"/>
+    </slot>
+    <slot delta="0.08">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="1.0232245"/>
+        <move id="3" angle="1.0232245"/>
+        <move id="4" angle="-22.36497"/>
+        <move id="5" angle="-22.36497"/>
+        <move id="6" angle="265.17767"/>
+        <move id="7" angle="265.17767"/>
+        <move id="8" angle="-133.80704"/>
+        <move id="9" angle="-133.80704"/>
+        <move id="10" angle="118.27272"/>
+        <move id="11" angle="118.27272"/>
+        <move id="12" angle="-7.289217"/>
+        <move id="13" angle="-7.289217"/>
+        <move id="14" angle="2.7956364"/>
+        <move id="15" angle="2.7956364"/>
+        <move id="16" angle="17.118933"/>
+        <move id="17" angle="17.118933"/>
+        <move id="18" angle="-27.083485"/>
+        <move id="19" angle="-27.083485"/>
+        <move id="20" angle="-8.698996"/>
+        <move id="21" angle="-8.698996"/>
+    </slot>
+    <slot delta="0.08">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-14.808299"/>
+        <move id="3" angle="-14.808299"/>
+        <move id="4" angle="37.39434"/>
+        <move id="5" angle="37.39434"/>
+        <move id="6" angle="7.6876955"/>
+        <move id="7" angle="7.6876955"/>
+        <move id="8" angle="-91.4461"/>
+        <move id="9" angle="-91.4461"/>
+        <move id="10" angle="-0.40869182"/>
+        <move id="11" angle="-0.40869182"/>
+        <move id="12" angle="-24.007051"/>
+        <move id="13" angle="-24.007051"/>
+        <move id="14" angle="8.485344"/>
+        <move id="15" angle="8.485344"/>
+        <move id="16" angle="7.828232"/>
+        <move id="17" angle="7.828232"/>
+        <move id="18" angle="5.688171"/>
+        <move id="19" angle="5.688171"/>
+        <move id="20" angle="-17.660221"/>
+        <move id="21" angle="-17.660221"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-89.41741"/>
+        <move id="3" angle="-89.41741"/>
+        <move id="4" angle="14.217276"/>
+        <move id="5" angle="14.217276"/>
+        <move id="6" angle="-3.3899987"/>
+        <move id="7" angle="-3.3899987"/>
+        <move id="8" angle="-103.90058"/>
+        <move id="9" angle="-103.90058"/>
+        <move id="10" angle="-48.087368"/>
+        <move id="11" angle="-48.087368"/>
+        <move id="12" angle="-21.949076"/>
+        <move id="13" angle="-21.949076"/>
+        <move id="14" angle="-0.5262981"/>
+        <move id="15" angle="-0.5262981"/>
+        <move id="16" angle="-9.913716"/>
+        <move id="17" angle="-9.913716"/>
+        <move id="18" angle="-0.202469"/>
+        <move id="19" angle="-0.202469"/>
+        <move id="20" angle="-12.194956"/>
+        <move id="21" angle="-12.194956"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-0.9640895"/>
+        <move id="3" angle="-0.9640895"/>
+        <move id="4" angle="-4.2894506"/>
+        <move id="5" angle="-4.2894506"/>
+        <move id="6" angle="-1.5769005"/>
+        <move id="7" angle="-1.5769005"/>
+        <move id="8" angle="5.6831093"/>
+        <move id="9" angle="5.6831093"/>
+        <move id="10" angle="4.317226"/>
+        <move id="11" angle="4.317226"/>
+        <move id="12" angle="-3.2686572"/>
+        <move id="13" angle="-3.2686572"/>
+        <move id="14" angle="50.114185"/>
+        <move id="15" angle="50.114185"/>
+        <move id="16" angle="-11.285558"/>
+        <move id="17" angle="-11.285558"/>
+        <move id="18" angle="-6.0098124"/>
+        <move id="19" angle="-6.0098124"/>
+        <move id="20" angle="-3.112287"/>
+        <move id="21" angle="-3.112287"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-60.664845"/>
+        <move id="3" angle="-60.664845"/>
+        <move id="4" angle="15.481741"/>
+        <move id="5" angle="15.481741"/>
+        <move id="6" angle="-1.4592165"/>
+        <move id="7" angle="-1.4592165"/>
+        <move id="8" angle="-121.534065"/>
+        <move id="9" angle="-121.534065"/>
+        <move id="10" angle="44.071327"/>
+        <move id="11" angle="44.071327"/>
+        <move id="12" angle="-10.756018"/>
+        <move id="13" angle="-10.756018"/>
+        <move id="14" angle="-4.711259"/>
+        <move id="15" angle="-4.711259"/>
+        <move id="16" angle="-7.3120046"/>
+        <move id="17" angle="-7.3120046"/>
+        <move id="18" angle="3.982705"/>
+        <move id="19" angle="3.982705"/>
+        <move id="20" angle="-11.348281"/>
+        <move id="21" angle="-11.348281"/>
+    </slot>
+    <slot delta="0.04">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-58.631565"/>
+        <move id="3" angle="-58.631565"/>
+        <move id="4" angle="31.81041"/>
+        <move id="5" angle="31.81041"/>
+        <move id="6" angle="100.52844"/>
+        <move id="7" angle="100.52844"/>
+        <move id="8" angle="-123.12153"/>
+        <move id="9" angle="-123.12153"/>
+        <move id="10" angle="38.84296"/>
+        <move id="11" angle="38.84296"/>
+        <move id="12" angle="-37.32096"/>
+        <move id="13" angle="-37.32096"/>
+        <move id="14" angle="-4.5341907"/>
+        <move id="15" angle="-4.5341907"/>
+        <move id="16" angle="6.23005"/>
+        <move id="17" angle="6.23005"/>
+        <move id="18" angle="12.395767"/>
+        <move id="19" angle="12.395767"/>
+        <move id="20" angle="-4.4955177"/>
+        <move id="21" angle="-4.4955177"/>
+    </slot>
+    <slot delta="0.14">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-62.06845"/>
+        <move id="3" angle="-62.06845"/>
+        <move id="4" angle="69.89282"/>
+        <move id="5" angle="69.89282"/>
+        <move id="6" angle="104.76725"/>
+        <move id="7" angle="104.76725"/>
+        <move id="8" angle="-132.93317"/>
+        <move id="9" angle="-132.93317"/>
+        <move id="10" angle="35.730618"/>
+        <move id="11" angle="35.730618"/>
+        <move id="12" angle="-33.17914"/>
+        <move id="13" angle="-33.17914"/>
+        <move id="14" angle="-19.49921"/>
+        <move id="15" angle="-19.49921"/>
+        <move id="16" angle="0.18734631"/>
+        <move id="17" angle="0.18734631"/>
+        <move id="18" angle="20.604797"/>
+        <move id="19" angle="20.604797"/>
+        <move id="20" angle="8.590274"/>
+        <move id="21" angle="8.590274"/>
+    </slot>
+    <slot delta="0.4">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-0.702901"/>
+        <move id="3" angle="-0.702901"/>
+        <move id="4" angle="7.3007526"/>
+        <move id="5" angle="7.3007526"/>
+        <move id="6" angle="36.762917"/>
+        <move id="7" angle="36.762917"/>
+        <move id="8" angle="-102.52497"/>
+        <move id="9" angle="-102.52497"/>
+        <move id="10" angle="65.24182"/>
+        <move id="11" angle="65.24182"/>
+        <move id="12" angle="7.8809857"/>
+        <move id="13" angle="7.8809857"/>
+        <move id="14" angle="-117.089615"/>
+        <move id="15" angle="-117.089615"/>
+        <move id="16" angle="37.752632"/>
+        <move id="17" angle="37.752632"/>
+        <move id="18" angle="47.93907"/>
+        <move id="19" angle="47.93907"/>
+        <move id="20" angle="-8.804468"/>
+        <move id="21" angle="-8.804468"/>
+    </slot>
+</behavior>

behaviors/slot/r0/Kick_Motion.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Kick motion with right leg" auto_head="1">
+    <slot delta="0.22">    <!-- Lean -->
+        <move id="5" angle="-10"/>  <!-- Left  leg roll -->
+        <move id="6" angle="40"/>   <!-- Left  leg pitch -->
+        <move id="7" angle="65"/>   <!-- Right leg pitch -->
+        <move id="8" angle="-60"/>  <!-- Left  knee -->
+        <move id="9" angle="-115"/> <!-- Right knee -->
+        <move id="10" angle="60"/>  <!-- Left  foot pitch -->
+        <move id="11" angle="10"/>  <!-- Right foot pitch -->
+    </slot>
+    <slot delta="0.12">    <!-- Kick -->
+        <move id="3" angle="-45"/> <!-- Right leg yaw/pitch -->
+        <move id="6" angle="-25"/> <!-- Left  leg pitch -->
+        <move id="7" angle="80"/>  <!-- Right leg pitch -->
+        <move id="8" angle="0"/>   <!-- Left knee -->
+        <move id="9" angle="0"/>   <!-- Right knee -->
+        <move id="10" angle="30"/> <!-- Left  foot pitch -->
+    </slot>
+</behavior>

behaviors/slot/r1/Get_Up_Back.xml

@@ -0,0 +1,148 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-33.468"/>
+        <move id="3" angle="-33.468"/>
+        <move id="4" angle="32.57773"/>
+        <move id="5" angle="32.57773"/>
+        <move id="6" angle="59.169636"/>
+        <move id="7" angle="59.169636"/>
+        <move id="8" angle="9.965677"/>
+        <move id="9" angle="9.965677"/>
+        <move id="10" angle="-2.0611076"/>
+        <move id="11" angle="-2.0611076"/>
+        <move id="12" angle="0.4622302"/>
+        <move id="13" angle="0.4622302"/>
+        <move id="14" angle="59.513363"/>
+        <move id="15" angle="59.513363"/>
+        <move id="16" angle="-65.481674"/>
+        <move id="17" angle="-65.481674"/>
+        <move id="18" angle="-40.493507"/>
+        <move id="19" angle="-40.493507"/>
+        <move id="20" angle="173.76538"/>
+        <move id="21" angle="173.76538"/>
+    </slot>
+    <slot delta="0.24">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-53.970947"/>
+        <move id="3" angle="-53.970947"/>
+        <move id="4" angle="-31.631035"/>
+        <move id="5" angle="-31.631035"/>
+        <move id="6" angle="19.487873"/>
+        <move id="7" angle="19.487873"/>
+        <move id="8" angle="16.130121"/>
+        <move id="9" angle="16.130121"/>
+        <move id="10" angle="48.766438"/>
+        <move id="11" angle="48.766438"/>
+        <move id="12" angle="-43.63519"/>
+        <move id="13" angle="-43.63519"/>
+        <move id="14" angle="8.0750675"/>
+        <move id="15" angle="8.0750675"/>
+        <move id="16" angle="26.09803"/>
+        <move id="17" angle="26.09803"/>
+        <move id="18" angle="76.53693"/>
+        <move id="19" angle="76.53693"/>
+        <move id="20" angle="166.2738"/>
+        <move id="21" angle="166.2738"/>
+    </slot>
+    <slot delta="0.32">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-58.35547"/>
+        <move id="3" angle="-58.35547"/>
+        <move id="4" angle="4.337753"/>
+        <move id="5" angle="4.337753"/>
+        <move id="6" angle="119.925354"/>
+        <move id="7" angle="119.925354"/>
+        <move id="8" angle="-24.74408"/>
+        <move id="9" angle="-24.74408"/>
+        <move id="10" angle="-96.68003"/>
+        <move id="11" angle="-96.68003"/>
+        <move id="12" angle="-38.341896"/>
+        <move id="13" angle="-38.341896"/>
+        <move id="14" angle="61.39873"/>
+        <move id="15" angle="61.39873"/>
+        <move id="16" angle="-69.773056"/>
+        <move id="17" angle="-69.773056"/>
+        <move id="18" angle="-76.832436"/>
+        <move id="19" angle="-76.832436"/>
+        <move id="20" angle="-33.894478"/>
+        <move id="21" angle="-33.894478"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-82.921234"/>
+        <move id="3" angle="-82.921234"/>
+        <move id="4" angle="62.38533"/>
+        <move id="5" angle="62.38533"/>
+        <move id="6" angle="-25.176954"/>
+        <move id="7" angle="-25.176954"/>
+        <move id="8" angle="-48.75859"/>
+        <move id="9" angle="-48.75859"/>
+        <move id="10" angle="-65.601974"/>
+        <move id="11" angle="-65.601974"/>
+        <move id="12" angle="-22.979483"/>
+        <move id="13" angle="-22.979483"/>
+        <move id="14" angle="5.9137707"/>
+        <move id="15" angle="5.9137707"/>
+        <move id="16" angle="21.222649"/>
+        <move id="17" angle="21.222649"/>
+        <move id="18" angle="-84.464584"/>
+        <move id="19" angle="-84.464584"/>
+        <move id="20" angle="38.63583"/>
+        <move id="21" angle="38.63583"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-28.479536"/>
+        <move id="3" angle="-28.479536"/>
+        <move id="4" angle="63.585136"/>
+        <move id="5" angle="63.585136"/>
+        <move id="6" angle="111.310234"/>
+        <move id="7" angle="111.310234"/>
+        <move id="8" angle="-159.30522"/>
+        <move id="9" angle="-159.30522"/>
+        <move id="10" angle="56.21947"/>
+        <move id="11" angle="56.21947"/>
+        <move id="12" angle="-46.503235"/>
+        <move id="13" angle="-46.503235"/>
+        <move id="14" angle="11.373715"/>
+        <move id="15" angle="11.373715"/>
+        <move id="16" angle="-54.145515"/>
+        <move id="17" angle="-54.145515"/>
+        <move id="18" angle="63.853237"/>
+        <move id="19" angle="63.853237"/>
+        <move id="20" angle="-32.70381"/>
+        <move id="21" angle="-32.70381"/>
+    </slot>
+    <slot delta="0.16">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-3.9309306"/>
+        <move id="3" angle="-3.9309306"/>
+        <move id="4" angle="28.753582"/>
+        <move id="5" angle="28.753582"/>
+        <move id="6" angle="73.12465"/>
+        <move id="7" angle="73.12465"/>
+        <move id="8" angle="-133.79837"/>
+        <move id="9" angle="-133.79837"/>
+        <move id="10" angle="37.535084"/>
+        <move id="11" angle="37.535084"/>
+        <move id="12" angle="5.768486"/>
+        <move id="13" angle="5.768486"/>
+        <move id="14" angle="-69.01422"/>
+        <move id="15" angle="-69.01422"/>
+        <move id="16" angle="50.282738"/>
+        <move id="17" angle="50.282738"/>
+        <move id="18" angle="51.603962"/>
+        <move id="19" angle="51.603962"/>
+        <move id="20" angle="80.4043"/>
+        <move id="21" angle="80.4043"/>
+    </slot>
+</behavior>

behaviors/slot/r1/Get_Up_Front.xml

@@ -0,0 +1,220 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
+    <slot delta="0.04">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="0.03765949"/>
+        <move id="3" angle="0.03765949"/>
+        <move id="4" angle="6.7361"/>
+        <move id="5" angle="6.7361"/>
+        <move id="6" angle="7.144582"/>
+        <move id="7" angle="7.144582"/>
+        <move id="8" angle="3.2917228"/>
+        <move id="9" angle="3.2917228"/>
+        <move id="10" angle="0.2270207"/>
+        <move id="11" angle="0.2270207"/>
+        <move id="12" angle="-0.62398136"/>
+        <move id="13" angle="-0.62398136"/>
+        <move id="14" angle="11.943922"/>
+        <move id="15" angle="11.943922"/>
+        <move id="16" angle="36.128757"/>
+        <move id="17" angle="36.128757"/>
+        <move id="18" angle="1.7096568"/>
+        <move id="19" angle="1.7096568"/>
+        <move id="20" angle="-10.422138"/>
+        <move id="21" angle="-10.422138"/>
+    </slot>
+    <slot delta="0.18">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="1.2124634"/>
+        <move id="3" angle="1.2124634"/>
+        <move id="4" angle="-16.6976"/>
+        <move id="5" angle="-16.6976"/>
+        <move id="6" angle="12.002005"/>
+        <move id="7" angle="12.002005"/>
+        <move id="8" angle="-3.8902621"/>
+        <move id="9" angle="-3.8902621"/>
+        <move id="10" angle="2.2115564"/>
+        <move id="11" angle="2.2115564"/>
+        <move id="12" angle="18.24385"/>
+        <move id="13" angle="18.24385"/>
+        <move id="14" angle="17.7995"/>
+        <move id="15" angle="17.7995"/>
+        <move id="16" angle="46.32759"/>
+        <move id="17" angle="46.32759"/>
+        <move id="18" angle="-5.752133"/>
+        <move id="19" angle="-5.752133"/>
+        <move id="20" angle="-23.737434"/>
+        <move id="21" angle="-23.737434"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="0.4612013"/>
+        <move id="3" angle="0.4612013"/>
+        <move id="4" angle="-10.992544"/>
+        <move id="5" angle="-10.992544"/>
+        <move id="6" angle="256.44278"/>
+        <move id="7" angle="256.44278"/>
+        <move id="8" angle="-130.99982"/>
+        <move id="9" angle="-130.99982"/>
+        <move id="10" angle="125.019295"/>
+        <move id="11" angle="125.019295"/>
+        <move id="12" angle="-4.830559"/>
+        <move id="13" angle="-4.830559"/>
+        <move id="14" angle="-3.4743867"/>
+        <move id="15" angle="-3.4743867"/>
+        <move id="16" angle="-17.395947"/>
+        <move id="17" angle="-17.395947"/>
+        <move id="18" angle="-7.197071"/>
+        <move id="19" angle="-7.197071"/>
+        <move id="20" angle="-18.81015"/>
+        <move id="21" angle="-18.81015"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-11.333593"/>
+        <move id="3" angle="-11.333593"/>
+        <move id="4" angle="44.738525"/>
+        <move id="5" angle="44.738525"/>
+        <move id="6" angle="6.1589823"/>
+        <move id="7" angle="6.1589823"/>
+        <move id="8" angle="-91.90975"/>
+        <move id="9" angle="-91.90975"/>
+        <move id="10" angle="3.823912"/>
+        <move id="11" angle="3.823912"/>
+        <move id="12" angle="-38.074776"/>
+        <move id="13" angle="-38.074776"/>
+        <move id="14" angle="15.952488"/>
+        <move id="15" angle="15.952488"/>
+        <move id="16" angle="-7.0655346"/>
+        <move id="17" angle="-7.0655346"/>
+        <move id="18" angle="-22.311245"/>
+        <move id="19" angle="-22.311245"/>
+        <move id="20" angle="-18.258038"/>
+        <move id="21" angle="-18.258038"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-89.49788"/>
+        <move id="3" angle="-89.49788"/>
+        <move id="4" angle="-3.2911317"/>
+        <move id="5" angle="-3.2911317"/>
+        <move id="6" angle="-2.9935064"/>
+        <move id="7" angle="-2.9935064"/>
+        <move id="8" angle="-97.67326"/>
+        <move id="9" angle="-97.67326"/>
+        <move id="10" angle="-45.384357"/>
+        <move id="11" angle="-45.384357"/>
+        <move id="12" angle="-23.928814"/>
+        <move id="13" angle="-23.928814"/>
+        <move id="14" angle="-8.738391"/>
+        <move id="15" angle="-8.738391"/>
+        <move id="16" angle="-5.594632"/>
+        <move id="17" angle="-5.594632"/>
+        <move id="18" angle="12.174933"/>
+        <move id="19" angle="12.174933"/>
+        <move id="20" angle="9.707994"/>
+        <move id="21" angle="9.707994"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-3.2422483"/>
+        <move id="3" angle="-3.2422483"/>
+        <move id="4" angle="10.094491"/>
+        <move id="5" angle="10.094491"/>
+        <move id="6" angle="0.105650306"/>
+        <move id="7" angle="0.105650306"/>
+        <move id="8" angle="3.6602314"/>
+        <move id="9" angle="3.6602314"/>
+        <move id="10" angle="4.2006454"/>
+        <move id="11" angle="4.2006454"/>
+        <move id="12" angle="12.860165"/>
+        <move id="13" angle="12.860165"/>
+        <move id="14" angle="41.729637"/>
+        <move id="15" angle="41.729637"/>
+        <move id="16" angle="4.5020647"/>
+        <move id="17" angle="4.5020647"/>
+        <move id="18" angle="0.37317705"/>
+        <move id="19" angle="0.37317705"/>
+        <move id="20" angle="-17.251738"/>
+        <move id="21" angle="-17.251738"/>
+    </slot>
+    <slot delta="0.04">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-54.546894"/>
+        <move id="3" angle="-54.546894"/>
+        <move id="4" angle="47.746372"/>
+        <move id="5" angle="47.746372"/>
+        <move id="6" angle="102.24254"/>
+        <move id="7" angle="102.24254"/>
+        <move id="8" angle="-124.605934"/>
+        <move id="9" angle="-124.605934"/>
+        <move id="10" angle="48.503883"/>
+        <move id="11" angle="48.503883"/>
+        <move id="12" angle="-61.207714"/>
+        <move id="13" angle="-61.207714"/>
+        <move id="14" angle="3.2614079"/>
+        <move id="15" angle="3.2614079"/>
+        <move id="16" angle="14.854502"/>
+        <move id="17" angle="14.854502"/>
+        <move id="18" angle="-23.213947"/>
+        <move id="19" angle="-23.213947"/>
+        <move id="20" angle="6.2566423"/>
+        <move id="21" angle="6.2566423"/>
+    </slot>
+    <slot delta="0.14">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-49.910076"/>
+        <move id="3" angle="-49.910076"/>
+        <move id="4" angle="41.691715"/>
+        <move id="5" angle="41.691715"/>
+        <move id="6" angle="115.67641"/>
+        <move id="7" angle="115.67641"/>
+        <move id="8" angle="-130.50008"/>
+        <move id="9" angle="-130.50008"/>
+        <move id="10" angle="45.15649"/>
+        <move id="11" angle="45.15649"/>
+        <move id="12" angle="-63.601254"/>
+        <move id="13" angle="-63.601254"/>
+        <move id="14" angle="-23.61024"/>
+        <move id="15" angle="-23.61024"/>
+        <move id="16" angle="-22.336332"/>
+        <move id="17" angle="-22.336332"/>
+        <move id="18" angle="9.258426"/>
+        <move id="19" angle="9.258426"/>
+        <move id="20" angle="75"/> <!-- avoid self collision -->
+        <move id="21" angle="75"/> <!-- avoid self collision -->
+    </slot>
+    <slot delta="0.38">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-1.9032228"/>
+        <move id="3" angle="-1.9032228"/>
+        <move id="4" angle="25.761885"/>
+        <move id="5" angle="25.761885"/>
+        <move id="6" angle="37.294437"/>
+        <move id="7" angle="37.294437"/>
+        <move id="8" angle="-92.91941"/>
+        <move id="9" angle="-92.91941"/>
+        <move id="10" angle="54.069553"/>
+        <move id="11" angle="54.069553"/>
+        <move id="12" angle="3.6394153"/>
+        <move id="13" angle="3.6394153"/>
+        <move id="14" angle="-115.9809"/>
+        <move id="15" angle="-115.9809"/>
+        <move id="16" angle="30.516266"/>
+        <move id="17" angle="30.516266"/>
+        <move id="18" angle="67.11856"/>
+        <move id="19" angle="67.11856"/>
+        <move id="20" angle="75"/> <!-- avoid self collision -->
+        <move id="21" angle="75"/> <!-- avoid self collision -->
+    </slot>
+</behavior>

behaviors/slot/r1/Kick_Motion.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Kick motion with right leg" auto_head="1">
+    <slot delta="0.22">    <!-- Lean -->
+        <move id="5" angle="-10"/>  <!-- Left  leg roll -->
+        <move id="6" angle="40"/>   <!-- Left  leg pitch -->
+        <move id="7" angle="-95"/>   <!-- Right leg pitch -->
+        <move id="8" angle="-60"/>  <!-- Left  knee -->
+        <move id="9" angle="-145"/> <!-- Right knee -->
+        <move id="10" angle="60"/>  <!-- Left  foot pitch -->
+        <move id="11" angle="0"/>   <!-- Right foot pitch -->
+    </slot>
+
+
+</behavior>

behaviors/slot/r2/Get_Up_Back.xml

@@ -0,0 +1,148 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
+    <slot delta="0.028081112120805277">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-24.101269"/>
+        <move id="3" angle="-24.101269"/>
+        <move id="4" angle="32.993332"/>
+        <move id="5" angle="32.993332"/>
+        <move id="6" angle="110.35986"/>
+        <move id="7" angle="110.35986"/>
+        <move id="8" angle="1.8788892"/>
+        <move id="9" angle="1.8788892"/>
+        <move id="10" angle="80.01187"/>
+        <move id="11" angle="80.01187"/>
+        <move id="12" angle="9.717135"/>
+        <move id="13" angle="9.717135"/>
+        <move id="14" angle="51.12714"/>
+        <move id="15" angle="51.12714"/>
+        <move id="16" angle="42.79977"/>
+        <move id="17" angle="42.79977"/>
+        <move id="18" angle="-86.264915"/>
+        <move id="19" angle="-86.264915"/>
+        <move id="20" angle="33.84708"/>
+        <move id="21" angle="33.84708"/>
+    </slot>
+    <slot delta="0.23351845363901297">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-45.0749"/>
+        <move id="3" angle="-45.0749"/>
+        <move id="4" angle="-14.139231"/>
+        <move id="5" angle="-14.139231"/>
+        <move id="6" angle="45.828407"/>
+        <move id="7" angle="45.828407"/>
+        <move id="8" angle="12.7746315"/>
+        <move id="9" angle="12.7746315"/>
+        <move id="10" angle="39.730194"/>
+        <move id="11" angle="39.730194"/>
+        <move id="12" angle="-41.445404"/>
+        <move id="13" angle="-41.445404"/>
+        <move id="14" angle="-20.96023"/>
+        <move id="15" angle="-20.96023"/>
+        <move id="16" angle="67.418335"/>
+        <move id="17" angle="67.418335"/>
+        <move id="18" angle="136.93271"/>
+        <move id="19" angle="136.93271"/>
+        <move id="20" angle="79.51946"/>
+        <move id="21" angle="79.51946"/>
+    </slot>
+    <slot delta="0.3334680044481105">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-47.639645"/>
+        <move id="3" angle="-47.639645"/>
+        <move id="4" angle="36.16746"/>
+        <move id="5" angle="36.16746"/>
+        <move id="6" angle="106.46471"/>
+        <move id="7" angle="106.46471"/>
+        <move id="8" angle="-39.363796"/>
+        <move id="9" angle="-39.363796"/>
+        <move id="10" angle="-72.647545"/>
+        <move id="11" angle="-72.647545"/>
+        <move id="12" angle="-25.91456"/>
+        <move id="13" angle="-25.91456"/>
+        <move id="14" angle="72.665886"/>
+        <move id="15" angle="72.665886"/>
+        <move id="16" angle="-46.327827"/>
+        <move id="17" angle="-46.327827"/>
+        <move id="18" angle="-66.61269"/>
+        <move id="19" angle="-66.61269"/>
+        <move id="20" angle="-31.427937"/>
+        <move id="21" angle="-31.427937"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-75.75934"/>
+        <move id="3" angle="-75.75934"/>
+        <move id="4" angle="22.589397"/>
+        <move id="5" angle="22.589397"/>
+        <move id="6" angle="29.006308"/>
+        <move id="7" angle="29.006308"/>
+        <move id="8" angle="-38.235386"/>
+        <move id="9" angle="-38.235386"/>
+        <move id="10" angle="-60.07112"/>
+        <move id="11" angle="-60.07112"/>
+        <move id="12" angle="-7.1391"/>
+        <move id="13" angle="-7.1391"/>
+        <move id="14" angle="-4.2825775"/>
+        <move id="15" angle="-4.2825775"/>
+        <move id="16" angle="134.62912"/>
+        <move id="17" angle="134.62912"/>
+        <move id="18" angle="131.15475"/>
+        <move id="19" angle="131.15475"/>
+        <move id="20" angle="73.77145"/>
+        <move id="21" angle="73.77145"/>
+    </slot>
+    <slot delta="0.05570955407520801">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-19.975779"/>
+        <move id="3" angle="-19.975779"/>
+        <move id="4" angle="41.732513"/>
+        <move id="5" angle="41.732513"/>
+        <move id="6" angle="102.82768"/>
+        <move id="7" angle="102.82768"/>
+        <move id="8" angle="-136.81155"/>
+        <move id="9" angle="-136.81155"/>
+        <move id="10" angle="14.452017"/>
+        <move id="11" angle="14.452017"/>
+        <move id="12" angle="-36.594322"/>
+        <move id="13" angle="-36.594322"/>
+        <move id="14" angle="-10.283749"/>
+        <move id="15" angle="-10.283749"/>
+        <move id="16" angle="46.129036"/>
+        <move id="17" angle="46.129036"/>
+        <move id="18" angle="-29.25866"/>
+        <move id="19" angle="-29.25866"/>
+        <move id="20" angle="-28.004427"/>
+        <move id="21" angle="-28.004427"/>
+    </slot>
+    <slot delta="0.15631983125111132">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-1.2392063"/>
+        <move id="3" angle="-1.2392063"/>
+        <move id="4" angle="50.825813"/>
+        <move id="5" angle="50.825813"/>
+        <move id="6" angle="81.91005"/>
+        <move id="7" angle="81.91005"/>
+        <move id="8" angle="-114.5607"/>
+        <move id="9" angle="-114.5607"/>
+        <move id="10" angle="46.413334"/>
+        <move id="11" angle="46.413334"/>
+        <move id="12" angle="-11.515432"/>
+        <move id="13" angle="-11.515432"/>
+        <move id="14" angle="-17.704185"/>
+        <move id="15" angle="-17.704185"/>
+        <move id="16" angle="11.881702"/>
+        <move id="17" angle="11.881702"/>
+        <move id="18" angle="116.02219"/>
+        <move id="19" angle="116.02219"/>
+        <move id="20" angle="85.50837"/>
+        <move id="21" angle="85.50837"/>
+    </slot>
+</behavior>

behaviors/slot/r2/Get_Up_Front.xml

@@ -0,0 +1,220 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
+    <slot delta="0.16">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="4.6313915"/>
+        <move id="3" angle="4.6313915"/>
+        <move id="4" angle="-11.846246"/>
+        <move id="5" angle="-11.846246"/>
+        <move id="6" angle="8.745557"/>
+        <move id="7" angle="8.745557"/>
+        <move id="8" angle="-1.8197118"/>
+        <move id="9" angle="-1.8197118"/>
+        <move id="10" angle="1.359597"/>
+        <move id="11" angle="1.359597"/>
+        <move id="12" angle="16.82137"/>
+        <move id="13" angle="16.82137"/>
+        <move id="14" angle="21.83775"/>
+        <move id="15" angle="21.83775"/>
+        <move id="16" angle="-27.174564"/>
+        <move id="17" angle="-27.174564"/>
+        <move id="18" angle="-12.889872"/>
+        <move id="19" angle="-12.889872"/>
+        <move id="20" angle="-14.989797"/>
+        <move id="21" angle="-14.989797"/>
+    </slot>
+    <slot delta="0.08">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="1.0406766"/>
+        <move id="3" angle="1.0406766"/>
+        <move id="4" angle="-7.939904"/>
+        <move id="5" angle="-7.939904"/>
+        <move id="6" angle="260.59564"/>
+        <move id="7" angle="260.59564"/>
+        <move id="8" angle="-131.98882"/>
+        <move id="9" angle="-131.98882"/>
+        <move id="10" angle="123.69923"/>
+        <move id="11" angle="123.69923"/>
+        <move id="12" angle="-8.521371"/>
+        <move id="13" angle="-8.521371"/>
+        <move id="14" angle="3.173905"/>
+        <move id="15" angle="3.173905"/>
+        <move id="16" angle="-19.583878"/>
+        <move id="17" angle="-19.583878"/>
+        <move id="18" angle="-13.1733055"/>
+        <move id="19" angle="-13.1733055"/>
+        <move id="20" angle="-17.131882"/>
+        <move id="21" angle="-17.131882"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-19.195496"/>
+        <move id="3" angle="-19.195496"/>
+        <move id="4" angle="42.399166"/>
+        <move id="5" angle="42.399166"/>
+        <move id="6" angle="4.278442"/>
+        <move id="7" angle="4.278442"/>
+        <move id="8" angle="-93.31827"/>
+        <move id="9" angle="-93.31827"/>
+        <move id="10" angle="2.5259771"/>
+        <move id="11" angle="2.5259771"/>
+        <move id="12" angle="-33.76639"/>
+        <move id="13" angle="-33.76639"/>
+        <move id="14" angle="23.958351"/>
+        <move id="15" angle="23.958351"/>
+        <move id="16" angle="-7.903243"/>
+        <move id="17" angle="-7.903243"/>
+        <move id="18" angle="-18.376505"/>
+        <move id="19" angle="-18.376505"/>
+        <move id="20" angle="-20.77221"/>
+        <move id="21" angle="-20.77221"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-87.61395"/>
+        <move id="3" angle="-87.61395"/>
+        <move id="4" angle="-3.732545"/>
+        <move id="5" angle="-3.732545"/>
+        <move id="6" angle="-3.1527936"/>
+        <move id="7" angle="-3.1527936"/>
+        <move id="8" angle="-97.287674"/>
+        <move id="9" angle="-97.287674"/>
+        <move id="10" angle="-46.131386"/>
+        <move id="11" angle="-46.131386"/>
+        <move id="12" angle="-25.069555"/>
+        <move id="13" angle="-25.069555"/>
+        <move id="14" angle="-7.6755543"/>
+        <move id="15" angle="-7.6755543"/>
+        <move id="16" angle="-9.066617"/>
+        <move id="17" angle="-9.066617"/>
+        <move id="18" angle="9.437028"/>
+        <move id="19" angle="9.437028"/>
+        <move id="20" angle="0.19799754"/>
+        <move id="21" angle="0.19799754"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-4.7361717"/>
+        <move id="3" angle="-4.7361717"/>
+        <move id="4" angle="4.523938"/>
+        <move id="5" angle="4.523938"/>
+        <move id="6" angle="1.5581197"/>
+        <move id="7" angle="1.5581197"/>
+        <move id="8" angle="0.91717005"/>
+        <move id="9" angle="0.91717005"/>
+        <move id="10" angle="-0.73723245"/>
+        <move id="11" angle="-0.73723245"/>
+        <move id="12" angle="7.8827386"/>
+        <move id="13" angle="7.8827386"/>
+        <move id="14" angle="42.908276"/>
+        <move id="15" angle="42.908276"/>
+        <move id="16" angle="6.8883567"/>
+        <move id="17" angle="6.8883567"/>
+        <move id="18" angle="4.924833"/>
+        <move id="19" angle="4.924833"/>
+        <move id="20" angle="-0.006686151"/>
+        <move id="21" angle="-0.006686151"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-62.821877"/>
+        <move id="3" angle="-62.821877"/>
+        <move id="4" angle="1.0078714"/>
+        <move id="5" angle="1.0078714"/>
+        <move id="6" angle="1.994555"/>
+        <move id="7" angle="1.994555"/>
+        <move id="8" angle="-124.40493"/>
+        <move id="9" angle="-124.40493"/>
+        <move id="10" angle="46.153664"/>
+        <move id="11" angle="46.153664"/>
+        <move id="12" angle="30.022522"/>
+        <move id="13" angle="30.022522"/>
+        <move id="14" angle="3.9094872"/>
+        <move id="15" angle="3.9094872"/>
+        <move id="16" angle="12.2626095"/>
+        <move id="17" angle="12.2626095"/>
+        <move id="18" angle="12.774431"/>
+        <move id="19" angle="12.774431"/>
+        <move id="20" angle="10.435047"/>
+        <move id="21" angle="10.435047"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-55.784325"/>
+        <move id="3" angle="-55.784325"/>
+        <move id="4" angle="47.548805"/>
+        <move id="5" angle="47.548805"/>
+        <move id="6" angle="101.01688"/>
+        <move id="7" angle="101.01688"/>
+        <move id="8" angle="-122.21617"/>
+        <move id="9" angle="-122.21617"/>
+        <move id="10" angle="46.225307"/>
+        <move id="11" angle="46.225307"/>
+        <move id="12" angle="-56.009483"/>
+        <move id="13" angle="-56.009483"/>
+        <move id="14" angle="16.3225"/>
+        <move id="15" angle="16.3225"/>
+        <move id="16" angle="4.7965903"/>
+        <move id="17" angle="4.7965903"/>
+        <move id="18" angle="-25.914183"/>
+        <move id="19" angle="-25.914183"/>
+        <move id="20" angle="2.689302"/>
+        <move id="21" angle="2.689302"/>
+    </slot>
+    <slot delta="0.14">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-58.72785"/>
+        <move id="3" angle="-58.72785"/>
+        <move id="4" angle="43.85045"/>
+        <move id="5" angle="43.85045"/>
+        <move id="6" angle="108.34738"/>
+        <move id="7" angle="108.34738"/>
+        <move id="8" angle="-131.66202"/>
+        <move id="9" angle="-131.66202"/>
+        <move id="10" angle="44.46511"/>
+        <move id="11" angle="44.46511"/>
+        <move id="12" angle="-53.461926"/>
+        <move id="13" angle="-53.461926"/>
+        <move id="14" angle="-17.58822"/>
+        <move id="15" angle="-17.58822"/>
+        <move id="16" angle="-16.696602"/>
+        <move id="17" angle="-16.696602"/>
+        <move id="18" angle="0.24892278"/>
+        <move id="19" angle="0.24892278"/>
+        <move id="20" angle="14.703634"/>
+        <move id="21" angle="14.703634"/>
+    </slot>
+    <slot delta="0.42">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-1.136161"/>
+        <move id="3" angle="-1.136161"/>
+        <move id="4" angle="28.925074"/>
+        <move id="5" angle="28.925074"/>
+        <move id="6" angle="35.894753"/>
+        <move id="7" angle="35.894753"/>
+        <move id="8" angle="-94.40036"/>
+        <move id="9" angle="-94.40036"/>
+        <move id="10" angle="57.54773"/>
+        <move id="11" angle="57.54773"/>
+        <move id="12" angle="-4.952044"/>
+        <move id="13" angle="-4.952044"/>
+        <move id="14" angle="-123.39889"/>
+        <move id="15" angle="-123.39889"/>
+        <move id="16" angle="18.326159"/>
+        <move id="17" angle="18.326159"/>
+        <move id="18" angle="65.677376"/>
+        <move id="19" angle="65.677376"/>
+        <move id="20" angle="-21.766216"/>
+        <move id="21" angle="-21.766216"/>
+    </slot>
+</behavior>

behaviors/slot/r2/Kick_Motion.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Kick motion with right leg" auto_head="1">
+    <slot delta="0.22">    <!-- Lean -->
+        <move id="5" angle="-10"/>  <!-- Left  leg roll -->
+        <move id="6" angle="40"/>   <!-- Left  leg pitch -->
+        <move id="7" angle="65"/>   <!-- Right leg pitch -->
+        <move id="8" angle="-60"/>  <!-- Left  knee -->
+        <move id="9" angle="-115"/> <!-- Right knee -->
+        <move id="10" angle="60"/>  <!-- Left  foot pitch -->
+        <move id="11" angle="25"/>  <!-- Right foot pitch -->
+    </slot>
+    <slot delta="0.12">    <!-- Kick -->
+        <move id="3" angle="-45"/> <!-- Right leg yaw/pitch -->
+        <move id="6" angle="-25"/> <!-- Left  leg pitch -->
+        <move id="7" angle="80"/>  <!-- Right leg pitch -->
+        <move id="8" angle="0"/>   <!-- Left knee -->
+        <move id="9" angle="0"/>   <!-- Right knee -->
+        <move id="10" angle="30"/> <!-- Left  foot pitch -->
+    </slot>
+</behavior>

behaviors/slot/r3/Get_Up_Back.xml

@@ -0,0 +1,148 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-38.572456"/>
+        <move id="3" angle="-38.572456"/>
+        <move id="4" angle="10.999639"/>
+        <move id="5" angle="10.999639"/>
+        <move id="6" angle="77.6285"/>
+        <move id="7" angle="77.6285"/>
+        <move id="8" angle="-15.158332"/>
+        <move id="9" angle="-15.158332"/>
+        <move id="10" angle="-35.486084"/>
+        <move id="11" angle="-35.486084"/>
+        <move id="12" angle="2.6835575"/>
+        <move id="13" angle="2.6835575"/>
+        <move id="14" angle="73.84404"/>
+        <move id="15" angle="73.84404"/>
+        <move id="16" angle="-54.934566"/>
+        <move id="17" angle="-54.934566"/>
+        <move id="18" angle="-113.41597"/>
+        <move id="19" angle="-113.41597"/>
+        <move id="20" angle="78.12353"/>
+        <move id="21" angle="78.12353"/>
+    </slot>
+    <slot delta="0.24">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-65.98044"/>
+        <move id="3" angle="-65.98044"/>
+        <move id="4" angle="-50.182472"/>
+        <move id="5" angle="-50.182472"/>
+        <move id="6" angle="30.041952"/>
+        <move id="7" angle="30.041952"/>
+        <move id="8" angle="34.80442"/>
+        <move id="9" angle="34.80442"/>
+        <move id="10" angle="49.432697"/>
+        <move id="11" angle="49.432697"/>
+        <move id="12" angle="-77.979546"/>
+        <move id="13" angle="-77.979546"/>
+        <move id="14" angle="-0.92560226"/>
+        <move id="15" angle="-0.92560226"/>
+        <move id="16" angle="52.747627"/>
+        <move id="17" angle="52.747627"/>
+        <move id="18" angle="127.680466"/>
+        <move id="19" angle="127.680466"/>
+        <move id="20" angle="57.550724"/>
+        <move id="21" angle="57.550724"/>
+    </slot>
+    <slot delta="0.32">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-62.44547"/>
+        <move id="3" angle="-62.44547"/>
+        <move id="4" angle="9.1681"/>
+        <move id="5" angle="9.1681"/>
+        <move id="6" angle="134.78099"/>
+        <move id="7" angle="134.78099"/>
+        <move id="8" angle="-27.68505"/>
+        <move id="9" angle="-27.68505"/>
+        <move id="10" angle="-115.14692"/>
+        <move id="11" angle="-115.14692"/>
+        <move id="12" angle="-44.91786"/>
+        <move id="13" angle="-44.91786"/>
+        <move id="14" angle="56.92144"/>
+        <move id="15" angle="56.92144"/>
+        <move id="16" angle="-52.54726"/>
+        <move id="17" angle="-52.54726"/>
+        <move id="18" angle="-54.15026"/>
+        <move id="19" angle="-54.15026"/>
+        <move id="20" angle="-9.677674"/>
+        <move id="21" angle="-9.677674"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-58.012352"/>
+        <move id="3" angle="-58.012352"/>
+        <move id="4" angle="43.231464"/>
+        <move id="5" angle="43.231464"/>
+        <move id="6" angle="-44.049873"/>
+        <move id="7" angle="-44.049873"/>
+        <move id="8" angle="-63.7723"/>
+        <move id="9" angle="-63.7723"/>
+        <move id="10" angle="-56.622414"/>
+        <move id="11" angle="-56.622414"/>
+        <move id="12" angle="-28.760904"/>
+        <move id="13" angle="-28.760904"/>
+        <move id="14" angle="-14.939745"/>
+        <move id="15" angle="-14.939745"/>
+        <move id="16" angle="23.633577"/>
+        <move id="17" angle="23.633577"/>
+        <move id="18" angle="-85.10401"/>
+        <move id="19" angle="-85.10401"/>
+        <move id="20" angle="17.66445"/>
+        <move id="21" angle="17.66445"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-24.942818"/>
+        <move id="3" angle="-24.942818"/>
+        <move id="4" angle="78.45739"/>
+        <move id="5" angle="78.45739"/>
+        <move id="6" angle="106.30913"/>
+        <move id="7" angle="106.30913"/>
+        <move id="8" angle="-152.12027"/>
+        <move id="9" angle="-152.12027"/>
+        <move id="10" angle="42.071213"/>
+        <move id="11" angle="42.071213"/>
+        <move id="12" angle="-65.846405"/>
+        <move id="13" angle="-65.846405"/>
+        <move id="14" angle="-57.11586"/>
+        <move id="15" angle="-57.11586"/>
+        <move id="16" angle="-81.987076"/>
+        <move id="17" angle="-81.987076"/>
+        <move id="18" angle="11.453229"/>
+        <move id="19" angle="11.453229"/>
+        <move id="20" angle="-39.00894"/>
+        <move id="21" angle="-39.00894"/>
+    </slot>
+    <slot delta="0.18">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-0.6814623"/>
+        <move id="3" angle="-0.6814623"/>
+        <move id="4" angle="35.28643"/>
+        <move id="5" angle="35.28643"/>
+        <move id="6" angle="75.47946"/>
+        <move id="7" angle="75.47946"/>
+        <move id="8" angle="-132.41306"/>
+        <move id="9" angle="-132.41306"/>
+        <move id="10" angle="46.746563"/>
+        <move id="11" angle="46.746563"/>
+        <move id="12" angle="-8.374908"/>
+        <move id="13" angle="-8.374908"/>
+        <move id="14" angle="-20.005192"/>
+        <move id="15" angle="-20.005192"/>
+        <move id="16" angle="9.418695"/>
+        <move id="17" angle="9.418695"/>
+        <move id="18" angle="186.1502"/>
+        <move id="19" angle="186.1502"/>
+        <move id="20" angle="133.16562"/>
+        <move id="21" angle="133.16562"/>
+    </slot>
+</behavior>

behaviors/slot/r3/Get_Up_Front.xml

@@ -0,0 +1,220 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-12.640889"/>
+        <move id="3" angle="-12.640889"/>
+        <move id="4" angle="4.646634"/>
+        <move id="5" angle="4.646634"/>
+        <move id="6" angle="17.342707"/>
+        <move id="7" angle="17.342707"/>
+        <move id="8" angle="3.5387506"/>
+        <move id="9" angle="3.5387506"/>
+        <move id="10" angle="3.3997679"/>
+        <move id="11" angle="3.3997679"/>
+        <move id="12" angle="0.6578274"/>
+        <move id="13" angle="0.6578274"/>
+        <move id="14" angle="-8.817135"/>
+        <move id="15" angle="-8.817135"/>
+        <move id="16" angle="61.96675"/>
+        <move id="17" angle="61.96675"/>
+        <move id="18" angle="1.5812168"/>
+        <move id="19" angle="1.5812168"/>
+        <move id="20" angle="-8.415477"/>
+        <move id="21" angle="-8.415477"/>
+    </slot>
+    <slot delta="0.18">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="5.4146767"/>
+        <move id="3" angle="5.4146767"/>
+        <move id="4" angle="-11.824036"/>
+        <move id="5" angle="-11.824036"/>
+        <move id="6" angle="8.015917"/>
+        <move id="7" angle="8.015917"/>
+        <move id="8" angle="-1.4786508"/>
+        <move id="9" angle="-1.4786508"/>
+        <move id="10" angle="6.2315826"/>
+        <move id="11" angle="6.2315826"/>
+        <move id="12" angle="11.371917"/>
+        <move id="13" angle="11.371917"/>
+        <move id="14" angle="6.024707"/>
+        <move id="15" angle="6.024707"/>
+        <move id="16" angle="61.55641"/>
+        <move id="17" angle="61.55641"/>
+        <move id="18" angle="-6.719823"/>
+        <move id="19" angle="-6.719823"/>
+        <move id="20" angle="-19.866516"/>
+        <move id="21" angle="-19.866516"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-2.2857966"/>
+        <move id="3" angle="-2.2857966"/>
+        <move id="4" angle="-5.441041"/>
+        <move id="5" angle="-5.441041"/>
+        <move id="6" angle="270.0556"/>
+        <move id="7" angle="270.0556"/>
+        <move id="8" angle="-127.3845"/>
+        <move id="9" angle="-127.3845"/>
+        <move id="10" angle="123.612"/>
+        <move id="11" angle="123.612"/>
+        <move id="12" angle="-6.389326"/>
+        <move id="13" angle="-6.389326"/>
+        <move id="14" angle="-2.5006394"/>
+        <move id="15" angle="-2.5006394"/>
+        <move id="16" angle="-8.59928"/>
+        <move id="17" angle="-8.59928"/>
+        <move id="18" angle="-10.413264"/>
+        <move id="19" angle="-10.413264"/>
+        <move id="20" angle="-10.369881"/>
+        <move id="21" angle="-10.369881"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-15.020902"/>
+        <move id="3" angle="-15.020902"/>
+        <move id="4" angle="39.801743"/>
+        <move id="5" angle="39.801743"/>
+        <move id="6" angle="7.683368"/>
+        <move id="7" angle="7.683368"/>
+        <move id="8" angle="-96.00759"/>
+        <move id="9" angle="-96.00759"/>
+        <move id="10" angle="2.5151913"/>
+        <move id="11" angle="2.5151913"/>
+        <move id="12" angle="-44.600082"/>
+        <move id="13" angle="-44.600082"/>
+        <move id="14" angle="28.740438"/>
+        <move id="15" angle="28.740438"/>
+        <move id="16" angle="-7.8307633"/>
+        <move id="17" angle="-7.8307633"/>
+        <move id="18" angle="-25.14883"/>
+        <move id="19" angle="-25.14883"/>
+        <move id="20" angle="-21.899431"/>
+        <move id="21" angle="-21.899431"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-91.19676"/>
+        <move id="3" angle="-91.19676"/>
+        <move id="4" angle="-1.6109102"/>
+        <move id="5" angle="-1.6109102"/>
+        <move id="6" angle="-3.0909305"/>
+        <move id="7" angle="-3.0909305"/>
+        <move id="8" angle="-97.02908"/>
+        <move id="9" angle="-97.02908"/>
+        <move id="10" angle="-44.9949"/>
+        <move id="11" angle="-44.9949"/>
+        <move id="12" angle="-24.864588"/>
+        <move id="13" angle="-24.864588"/>
+        <move id="14" angle="-10.729849"/>
+        <move id="15" angle="-10.729849"/>
+        <move id="16" angle="-12.871218"/>
+        <move id="17" angle="-12.871218"/>
+        <move id="18" angle="10.0304985"/>
+        <move id="19" angle="10.0304985"/>
+        <move id="20" angle="10.550584"/>
+        <move id="21" angle="10.550584"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-3.6664605"/>
+        <move id="3" angle="-3.6664605"/>
+        <move id="4" angle="2.7512116"/>
+        <move id="5" angle="2.7512116"/>
+        <move id="6" angle="4.2976775"/>
+        <move id="7" angle="4.2976775"/>
+        <move id="8" angle="-1.8928692"/>
+        <move id="9" angle="-1.8928692"/>
+        <move id="10" angle="-1.3523235"/>
+        <move id="11" angle="-1.3523235"/>
+        <move id="12" angle="-4.7275023"/>
+        <move id="13" angle="-4.7275023"/>
+        <move id="14" angle="44.758335"/>
+        <move id="15" angle="44.758335"/>
+        <move id="16" angle="17.067863"/>
+        <move id="17" angle="17.067863"/>
+        <move id="18" angle="-7.7519336"/>
+        <move id="19" angle="-7.7519336"/>
+        <move id="20" angle="-4.652607"/>
+        <move id="21" angle="-4.652607"/>
+    </slot>
+    <slot delta="0.04">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-59.41754"/>
+        <move id="3" angle="-59.41754"/>
+        <move id="4" angle="49.316"/>
+        <move id="5" angle="49.316"/>
+        <move id="6" angle="100.66388"/>
+        <move id="7" angle="100.66388"/>
+        <move id="8" angle="-121.67775"/>
+        <move id="9" angle="-121.67775"/>
+        <move id="10" angle="46.480865"/>
+        <move id="11" angle="46.480865"/>
+        <move id="12" angle="-57.7324"/>
+        <move id="13" angle="-57.7324"/>
+        <move id="14" angle="5.5761175"/>
+        <move id="15" angle="5.5761175"/>
+        <move id="16" angle="18.378542"/>
+        <move id="17" angle="18.378542"/>
+        <move id="18" angle="-27.80392"/>
+        <move id="19" angle="-27.80392"/>
+        <move id="20" angle="10.984678"/>
+        <move id="21" angle="10.984678"/>
+    </slot>
+    <slot delta="0.12">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-47.20175"/>
+        <move id="3" angle="-47.20175"/>
+        <move id="4" angle="47.4648"/>
+        <move id="5" angle="47.4648"/>
+        <move id="6" angle="115.67783"/>
+        <move id="7" angle="115.67783"/>
+        <move id="8" angle="-135.32626"/>
+        <move id="9" angle="-135.32626"/>
+        <move id="10" angle="42.217873"/>
+        <move id="11" angle="42.217873"/>
+        <move id="12" angle="-65.463715"/>
+        <move id="13" angle="-65.463715"/>
+        <move id="14" angle="-19.853582"/>
+        <move id="15" angle="-19.853582"/>
+        <move id="16" angle="-26.5262"/>
+        <move id="17" angle="-26.5262"/>
+        <move id="18" angle="17.335098"/>
+        <move id="19" angle="17.335098"/>
+        <move id="20" angle="7.6011276"/>
+        <move id="21" angle="7.6011276"/>
+    </slot>
+    <slot delta="0.4">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-1.6223584"/>
+        <move id="3" angle="-1.6223584"/>
+        <move id="4" angle="24.222506"/>
+        <move id="5" angle="24.222506"/>
+        <move id="6" angle="38.50319"/>
+        <move id="7" angle="38.50319"/>
+        <move id="8" angle="-93.59837"/>
+        <move id="9" angle="-93.59837"/>
+        <move id="10" angle="53.80671"/>
+        <move id="11" angle="53.80671"/>
+        <move id="12" angle="-2.3789244"/>
+        <move id="13" angle="-2.3789244"/>
+        <move id="14" angle="-119.351814"/>
+        <move id="15" angle="-119.351814"/>
+        <move id="16" angle="0.0"/>
+        <move id="17" angle="0.0"/>
+        <move id="18" angle="67.36445"/>
+        <move id="19" angle="67.36445"/>
+        <move id="20" angle="90"/>
+        <move id="21" angle="90"/>
+    </slot>
+</behavior>

behaviors/slot/r3/Kick_Motion.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Kick motion with right leg" auto_head="1">
+    <slot delta="0.22">    <!-- Lean -->
+        <move id="5" angle="-10"/>  <!-- Left  leg roll -->
+        <move id="6" angle="40"/>   <!-- Left  leg pitch -->
+        <move id="7" angle="65"/>   <!-- Right leg pitch -->
+        <move id="8" angle="-60"/>  <!-- Left  knee -->
+        <move id="9" angle="-115"/> <!-- Right knee -->
+        <move id="10" angle="60"/>  <!-- Left  foot pitch -->
+        <move id="11" angle="-5"/>  <!-- Right foot pitch -->
+    </slot>
+    <slot delta="0.12">    <!-- Kick -->
+        <move id="3" angle="-45"/> <!-- Right leg yaw/pitch -->
+        <move id="6" angle="-25"/> <!-- Left  leg pitch -->
+        <move id="7" angle="80"/>  <!-- Right leg pitch -->
+        <move id="8" angle="0"/>   <!-- Left knee -->
+        <move id="9" angle="0"/>   <!-- Right knee -->
+        <move id="10" angle="30"/> <!-- Left  foot pitch -->
+        <move id="17" angle="40"/> <!-- Right arm roll -->
+    </slot>
+</behavior>

behaviors/slot/r4/Get_Up_Back.xml

@@ -0,0 +1,148 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-48.405083"/>
+        <move id="3" angle="-48.405083"/>
+        <move id="4" angle="33.774643"/>
+        <move id="5" angle="33.774643"/>
+        <move id="6" angle="100.784706"/>
+        <move id="7" angle="100.784706"/>
+        <move id="8" angle="7.756978"/>
+        <move id="9" angle="7.756978"/>
+        <move id="10" angle="-23.10689"/>
+        <move id="11" angle="-23.10689"/>
+        <move id="12" angle="85.49136"/>
+        <move id="13" angle="85.49136"/>
+        <move id="14" angle="38.31756"/>
+        <move id="15" angle="38.31756"/>
+        <move id="16" angle="-67.331825"/>
+        <move id="17" angle="-67.331825"/>
+        <move id="18" angle="22.26954"/>
+        <move id="19" angle="22.26954"/>
+        <move id="20" angle="74.803024"/>
+        <move id="21" angle="74.803024"/>
+    </slot>
+    <slot delta="0.23336456513404846">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-70.5122"/>
+        <move id="3" angle="-70.5122"/>
+        <move id="4" angle="-33.731937"/>
+        <move id="5" angle="-33.731937"/>
+        <move id="6" angle="24.08416"/>
+        <move id="7" angle="24.08416"/>
+        <move id="8" angle="43.438618"/>
+        <move id="9" angle="43.438618"/>
+        <move id="10" angle="58.60381"/>
+        <move id="11" angle="58.60381"/>
+        <move id="12" angle="-85.92165"/>
+        <move id="13" angle="-85.92165"/>
+        <move id="14" angle="5.59869"/>
+        <move id="15" angle="5.59869"/>
+        <move id="16" angle="122.71095"/>
+        <move id="17" angle="122.71095"/>
+        <move id="18" angle="55.634182"/>
+        <move id="19" angle="55.634182"/>
+        <move id="20" angle="85.27185"/>
+        <move id="21" angle="85.27185"/>
+    </slot>
+    <slot delta="0.33675475766658775">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-38.059784"/>
+        <move id="3" angle="-38.059784"/>
+        <move id="4" angle="46.120163"/>
+        <move id="5" angle="46.120163"/>
+        <move id="6" angle="120.91053"/>
+        <move id="7" angle="120.91053"/>
+        <move id="8" angle="-37.19999"/>
+        <move id="9" angle="-37.19999"/>
+        <move id="10" angle="-60.143646"/>
+        <move id="11" angle="-60.143646"/>
+        <move id="12" angle="-81.65832"/>
+        <move id="13" angle="-81.65832"/>
+        <move id="14" angle="35.256054"/>
+        <move id="15" angle="35.256054"/>
+        <move id="16" angle="-17.517155"/>
+        <move id="17" angle="-17.517155"/>
+        <move id="18" angle="18.431877"/>
+        <move id="19" angle="18.431877"/>
+        <move id="20" angle="-25.010399"/>
+        <move id="21" angle="-25.010399"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-69.23109"/>
+        <move id="3" angle="-69.23109"/>
+        <move id="4" angle="15.320035"/>
+        <move id="5" angle="15.320035"/>
+        <move id="6" angle="30.714127"/>
+        <move id="7" angle="30.714127"/>
+        <move id="8" angle="-47.507515"/>
+        <move id="9" angle="-47.507515"/>
+        <move id="10" angle="-61.518223"/>
+        <move id="11" angle="-61.518223"/>
+        <move id="12" angle="-27.325377"/>
+        <move id="13" angle="-27.325377"/>
+        <move id="14" angle="45.20161"/>
+        <move id="15" angle="45.20161"/>
+        <move id="16" angle="71.49224"/>
+        <move id="17" angle="71.49224"/>
+        <move id="18" angle="37.682297"/>
+        <move id="19" angle="37.682297"/>
+        <move id="20" angle="10.35512"/>
+        <move id="21" angle="10.35512"/>
+    </slot>
+    <slot delta="0.05201605577468872">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-28.16217"/>
+        <move id="3" angle="-28.16217"/>
+        <move id="4" angle="52.758217"/>
+        <move id="5" angle="52.758217"/>
+        <move id="6" angle="111.2359"/>
+        <move id="7" angle="111.2359"/>
+        <move id="8" angle="-118.02957"/>
+        <move id="9" angle="-118.02957"/>
+        <move id="10" angle="54.038685"/>
+        <move id="11" angle="54.038685"/>
+        <move id="12" angle="-123.210915"/>
+        <move id="13" angle="-123.210915"/>
+        <move id="14" angle="-26.220789"/>
+        <move id="15" angle="-26.220789"/>
+        <move id="16" angle="28.817947"/>
+        <move id="17" angle="28.817947"/>
+        <move id="18" angle="-14.253399"/>
+        <move id="19" angle="-14.253399"/>
+        <move id="20" angle="17.878008"/>
+        <move id="21" angle="17.878008"/>
+    </slot>
+    <slot delta="0.15276842055320738">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-5.5297728"/>
+        <move id="3" angle="-5.5297728"/>
+        <move id="4" angle="90.15973"/>
+        <move id="5" angle="90.15973"/>
+        <move id="6" angle="50.418648"/>
+        <move id="7" angle="50.418648"/>
+        <move id="8" angle="-90.911835"/>
+        <move id="9" angle="-90.911835"/>
+        <move id="10" angle="50.15513"/>
+        <move id="11" angle="50.15513"/>
+        <move id="12" angle="-45.83729"/>
+        <move id="13" angle="-45.83729"/>
+        <move id="14" angle="-39.350525"/>
+        <move id="15" angle="-39.350525"/>
+        <move id="16" angle="-23.518953"/>
+        <move id="17" angle="-23.518953"/>
+        <move id="18" angle="72.38919"/>
+        <move id="19" angle="72.38919"/>
+        <move id="20" angle="-77.339966"/>
+        <move id="21" angle="-77.339966"/>
+    </slot>
+</behavior>

behaviors/slot/r4/Get_Up_Front.xml

@@ -0,0 +1,220 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
+    <slot delta="0.16">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="4.6313915"/>
+        <move id="3" angle="4.6313915"/>
+        <move id="4" angle="-11.846246"/>
+        <move id="5" angle="-11.846246"/>
+        <move id="6" angle="8.745557"/>
+        <move id="7" angle="8.745557"/>
+        <move id="8" angle="-1.8197118"/>
+        <move id="9" angle="-1.8197118"/>
+        <move id="10" angle="1.359597"/>
+        <move id="11" angle="1.359597"/>
+        <move id="12" angle="16.82137"/>
+        <move id="13" angle="16.82137"/>
+        <move id="14" angle="21.83775"/>
+        <move id="15" angle="21.83775"/>
+        <move id="16" angle="-27.174564"/>
+        <move id="17" angle="-27.174564"/>
+        <move id="18" angle="-12.889872"/>
+        <move id="19" angle="-12.889872"/>
+        <move id="20" angle="-14.989797"/>
+        <move id="21" angle="-14.989797"/>
+    </slot>
+    <slot delta="0.08">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="1.0406766"/>
+        <move id="3" angle="1.0406766"/>
+        <move id="4" angle="-7.939904"/>
+        <move id="5" angle="-7.939904"/>
+        <move id="6" angle="260.59564"/>
+        <move id="7" angle="260.59564"/>
+        <move id="8" angle="-131.98882"/>
+        <move id="9" angle="-131.98882"/>
+        <move id="10" angle="123.69923"/>
+        <move id="11" angle="123.69923"/>
+        <move id="12" angle="-8.521371"/>
+        <move id="13" angle="-8.521371"/>
+        <move id="14" angle="3.173905"/>
+        <move id="15" angle="3.173905"/>
+        <move id="16" angle="-19.583878"/>
+        <move id="17" angle="-19.583878"/>
+        <move id="18" angle="-13.1733055"/>
+        <move id="19" angle="-13.1733055"/>
+        <move id="20" angle="-17.131882"/>
+        <move id="21" angle="-17.131882"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-19.195496"/>
+        <move id="3" angle="-19.195496"/>
+        <move id="4" angle="42.399166"/>
+        <move id="5" angle="42.399166"/>
+        <move id="6" angle="4.278442"/>
+        <move id="7" angle="4.278442"/>
+        <move id="8" angle="-93.31827"/>
+        <move id="9" angle="-93.31827"/>
+        <move id="10" angle="2.5259771"/>
+        <move id="11" angle="2.5259771"/>
+        <move id="12" angle="-33.76639"/>
+        <move id="13" angle="-33.76639"/>
+        <move id="14" angle="23.958351"/>
+        <move id="15" angle="23.958351"/>
+        <move id="16" angle="-7.903243"/>
+        <move id="17" angle="-7.903243"/>
+        <move id="18" angle="-18.376505"/>
+        <move id="19" angle="-18.376505"/>
+        <move id="20" angle="-20.77221"/>
+        <move id="21" angle="-20.77221"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-87.61395"/>
+        <move id="3" angle="-87.61395"/>
+        <move id="4" angle="-3.732545"/>
+        <move id="5" angle="-3.732545"/>
+        <move id="6" angle="-3.1527936"/>
+        <move id="7" angle="-3.1527936"/>
+        <move id="8" angle="-97.287674"/>
+        <move id="9" angle="-97.287674"/>
+        <move id="10" angle="-46.131386"/>
+        <move id="11" angle="-46.131386"/>
+        <move id="12" angle="-25.069555"/>
+        <move id="13" angle="-25.069555"/>
+        <move id="14" angle="-7.6755543"/>
+        <move id="15" angle="-7.6755543"/>
+        <move id="16" angle="-9.066617"/>
+        <move id="17" angle="-9.066617"/>
+        <move id="18" angle="9.437028"/>
+        <move id="19" angle="9.437028"/>
+        <move id="20" angle="0.19799754"/>
+        <move id="21" angle="0.19799754"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-4.7361717"/>
+        <move id="3" angle="-4.7361717"/>
+        <move id="4" angle="4.523938"/>
+        <move id="5" angle="4.523938"/>
+        <move id="6" angle="1.5581197"/>
+        <move id="7" angle="1.5581197"/>
+        <move id="8" angle="0.91717005"/>
+        <move id="9" angle="0.91717005"/>
+        <move id="10" angle="-0.73723245"/>
+        <move id="11" angle="-0.73723245"/>
+        <move id="12" angle="7.8827386"/>
+        <move id="13" angle="7.8827386"/>
+        <move id="14" angle="42.908276"/>
+        <move id="15" angle="42.908276"/>
+        <move id="16" angle="6.8883567"/>
+        <move id="17" angle="6.8883567"/>
+        <move id="18" angle="4.924833"/>
+        <move id="19" angle="4.924833"/>
+        <move id="20" angle="-0.006686151"/>
+        <move id="21" angle="-0.006686151"/>
+    </slot>
+    <slot delta="0.02">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-62.821877"/>
+        <move id="3" angle="-62.821877"/>
+        <move id="4" angle="1.0078714"/>
+        <move id="5" angle="1.0078714"/>
+        <move id="6" angle="1.994555"/>
+        <move id="7" angle="1.994555"/>
+        <move id="8" angle="-124.40493"/>
+        <move id="9" angle="-124.40493"/>
+        <move id="10" angle="46.153664"/>
+        <move id="11" angle="46.153664"/>
+        <move id="12" angle="30.022522"/>
+        <move id="13" angle="30.022522"/>
+        <move id="14" angle="3.9094872"/>
+        <move id="15" angle="3.9094872"/>
+        <move id="16" angle="12.2626095"/>
+        <move id="17" angle="12.2626095"/>
+        <move id="18" angle="12.774431"/>
+        <move id="19" angle="12.774431"/>
+        <move id="20" angle="10.435047"/>
+        <move id="21" angle="10.435047"/>
+    </slot>
+    <slot delta="0.06">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-55.784325"/>
+        <move id="3" angle="-55.784325"/>
+        <move id="4" angle="47.548805"/>
+        <move id="5" angle="47.548805"/>
+        <move id="6" angle="101.01688"/>
+        <move id="7" angle="101.01688"/>
+        <move id="8" angle="-122.21617"/>
+        <move id="9" angle="-122.21617"/>
+        <move id="10" angle="46.225307"/>
+        <move id="11" angle="46.225307"/>
+        <move id="12" angle="-56.009483"/>
+        <move id="13" angle="-56.009483"/>
+        <move id="14" angle="16.3225"/>
+        <move id="15" angle="16.3225"/>
+        <move id="16" angle="4.7965903"/>
+        <move id="17" angle="4.7965903"/>
+        <move id="18" angle="-25.914183"/>
+        <move id="19" angle="-25.914183"/>
+        <move id="20" angle="2.689302"/>
+        <move id="21" angle="2.689302"/>
+    </slot>
+    <slot delta="0.14">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-58.72785"/>
+        <move id="3" angle="-58.72785"/>
+        <move id="4" angle="43.85045"/>
+        <move id="5" angle="43.85045"/>
+        <move id="6" angle="108.34738"/>
+        <move id="7" angle="108.34738"/>
+        <move id="8" angle="-131.66202"/>
+        <move id="9" angle="-131.66202"/>
+        <move id="10" angle="44.46511"/>
+        <move id="11" angle="44.46511"/>
+        <move id="12" angle="-53.461926"/>
+        <move id="13" angle="-53.461926"/>
+        <move id="14" angle="-17.58822"/>
+        <move id="15" angle="-17.58822"/>
+        <move id="16" angle="-16.696602"/>
+        <move id="17" angle="-16.696602"/>
+        <move id="18" angle="0.24892278"/>
+        <move id="19" angle="0.24892278"/>
+        <move id="20" angle="14.703634"/>
+        <move id="21" angle="14.703634"/>
+    </slot>
+    <slot delta="0.42">
+        <move id="0" angle="0.0"/>
+        <move id="1" angle="0.0"/>
+        <move id="2" angle="-1.136161"/>
+        <move id="3" angle="-1.136161"/>
+        <move id="4" angle="28.925074"/>
+        <move id="5" angle="28.925074"/>
+        <move id="6" angle="35.894753"/>
+        <move id="7" angle="35.894753"/>
+        <move id="8" angle="-94.40036"/>
+        <move id="9" angle="-94.40036"/>
+        <move id="10" angle="57.54773"/>
+        <move id="11" angle="57.54773"/>
+        <move id="12" angle="-4.952044"/>
+        <move id="13" angle="-4.952044"/>
+        <move id="14" angle="-123.39889"/>
+        <move id="15" angle="-123.39889"/>
+        <move id="16" angle="18.326159"/>
+        <move id="17" angle="18.326159"/>
+        <move id="18" angle="65.677376"/>
+        <move id="19" angle="65.677376"/>
+        <move id="20" angle="-21.766216"/>
+        <move id="21" angle="-21.766216"/>
+    </slot>
+</behavior>

behaviors/slot/r4/Kick_Motion.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<behavior description="Kick motion with right leg" auto_head="1">
+    <slot delta="0.12">    <!-- Lean -->
+        <move id="5" angle="-5"/>   <!-- Left  leg roll -->
+        <move id="7" angle="65"/>   <!-- Right leg pitch -->
+        <move id="8" angle="-70"/>  <!-- Left  knee -->
+        <move id="9" angle="-130"/> <!-- Right knee -->
+        <move id="10" angle="60"/>  <!-- Left  foot pitch -->
+        <move id="11" angle="30"/>  <!-- Right foot pitch -->
+    </slot>
+
+
+</behavior>