<jupyter_start><jupyter_text>CMA for SwingThe code for this is based on <NAME>'s "CMA Cart Pole" code, distributed in the Advanced Topics in Machine Learning, 2018.<jupyter_code>import tensorflow as tf
import numpy as np
import matplotlib.pyplot as plt
import cma
import math
import rpyc
from IPython import display
import time
from sklearn.linear_model import LinearRegression
import pickle<jupyter_output>/home/dm/.local/share/virtualenvs/LEGO-machine-learning-LdgK-kXV/lib/python3.5/site-packages/tensorflow/python/framework/dtypes.py:523: FutureWarning: Passing (type, 1) or '1type' as a synonym of type is deprecated; in a future version of numpy, it will be understood as (type, (1,)) / '(1,)type'.
  _np_qint8 = np.dtype([("qint8", np.int8, 1)])
/home/dm/.local/share/virtualenvs/LEGO-machine-learning-LdgK-kXV/lib/python3.5/site-packages/tensorflow/python/framework/dtypes.py:524: FutureWarning: Passing (type, 1) or '1type' as a synonym of type is deprecated; in a future version of numpy, it will be understood as (type, (1,)) / '(1,)type'.
  _np_quint8 = np.dtype([("quint8", np.uint8, 1)])
/home/dm/.local/share/virtualenvs/LEGO-machine-learning-LdgK-kXV/lib/python3.5/site-packages/tensorflow/python/framework/dtypes.py:525: FutureWarning: Passing (type, 1) or '1type' as a synonym of type is deprecated; in a future version of numpy, it will be understood as (type, (1,)) / '(1,)type'.
  _np_q[...]<jupyter_text>Robot ControlsThe robot controller is described in the robot_controler notebook. With more time, an improvement could be moving this blcok into a .py file and importing it instead.<jupyter_code>class RobotController:
    """
    The robot controller class.
    """
    def __init__(self, connection):
        motor_module = connection.modules['ev3dev2.motor']
        self.motors = [motor_module.LargeMotor('outB'), motor_module.LargeMotor('outC')]
        sensors = conn.modules['ev3dev2.sensor.lego'] 
        self.gyro = sensors.GyroSensor('in2')
        
        self.motors[0].stop_action = 'hold'
        self.motors[1].stop_action = 'coast'
        self.motors[1].stop()
        
        self.up_position_degrees = -90 # position of legs fully in front
        self.maxspeed = 80 # maximum speed of leg movement
        self.minspeed = 3 # minimum speed of leg movement
        self.angle_normalizer = 0.01
        self.rate_normalizer = 0.01
        
        
    
    def reset(self):
        """ resets the robot, performs calibration and maintainance, return when robot is reset.
        Called at the start of each episode"""
        # move legs to base position
        self._move_motors_to_pos(0, 20)
        self._wait_until_no_standstill(tolerance = 0.05)
        _, angle = self.get_rate_and_angle()
        if abs(angle) >= 0.04:
            # drifted pretty far, recalibrate
            self._wait_until_no_standstill(tolerance = 0.01)
            self._calibrate_gyro()
            return
    
    def get_rate_and_angle(self):
        """ returns the rate and angle of the robot movement"""
        angle, rate = self.gyro.angle_and_rate
        return (rate * self.rate_normalizer, angle * self.angle_normalizer)

    def get_current_leg_pos(self):
        """ returns the curent leg pos, on a [-1, 1] interval"""
        return self.motors[0].position /  self.up_position_degrees
    
    def move_legs(self, pos):
        """ instructs the robot where to move its legs to, on a [-1, 1] interval. Function returns immediatly."""
        pos = max(-1., min(pos, 1.))
        pos_now = self.get_current_leg_pos()
        
        s = abs(pos - pos_now) / 2 *  self.maxspeed
        s = max(self.minspeed, min(s,  self.maxspeed))
        
        self._move_motors_to_pos(pos, s)
        return
    
    def _move_motors_to_pos(self, pos, speed):
        """
        moves the motors to a position [-1, 1] at speed [0,100].
        """
        assert -1. <= pos <= 1.,\
                    "{} is an invalid position, must be between -1 and 1 (inclusive)".format(pos)
        assert 0. <= speed <= 100.,\
                    "{} is an invalid position, must be between -1 and 1 (inclusive)".format(pos)
        
        self.motors[0].on_to_position(speed, position=pos*self.up_position_degrees, brake=True, block=False) # moves motors, non blocking.
        # Only move one motor, the other is connected with an extra joint and doesn't move by itself
        return
            
            
    def _calibrate_gyro(self):
        print('calibrating gyro')
        self.gyro.mode='GYRO-CAL'
        time.sleep(1)
        self.gyro.mode="GYRO-ANG"
        
    def _wait_until_no_standstill(self, tolerance=0.01):
        """
        waits until the gyro acceleration does not change for 3 seconds
        """
        print("Waiting to stand still.")
        change = 9999
        while abs(change) > tolerance: 

            rates = []
            times = []
            # sample for one second
            t0 = t = time.time()
            while t - t0 < 3:
                _, rate = self.gyro.angle_and_rate
                rates.append(rate)
                t = time.time()
                times.append(t)


            # check change in accel with linreg
            rates = abs(np.array(rates)) # take absolute rates so swinging doesnt cancel each other out
            times = np.array(times)
            lr = LinearRegression()
            lr.fit(times[:, np.newaxis], rates)  
            change = lr.coef_

        return

class RobotControllerVerboseWrapper(RobotController):
    """
    Wrapper / decorator for the robot controller to add verbose logging
    for design pattern see https://github.com/faif/python-patterns/blob/master/structural/decorator.py
    """
    def __init__(self, robotController):
        self.wrapped = robotController
        self.last_move = time.time()
        
    def reset(self):
        return self.wrapped.reset()
    
    def get_rate_and_angle(self):
        return self.wrapped.get_rate_and_angle()
    
    def get_current_leg_pos(self):
        return self.wrapped.get_current_leg_pos()
    
    def move_legs(self, pos):
        print("Moving legs to {}, time since last move {}".format(pos, time.time() - self.last_move))
        self.last_move = time.time()
        return self.wrapped.move_legs(pos)

class RobotControllerExceptionWrapper(RobotController):
    """
    Wrapper / decorator for the robot controller to handle time out exceptions
    for design pattern see https://github.com/faif/python-patterns/blob/master/structural/decorator.py
    """
    def __init__(self, robotController):
        self.wrapped = robotController
        
    def reset(self):
        try:
            return self.wrapped.reset()
        except TimeoutError:
            print('TimeoutError occured! retrying...')
            time.sleep(3)
            return self.reset()
    
    def get_rate_and_angle(self):
        try:
            return self.wrapped.get_rate_and_angle()
        except TimeoutError:
            print('TimeoutError occured! retrying...')
            time.sleep(3)
            return self.get_rate_and_angle()
    
    def get_current_leg_pos(self):
        try:
            return self.wrapped.get_current_leg_pos()
        except TimeoutError:
            print('TimeoutError occured! retrying...')
            time.sleep(3)
            return self.get_current_leg_pos()
    
    def move_legs(self, pos):
        try:
            return self.wrapped.move_legs(pos)
        except TimeoutError:
            print('TimeoutError occured! retrying...')
            time.sleep(3)
            return self.move_legs(pos)<jupyter_output><empty_output><jupyter_text>SetupAs usual, connection is made to the robot, in addition, we initialize the RobotController class and define helper functions for trainable parameters.<jupyter_code>try:
    #ROBOT_HOSTNAME = "ev3dev.local"
    #ROBOT_HOSTNAME = "192.168.137.131"
    ROBOT_HOSTNAME = "10.42.0.3"
    conn = rpyc.classic.connect(ROBOT_HOSTNAME)
    conn.execute("print('Hello Slave. I am your master!')")
except:
    raise Exception('No connection to rpyc server on robot possible! Is the robot conneced? Is the rpyc server started?')
    

weights = []
timestep = []
rewards = []

rc = RobotControllerExceptionWrapper(RobotController(conn))
# Helper functions for setting the parameters of the neural network
def number_of_trainable_parameters(session):
    variables_names = [v.name for v in tf.trainable_variables()]
    values = session.run(variables_names)    
    number_of_parameters = 0
    for v in values:
        number_of_parameters += v.size
    return number_of_parameters

def set_trainable_parameters(session, parameter_vector):
    assert number_of_trainable_parameters(session) == parameter_vector.size, \
      'number of parameters do not match: %r vs. %r' % (number_of_trainable_parameters(session), parameter_vector.size)
    variables_names = [v.name for v in tf.trainable_variables()]
    values = session.run(variables_names) 
    idx = 0
    for k, v in zip(tf.trainable_variables(), values):    
        new_value = parameter_vector[idx:idx + v.size].reshape(v.shape)
        k.load(new_value, session)  # load does not add a new node to the graph
        idx += v.size<jupyter_output><empty_output><jupyter_text>Swing motion functionThis is the function we attempt to optimize. It recieves data about the current state and feeds this into a neural network (defined later) to get an action, which it performs. During this, it keeps track of the maximal absolute rate observed to be able to pass a final reward back<jupyter_code># Simple evaluation function, just considering a single (random) start state
def swing_motion(x, sess, duration=30):
    # set params to NN
    set_trainable_parameters(sess, x)
    
    # reset swing
    rc.reset()
    
    # simulate
    reward = 0
    pos = 0
    starttime = time.time()
    rate, angle = rc.get_rate_and_angle()
    while time.time()-starttime < duration :

        state = np.array([rate, angle, pos])
        
        pos = sess.run(action, {inputs: state.reshape(1,*state.shape)})[0][0]
        
        rc.move_legs(pos)
        
        rate, angle = rc.get_rate_and_angle()
        reward = max(reward, abs(rate))
    
    # log rewards of session
    print("total reward for session: ",reward)
    weights.append(x)
    timestep.append(starttime)
    rewards.append(reward)
    return -reward
# Neural network parameters
state_space_dimension = 3
number_of_actions = 1
number_of_hidden_neurons = 2 # currently unused, becomes relevant if the hidden layer is reimplemented.<jupyter_output><empty_output><jupyter_text>Neural network and CMA-ESHere, we define the policy network, which is very simple for our implementation, but with this setup it can easily be expanded to include hidden layers, or (with slightly more work) be recurrent.  Afterwards, a session is started and we run the CMA minimizer, using our guess for initial weights.<jupyter_code>tf.reset_default_graph()

# Define policy network mapping state to action
with tf.name_scope('policy_network'):
    inputs = tf.placeholder(shape=[1, state_space_dimension],dtype=tf.float32) 
    #hidden = tf.layers.dense(inputs, number_of_hidden_neurons, activation=tf.tanh, use_bias=True)
    action = tf.layers.dense(inputs, number_of_actions, activation=tf.tanh, use_bias=True)

# Do the learning
init = tf.global_variables_initializer()

with tf.Session() as sess:
    sess.run(init)
    sess.graph.finalize()  # graph is read-only after this statement
    #initial_weights = np.random.normal(0, 0.1, number_of_trainable_parameters(sess))
    # best guess for starting weights: 
    #     large weight to move in the direction of the rate
    #     smaller negative weight to move less extreme when on a far end
    #     small bias of 0.2 to start the movement
    initial_weights = np.array([10, -5, -1, 0.2])
    initial_weights = np.array([16, -2, -18, 0.2])
    res = cma.fmin(swing_motion, initial_weights, 1, {'timeout': 8 * 60**2, 'ftarget':-4.,}, args=([sess]), restart_from_best=True)
plt.plot(rewards)
plt.show()
pickle.dump({'rewards':rewards, 'times':timestep, 'weights': weights}, open('CMA_results.pickl', 'wb'))<jupyter_output><empty_output><jupyter_text>Play best solutionSince the weights from each iteration is saved, we can replay them easily.<jupyter_code>dmp = pickle.load(open('./cma_results/CMA_results_20210930.pickl', 'rb'))
weights = dmp['weights']
timestep = dmp['times']
rewards = dmp['rewards']
middle=weights[150]
middle
i = np.argmax(rewards)
x_opt = weights[i]
x_opt
# final CMA cluster center
x_final = np.array([27.6228857208, -26.4064626352, -11.7426286304, 6.24397330849])
x_middle = np.array([13.3768323511, -7.8391528124, -2.50238021705, 1.06962506962])
x_early = np.array([10.4974431805, -3.76115577714, -0.857282768428, 0.824748263562])
x_middle = np.array([14.10761362, -4.90616217, -7.07537308, 1.18769683])
x_pretty_good = np.array([16.1519993, -2.06677507, -18.2854088, 4.19943221])
x_best = np.array([ 17.39535257,  -3.89646022, -15.95160254,   3.69107796])
time.sleep(3)
x = x_pretty_good
with tf.Session() as sess:
    sess.run(init)
    sess.graph.finalize()
    try:
        swing_motion(x_pretty_good, sess, duration=120)
    except AttributeError:
        pass
    rc.reset()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.10551935894555531, 'fraction_numerical': 0.03295139668988242, 'mean_word_length': 2.969678953626635, 'pattern_counts': {'":': 0, '<': 31, '<?xml version=': 0, '>': 27, 'https://': 2, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 2, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '6', 'dir': 'jupyter-structured-clean-dedup', 'id': '253768', 'license': 'MIT', 'n_tokens_mistral': 4491, 'n_tokens_neox': 4180, 'n_words': 1125, 'path': 'src/swing_robot/CMA_Swing_Robot.ipynb', 'repo_name': 'DatalogiForAlle/LEGO-machine-learning', 'size': '13361'}