# Project 2 - Mc907/Mo651 - Mobile Robotics

### Student:
Luiz Eduardo Cartolano - RA: 183012

### Instructor:
Esther Luna Colombini

### Github Link:
[Project Repository](https://github.com/luizcartolano2/mc907-mobile-robotics)

### Youtube Link:
[Link to Video](https://youtu.be/uqNeEhWo0dA)

### Subject of this Work:
The general objective of this work is to implement and evaluate at least 1 robot control behavior per group member.

### Goals:
1. Implement and evaluate at least 1 robot control behavior per group member (AvoidObstacle, WallFollow, Go- ToGoal) using models based on PID, Fuzzy, Neural Networks, etc;
2. Propose a behavior coordination strategy (state machine, planner, AR, subsumption, etc.)

# Code Starts Here

Import of used libraries

```python
from lib import vrep
import sys, time
from src import robot as rb
from src.utils import vrep2array
import math
from time import time
import numpy as np
import cv2
import matplotlib.pyplot as plt
from sklearn.cluster import KMeans
import skfuzzy
import skfuzzy as fuzz
import skfuzzy.control as ctrl
# from reinforcement_learning.train import *
```

# Defining the kinematic model of the Pionner P3DX
For this project, we are going to use the configuration of the mobile robot being characterized by the position (x,y) and the orientation in a Cartesian coordinate.

Using the follow parameters:
1. $V_R$: linear velocity of the right wheel.
2. $V_L$: linear velocity of the left wheel.
3. $W$: angular velocity of the mobile robot.
4. $X$: abscissa of the robot.
5. $Y$: intercept of the robot.
6. $X,Y$ : the actual position coordinates.
7. $\theta$: orientation of the robot.
8. $L$: the distance between the driving wheels.

The kinematic model is given by these equations [1](https://www.hindawi.com/journals/cin/2016/9548482/abs/):

\begin{align}
\frac{dX}{dt} & = \frac{V_L + V_R}{2} \cdot cos(\theta) \\
\frac{dY}{dt} & = \frac{V_L + V_R}{2} \cdot sen(\theta) \\
\frac{d \theta}{dt} & = \frac{V_L - V_R}{2} \\
\end{align}

Where ($X$,$Y$ and $\theta$) are the robot actual position and orientation angle in world reference frame. In simulation, we use the discrete form to build a model of the robot. The discrete form of the kinematic model is given by the following equations:

\begin{align}
X_{k+1} & = X_k + T \cdot \frac{V_{lk} + V_{rk}}{2} \cdot cos(\theta_k + \frac{d \theta}{dt} ) \\
Y_{k+1} & = Y_k + T \cdot \frac{V_{lk} + V_{rk}}{2} \cdot sen(\theta_k + \frac{d \theta}{dt}) \\
\theta_{k+1} & = \theta_k + T \cdot \frac{V_{lk} + V_{rk}}{L} \\
\end{align}

where $X_{k+1}$ and $Y_{k+1}$ represent the position of the center axis of the mobile robot and $T$ is the sampling time.

```python
class Pose:
    """
    A class used to store the robot pose.

    ...

    Attributes
    ----------
    x : double
        The x position of the robot on the map
    y : double
        The y position of the robot on the map
    orientation : double
        The angle theta of the robot on the map

    Methods
    -------
    The class doesn't have any methods
    """
    def __init__(self, x=None, y=None, orientation=None):
        self.x = x
        self.y = y
        self.orientation = orientation
```

```python
class Odometry():
    """
    A class used to implement methods that allow a robot to calculate his own odometry.

    ...

    Attributes
    ----------
    robot : obj
        The robot object
    lastPose : obj Pose
        Store the robot's pose during his movement
    lastTimestamp : time
        Store the last timestamp
    left_vel : double
        Store the velocity of the left robot wheel
    right_vel : double
        Store the velocity of the right robot wheel
    delta_time : double
        Store how much time has passed
    delta_theta : double
        Store how the orientation change
    delta_space : double
        Store how the (x,y) change

    Methods
    -------
    ground_truth_updater()
        Function to update the ground truth, the real pose of the robot at the simulator
    odometry_pose_updater()
        Function to estimate the pose of the robot based on the kinematic model
    """
    def __init__(self, robot):
        self.robot = robot
        self.lastPose = None
        self.lastTimestamp = time()
        self.left_vel = 0
        self.right_vel = 0
        self.delta_time = 0
        self.delta_theta = 0
        self.delta_space = 0

    def ground_truth_updater(self):
        """
            Function to update the ground truth, the real pose of the robot at the simulator
        """
        # get the (x,y,z) position of the robot at the simulator
        pose = self.robot.get_current_position()
        # get the orientation of the robot (euler angles)
        orientation = self.robot.get_current_orientation()

        # return an pose object (x,y,theta)
        return Pose(x=pose[0], y=pose[1], orientation=orientation[2])

    def odometry_pose_updater(self):
        """
            Function to estimate the pose of the robot based on the knematic model
        """
        if self.lastPose is None:
            self.lastPose = self.ground_truth_updater()
            return self.lastPose

        # get the actual timestamp
        time_now = time()
        # get the robot linear velocity for the left and right wheel
        left_vel, right_vel = self.robot.get_linear_velocity()
        # calculate the difference between the acutal and last timestamp
        delta_time = time_now - self.lastTimestamp
        # calculate the angle deslocation - based on the kinematic model
        delta_theta = (right_vel - left_vel) * (delta_time / self.robot.ROBOT_WIDTH)
        # calculate the distance deslocation - based on the kinematic model
        delta_space = (right_vel + left_vel) * (delta_time / 2)

        # auxiliary function to sum angles
        add_deltha = lambda start, delta: (((start+delta)%(2*math.pi))-(2*math.pi)) if (((start+delta)%(2*math.pi))>math.pi) else ((start+delta)%(2*math.pi))

        # calculate the new X pose
        x = self.lastPose.x + (delta_space * math.cos(add_deltha(self.lastPose.orientation, delta_theta/2)))
        # calculate the new Y pose
        y = self.lastPose.y + (delta_space * math.sin(add_deltha(self.lastPose.orientation, delta_theta/2)))
        # calculate the new Orientation pose
        theta = add_deltha(self.lastPose.orientation, delta_theta)

        # uptade the state of the class
        self.lastPose = Pose(x, y, theta)
        self.lastTimestamp = time_now
        self.left_vel = left_vel
        self.right_vel = right_vel
        self.delta_time = delta_time
        self.delta_theta = delta_theta
        self.delta_space = delta_space

        return self.lastPose
```

# Defining the class that controls the robot walker

For this project we are going to use two different controllers in order to make the robot avoid obstacles in the map. The first one is a classical fuzzy based system, and the second one, is a more modern approach, based on artificial intelligence, called reinforcement learning.

### Controllers:
**1. Fuzzy**

   Fuzzy logic is a very common technique in the Artificial Intelligence branch. It is a method that introduces the concept of partially correct and / or wrong, different from Boolean logic, in which only binary values are allowed. This fact allows generalist logic in which it is not necessary to deal with all possible cases, ideal for applications with limited memory and / or time.

   A fuzzy control system is a control system based on fuzzy logic—a mathematical system that analyzes analog input values in terms of logical variables that take on continuous values between 0 and 1, in contrast to classical or digital logic, which operates on discrete values of either 1 or 0 (true or false, respectively).

   The input variables in a fuzzy control system are in general mapped by sets of membership functions similar to this, known as "fuzzy sets". The process of converting a crisp input value to a fuzzy value is called "fuzzification". A control system may also have various types of switch, or "ON-OFF", inputs along with its analog inputs, and such switch inputs of course will always have a truth value equal to either 1 or 0, but the scheme can deal with them as simplified fuzzy functions that happen to be either one value or another. Given "mappings" of input variables into membership functions and truth values, the microcontroller then makes decisions for what action to take, based on a set of "rules", each of the form:
~~~
    IF brake temperature IS warm AND speed IS not very fast
    THEN brake pressure IS slightly decreased.
~~~

   For this project the implemented fuzzy was very simples, and aims just to make the robot abble to avoid obstacles on his way. He uses the ultrassonic sensors for his three inputs (front, left and right distance) and outputs the linear velocity for both weels.

   Some fundamental concepts to understand logic are:

1. **Degree of Relevance:** value in the range $[0,1]$ that determines the degree to which a given element belongs to a set, allowing a gradual transition from falsehood to truth.
2. **Fuzzy Set:** A set A in X is expressed as a set of ordered pairs: $A = {{(x, \mu_A (X)) | x \in X }}$.
3. **Fuzzy Rules:** are created to evaluate the antecedent (input) and apply the result to the consequent (output). They are partially activated depending on the antecedent.
4. **Fuzzy Steps:**
    1. *Fuzification:* stage in which subjective linguistic variables and pertinence functions are defined.
    2. *Inference:* stage at which rules are defined and evaluated.
    3. *Defuzification:* step in which the resulting regions are converted to values for the system output variable. The best known methods of defuzification are: centroid, bisector, lowest of maximum (SOM), middle of maximum (MOM), highest of maximum (LOM).

Now, we are given a more detailed explanation about the implemented system, showing how we model the inputs, outputs, system rules and the defuzzify methods.

1. **Inputs and Outputs:** For this fuzzy we use three antecedents (all of them has the same shape of the one show bellow) and two consequents. The antecedents works mapping the left, right and front sensors of the ultrassonic sensors of the robot. As we can see, inputs are divide in three sets, low, medium and high distances that aims to tell the system how far he is from some object in the map. The consequents, by the other side, aims to mapping the velocity of both wheels of the robot, they are split in four velocities.
    Fuzzy Antecedent        |  Fuzzy Consequent
    :-------------------------:|:-------------------------:
      |

2. **Rules:** The system is implemented using eleven rules, that we enough to make the robot able to escape from obstacles from the map with a stable control. The rules can be describe as follow:
~~~
        left['low'] AND right['low'] AND (front['medium'] OR front['far'])),
        output_left['low'], output_right['low']

        left['low'] AND right['low'] AND front['low'],
        output_left['reverse'], output_right['low']

        left['medium'] OR left['far'] AND right['low'] AND front['low'],
        output_left['low'], output_right['high']

        left['medium'] OR left['far'] AND right['low'] AND front['medium'] OR front['far'],
        output_left['low'], output_right['high']

        left['far'] AND right['medium'] AND front['low'],
        output_left['low'], output_right['high']

        left['far'] AND right['far'] AND front['low'],
        output_left['high'], output_right['low']

        left['medium'] AND right['medium'] AND front['low'],
        output_left['high'], output_right['low']

        left['medium'] AND right['far'] AND front['low'],
        output_left['high'], output_right['low']

        left['low'] AND right['medium'] OR right['far'] AND front['low'],
        output_left['high'], output_right['low']

        left['low'] AND right['medium'] OR right['far'] AND front['medium'] OR front['far'],
        output_left['high'], output_right['low']

        left['medium'] OR left['far'] AND right['medium'] OR right['far'] AND front['medium'] OR front['far'],
        output_left['medium'], output_right['medium']
~~~

3. **Defuzzification:** In order to understand how the choosen defuzzification afects the controller we test two different models, the minimum of the maximuns (SOM) and maximum of the maximuns(LOM), the consequences of this approach will be commented at the Results section.

```python
class FuzzyControler():
    """
    A class used to implement methods that allow a robot to walk, based on a fuzzy logic controller.

    ...

    Attributes
    ----------
    forward: skfuzzy object
        Skfuzzy input object
    left: skfuzzy object
        Skfuzzy input object
    right: skfuzzy object
        Skfuzzy input object
    output_left: skfuzzy object
        Skfuzzy output object
    output_right: skfuzzy object
        Skfuzzy output object
    rules: skfuzzy object
        List of rules to the fuzzy
    control: skfuzzy object
        Skfuzzy controller object
    simulator: skfuzzy object
        Skfuzzy simulator object

    Methods:

    -------
    create_inputs()
        Function to create skfuzzy input functions
    create_outputs()
        Function to create skfuzzy output functions
    create_rules()
        Function to create skfuzzy rules
    create_control()
        Function to create skfuzzy controller
    show_fuzzy()
        Function to show the fuzzy rules as a graph
    create_simulator()
        Function that controls the fuzzy pipeline
    simulate()
        Function that give outputs velocity based on input distance
    """

    def __init__(self, behavior):
        self.front = None
        self.left = None
        self.right = None
        self.output_left = None
        self.output_right = None
        self.rules = []
        self.control = None
        self.simulator = None
        self.behavior = behavior

    def create_inputs(self):
        # set the variable universe as near, medium and far
        self.front = ctrl.Antecedent(np.arange(0, 5.01, 0.01), 'front')
        self.front['low'] = fuzz.trapmf(self.front.universe, [0, 0, 0.6, 1])
        self.front['medium'] = fuzz.trimf(self.front.universe, [0.6, 1, 1.4])
        self.front['far'] = fuzz.trapmf(self.front.universe, [1, 1.5, 5, 5])

        self.left = ctrl.Antecedent(np.arange(0, 5.01, 0.01), 'left')
        self.left['low'] = fuzz.trapmf(self.left.universe, [0, 0, 0.6, 1])
        self.left['medium'] = fuzz.trimf(self.left.universe, [0.6, 1, 1.4])
        self.left['far'] = fuzz.trapmf(self.left.universe, [1, 1.5, 5, 5])

        self.right = ctrl.Antecedent(np.arange(0, 5.01, 0.01), 'right')
        self.right['low'] = fuzz.trapmf(self.right.universe, [0, 0, 0.6, 1])
        self.right['medium'] = fuzz.trimf(self.right.universe, [0.6, 1, 1.4])
        self.right['far'] = fuzz.trapmf(self.right.universe, [1, 1.5, 5, 5])

        return

    def create_outputs(self):

        self.output_left = ctrl.Consequent(np.arange(-1, 2.01, 0.1), 'output_left')
        self.output_left['reverse'] = fuzz.trapmf(self.output_left.universe, [-1,-1, 0, 0.2])
        self.output_left['low'] = fuzz.trimf(self.output_left.universe, [0,1, 1.3])
        self.output_left['medium'] = fuzz.trimf(self.output_left.universe, [1,1.5, 1.75])
        self.output_left['high'] = fuzz.trimf(self.output_left.universe, [1.2,1.8, 2])
        self.output_left.defuzzify_method = 'lom'

        self.output_right = ctrl.Consequent(np.arange(-1, 2.01, 0.1), 'output_right')
        self.output_right['reverse'] = fuzz.trapmf(self.output_left.universe, [-1,-1, 0, 0.2])
        self.output_right['low'] = fuzz.trimf(self.output_left.universe, [0,1, 1.3])
        self.output_right['medium'] = fuzz.trimf(self.output_left.universe, [1,1.5, 1.75])
        self.output_right['high'] = fuzz.trimf(self.output_left.universe, [1.2,1.8, 2])
        self.output_right.defuzzify_method = 'lom'

        return

    def create_rules(self, front, left, right, output_left, output_right):

        rule1 = ctrl.Rule(antecedent=(left['low'] & right['low'] & (front['medium'] | front['far'])),
              consequent=(output_left['low'], output_right['low']))

        rule2 = ctrl.Rule(antecedent=(left['low'] & right['low'] & front['low']),
              consequent=(output_left['reverse'], output_right['low']))

        rule3 = ctrl.Rule(antecedent=((left['medium'] | left['far']) & right['low'] & front['low']),
              consequent=(output_left['low'], output_right['high']))

        rule4 = ctrl.Rule(antecedent=((left['medium'] | left['far']) & right['low'] & (front['medium'] | front['far'])),
              consequent=(output_left['low'], output_right['high']))

        rule5 = ctrl.Rule(antecedent=(left['far'] & right['medium'] & front['low']),
              consequent=(output_left['low'], output_right['high']))

        rule6 = ctrl.Rule(antecedent=(left['far'] & right['far'] & front['low']),
              consequent=(output_left['high'], output_right['low']))

        rule7 = ctrl.Rule(antecedent=(left['medium'] & right['medium'] & front['low']),
              consequent=(output_left['high'], output_right['low']))

        rule8 = ctrl.Rule(antecedent=(left['medium'] & right['far'] & front['low']),
              consequent=(output_left['high'], output_right['low']))

        rule9 = ctrl.Rule(antecedent=(left['low'] & (right['medium'] | right['far']) & front['low']),
              consequent=(output_left['high'], output_right['low']))

        rule10 = ctrl.Rule(antecedent=(left['low'] & (right['medium'] | right['far']) & (front['medium'] | front['far'])),
              consequent=(output_left['high'], output_right['low']))

        rule11 = ctrl.Rule(antecedent=((left['medium'] | left['far']) & (right['medium'] | right['far']) & (front['medium'] | front['far'])),
              consequent=(output_left['medium'], output_right['medium']))

        for i in range(1, 12):
            self.rules.append(eval("rule" + str(i)))

        return

    def create_control(self):
        # call function to create robot input
        self.create_inputs()
        # call function to create robot output
        self.create_outputs()
        if self.behavior == "avoid_obstacle":
            # call function to create rules
            self.create_rules(self.front, self.left, self.right, self.output_left, self.output_right)
            # create controller
            self.control = skfuzzy.control.ControlSystem(self.rules)

        return

    def show_fuzzy(self):
        if self.control is None:
            raise Exception("Control not created yet!")
        else:
            self.control.view()

        return

    def create_simulator(self):
        if self.control is None:
            # crete controller if it doensn't exist
            self.create_control()

        # create simulator object
        self.simulator = ctrl.ControlSystemSimulation(self.control)

        return

    def simulate(self, input_foward=None, input_left=None, input_right=None):
        if self.simulator is None:
            # crete simulator if it doensn't exist
            self.create_simulator()

        # if there is no input raise exception
        if input_foward is None or input_left is None or input_right is None:
            raise Exception("Inputs can't be none")

        # simulte the robot linear velocity based on given inputs
        self.simulator.input['front'] = input_foward
        self.simulator.input['left'] = input_left
        self.simulator.input['right'] = input_right

        self.simulator.compute()

        return self.simulator.output['output_left'], self.simulator.output['output_right']
```

**2. Reinforcement Learning**

Reinforcement learning is an area of Machine Learning. Reinforcement. It is about taking suitable action to maximize reward in a particular situation. Reinforcement learning differs from the supervised learning in a way that in supervised learning the training data has the answer key with it so the model is trained with the correct answer itself whereas in reinforcement learning, there is no answer but the reinforcement agent decides what to do to perform the given task. In the absence of training dataset, it is bound to learn from its experience.

**Main points in Reinforcement learning:**

1. Input: The input should be an initial state from which the model will start
2. Output: There are many possible output as there are variety of solution to a particular problem
3. Training: The training is based upon the input, The model will return a state and the user will decide to reward or punish the model based on its output.
4. The model keeps continues to learn.
5. The best solution is decided based on the maximum reward.

**Types of Reinforcement:**

1. Positive - is defined as when an event, occurs due to a particular behavior, increases the strength and the frequency of the behavior.
2. Negative - is defined as strengthening of a behavior because a negative condition is stopped or avoided.

One of the most common RL algorithms is the **Q-Learning**, a basic form which uses Q-values (also called action values) to iteratively improve the behavior of the learning agent. A brief introduction can be done with the following informations:

1. **Q-Values:** Q-values are defined for states and actions. $Q(S, A)$ is an estimation of how good is it to take the action A at the state S.
2. **Rewards and Episodes:** an agent over the course of its lifetime starts from a start state, makes a number of transitions from its current state to a next state based on its choice of action and also the environment the agent is interacting in. At every step of transition, the agent from a state takes an action, observes a reward from the environment, and then transits to another state.
3. **TD-Update:** the Temporal Difference or TD-Update rule can be represented as follows: $Q(S,A) = Q(S,A) + \alpha \cdot (R + \gamma \cdot (Q',S') - Q(S,A))$. Where the variables can be described as follow:
    1. S: current state
    2. A: current action
    3. S': next state
    4. A': next action
    5. R: curren reward
    6. $\gamma$: discounting factor for future rewards
    7. $\alpha$: learning rate
4. **Choosing the Action:** the policy for choosing an action is very simple. It goes as follows :
    1. with probability $(1-\epsilon)$ choose the action which has the highest Q-value
    2. with probability $(\epsilon)$ choose any action at random

In order to implement the behavior for this project we need to make two main components, the enviroment (who communicate with the V-Rep interface) and the training function that implements the policy and the Q-Learning algorithm, the last one was done based on [this](https://www.geeksforgeeks.org/q-learning-in-python/) implementation.

Now, we are going to explain, with more details the enviroment and training implementations, both codes can be found [here](https://github.com/luizcartolano2/mc907-mobile-robotics/blob/project2/reinforcement_learning/environment.py) and [here](https://github.com/luizcartolano2/mc907-mobile-robotics/blob/project2/reinforcement_learning/train.py), and a function call can be seen bellow.

**Enviroment Implementation:**
The enviroment, on a Q-Learning situation, must have the hability to start and restart the simulation, to inform states and, more important, to take actions and identify the consequences (reward) of that actions.
1. **State:**
    Since we aim to create a reinforcement behavior that "teaches" the robot how to avoid obstacles in the scene, we simply choose as a state the ultrassonic sensors observations, that is, the read distance in all of the robot sensors, what was implemented as follow:
~~~
    observations = {}
    observations['proxy_sensor'] = [np.array(self.read_ultrassonic_sensors())]
~~~

2. **Actions:**
    The robot actions were limited to three options:
    1. Walk straight: $[1.5,1.5]$.
    2. Turn Left: $[0.5,1.5]$.
    3. Turn Right: $[1.5,0.5]$.

3. **Reset Function:**
    In order to training the model the code have to be able to restart the simulation at the start of every episode, in order to make it along the V-Rep simulator, it was necessary to implement a restart function, that stop the simultion and start it again, between both actions a delay is required in order to make sure the older simulation were completely killed. Also, the reset function has to return to the training function the intial state of the simulation.
    This was done with the following lines of code:
~~~
    stop = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_blocking)
    time.sleep(5)
    start = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
    observations = {}
    observations['proxy_sensor'] = [np.array(self.read_ultrassonic_sensors())]
~~~

4. **Rewards model:**
    The robot rewards were given based on the following situations:
    1. Punishment to be close to objects:
        ``` (np.array(observations['proxy_sensor']) < 0.7).sum() * -2 ```
    2. Punishment to be very close to objects:
        ``` (np.array(observations['proxy_sensor']) < 0.2).sum() * -10 ```
    3. Rewarded for movement:
        ``` np.clip(np.sum(np.absolute(action)) * 2, 0, 2) ```
    4. Reward for walking:
        ``` if dist > 0.1: reward['proxy_sensor'] += 50 ```
    5. Punishment for dying:
        ``` if np.any(np.array(observations['proxy_sensor']) < 0.1): reward['proxy_sensor'] -= 100000 ```

5. **Step Update Function:**
    The step/update function receive an action as input, perform and evalute it. At the end, it checks if the episode is done (the robot colide with something). Due to V-Rep problemns at implementation, we consider that, if any of the sensors were ten centimeters or less from an object, the robot is considered as "dead".

**Q-Learning Implementation:**
We can split the Q-Learning part in three main functionalities, as we follow explain:
1. **Action Policy:**
    The action policy is the function to created to updade the action probabilities, it works as follow:
    ~~~
        action_probabilities = np.ones(num_actions,dtype=float) * epsilon / num_actions
        best_action = np.argmax(Q[state])
        action_probabilities[best_action] += (1.0 - epsilon)
    ~~~

2. **Q-Learning Loop:**
    The Q-Leanrning function iterate over the episodes, choosing actions based on their probabilities and updating the probabilities based on the TD-Rule and the state reward.

3. **Save/Load model:**
    In order to save the model over many iterations and be able to keep improving the quality of the controller, at the end of every simulation, we create save the state/probability dictionary at a text file and, at the start of every simulation, we load that values into the dictionary.

#### Observation
Once that the reinforcement behavior were a test we develop it in a separate branch and using .py files, not a Jupyter Notebook, so here we are just going to show how to call the Q-Learning function, and to present the obtained results in a following section. Links to the implemented files are available to.

```python
# create the simulation enviroment
env = Robot()

# calls the Q-Learning function
Q, stats = qLearning(env, 500)

# save the learned model
with open('model.txt', 'w') as f:
    json.dump({str(k): str(tuple(v)) for k, v in Q.items()}, f)

# plot few results charts
plotting.plot_episode_stats(stats)
```

# Controls robot actions

A state machine, is a mathematical model of computation. It is an abstract machine that can be in exactly one of a finite number of states at any given time. The state machine can change from one state to another in response to some external inputs and/or a condition is satisfied, the change from one state to another is called a transition.

For this project, we implemented two behaviors with same goal, to avoid obstacles, but, the one who uses reinforcement learning were tested in a separate situation, so he isn't implemented at the state machine. The one that is implemented, the fuzzy controller, has two simple states, the one before he is initialized, and the one his working. For the firts stage we create all the used objects and transit to next stage. At the next stage, we read the sensors inputs, call the fuzzy simulator in order to get the outputs and change the robot velocities.

The fuzzy controller class has his own state machine implemented in order to create all the need behaviors, that is, it checks if all conditions are satisfied and, if not, create the antecedents, consequents, rules and put all together in a controller object, and then, it make the simulations.

```python
def state_machine(behavior="avoid_obstacle"):
    # stage
    stage = 0

    if behavior == "follow_wall":
        raise Exception("Not implemented!")
    elif behavior == "avoid_obstacle":
        while True:
            if stage == 0:
                # first we create the robot and the walker object
                robot = rb.Robot()
                fuzzy = FuzzyControler(behavior=behavior)

                # instantiate the odometry calculator
                odometry_calculator = Odometry(robot=robot)

                stage = 1
            if stage == 1:
                sensors = robot.read_ultrassonic_sensors()
                front_sensors = min(sensors[3], sensors[4])
                left_sensors = min(sensors[0], sensors[1], sensors[2])
                right_sensors = min(sensors[5], sensors[6], sensors[7])

                left_vel, right_vel = fuzzy.simulate(input_foward=front_sensors, input_left=left_sensors, input_right=right_sensors)

                robot.set_left_velocity(left_vel)
                robot.set_right_velocity(right_vel)

    else:
        raise Exception("Not implemented!")

```

# Main function - Execute the code here!
Here is a simple signal handler implement in order to make the simulator execution last for a given time period.

```python
import signal
from contextlib import contextmanager

class TimeoutException(Exception): pass

@contextmanager
def time_limit(seconds):
    def signal_handler(signum, frame):
        raise TimeoutException("Timed out!")
    signal.signal(signal.SIGALRM, signal_handler)
    signal.alarm(seconds)
    try:
        yield
    finally:
        signal.alarm(0)

try:
    ground_truth = []
    odometry = []
    lines = []
    corners = []
    points_kmeans = []
    with time_limit(90):
        state_machine()
except TimeoutException as e:
    print("Timed out!")
```

    Connected to remoteApi server.
     Pioneer_p3dx_ultrasonicSensor1 connected.
     Pioneer_p3dx_ultrasonicSensor2 connected.
     Pioneer_p3dx_ultrasonicSensor3 connected.
     Pioneer_p3dx_ultrasonicSensor4 connected.
     Pioneer_p3dx_ultrasonicSensor5 connected.
     Pioneer_p3dx_ultrasonicSensor6 connected.
     Pioneer_p3dx_ultrasonicSensor7 connected.
     Pioneer_p3dx_ultrasonicSensor8 connected.
     Pioneer_p3dx_ultrasonicSensor9 connected.
     Pioneer_p3dx_ultrasonicSensor10 connected.
     Pioneer_p3dx_ultrasonicSensor11 connected.
     Pioneer_p3dx_ultrasonicSensor12 connected.
     Pioneer_p3dx_ultrasonicSensor13 connected.
     Pioneer_p3dx_ultrasonicSensor14 connected.
     Pioneer_p3dx_ultrasonicSensor15 connected.
     Pioneer_p3dx_ultrasonicSensor16 connected.
     Vision sensor connected.
     Laser connected.
     Left motor connected.
     Right motor connected.
     Robot connected.
    Timed out!

# Results
In order to show the obtained results we are going to demonstrate a video with the running simulation for both behaviors. For the fuzzy system we are going to evaluate it beyond some different start pose situations and changing the input/output universe. The reinforcement learning, by the other side, has changes only at the enviroment, also, we are going to show how it performed as the episodes were growing.

An important observation we want to do respect about the topic that ask to evaluate the model using the ground truth position and the odometry one, since both controllers aims to avoid obstacles in the scene, and that, both of them have a react behavior, none of them uses the pose as input for the behavior, so it doesn't make any difference for the obtained results.

The robot can be seen in action at [youtube](https://youtu.be/uqNeEhWo0dA). And the video behavior will be following commented. Before go into the obtained results we are going to first explain how the tests were done. For both the fuzzy controller and the reinforcement learning.

### Fuzzy Controller

In order to test and understand the fuzzy behavior in a range of different situations, we create five different scenarios for it and test each of them in three different start poses at the scene. The start poses are show in the follow images:

First Pose       |  Second Pose       |  Third Pose
:-------------------------:|:-------------------------:|:-------------------------:
  |   |

Beyond the different starting we change, for each experiment, or the shape/range of the fuzzy antecedents or the deffuzitication method. Each experiment can be described as follow:

1. **Experiment 1:**
    For the first experiment the fuzzy set were the default one, and the deffuzitication method was the smallest of the maximuns (SOM).

2. **Experiment 2:**
    For the second one, we expand the distance considered as "close" and still use the the smallest of the maximuns (SOM).

3. **Experiment 3:**
    For the third one, we expand the distance considered as "medium" and still use the the smallest of the maximuns (SOM).

4. **Experiment 4:**
    For the fourth one, we expand the distance considered as "medium", reduce the considered as "far" and still use the the smallest of the maximuns (SOM).

5. **Experiment 5:**
    For the last one, we keep the default antecedent and change the deffuzification method for the largest of the maximuns (LOM).

The antecedent sets are show here:

Experiment 1       |  Experiment 2      |  Experiment 3      |  Experiment 4      |  Experiment 5
:-------------------------:|:-------------------------:|:-------------------------:|:-------------------------:|:-------------------------:
  |   |      |      |

As we can observe in the [video](https://youtu.be/uqNeEhWo0dA), there isn't significant difference between the experiments 1, 3 e 4. The bigger ones happens when we change the "close" range of the input and, mainly, when we change the defuzzification method.

We can say that the experiment 1 show a robust and stable controller, where the robot is able to escape from obstacles with consistency and without taking major risks, besides that, he also walks with a good speed and do smooth movements.

For the second experiments we experience the worst fuzzy results. The robot achieves his goal of don't collide with other obstacles, but, he has sudden movements and once he considers that a lot of obstacles are close to him, a lot of bad decisions are taking, including moments when it basically turns on its own axis.

For the third experiment we have a closer situation for the first one, except for one aspect, he walks to close to the scene objects in a lot of situations.

The fourt and fifth experiments exhibit similar behaviors, they are consistent on their actions, don't take big risks, but they move to slow.

### Reinforcement Learning

The reinforcement learning was test on only one scenario, the same of Experiment 1. We achieve pretty good results, considering the lack of training time. After about 2500 episodes the robot were able, as we can observe in the video, to avoid the first obstacle. However, better results can be longed for, once that he doesn't survive a lot of time at the enviroment.

The scenario, and few charts describing the results are displayed below:

Enviroment       |  Episode Length       |  Episode Reward
:-------------------------:|:-------------------------:|:-------------------------:
  |   |

As we expect, lenght and reward grow as the time passed, showing that we are on the right path.

# Discussion

Both controllers achieved satisfactory results, as expected at the start of the development.

### Fuzzy Controller

The fuzzy one, turned out to be a very good option when we talk about a reactive behavior, the implementation was quite easy and the results were satisfactory. Analysing the experiments we also could extract interesting insights. For example, we realize that increase the ```close``` or the ```medium``` intervals of the antecedents sets makes the robots moves less soft and he starts to take more risk and lowest consistent actions.

By the other side, consider less objects in a ```far``` distance and take the maximum of the maximuns as the defuzzification methods both made the robot movements more conservative, walking at a much lower speed. That behavior can be easily explained for both situations. For the first one, the robot starts to "imagine" that objects are close to him than he expects, so he acts more carefully. For the second one, once we start to defuzzify our set taking the maximum value, any sensor who make a ```low``` or ```medium``` read will overlap the other ones, and the robot will, again, act carefully.

Lastly, we decide that the first configuration (Experiment 1) of antecedents and defuzzification were the best one, and set it as the 'defaul' one. The choice was based on the robot's consistency and robustness. That results makes a lot of sense when we analyse how the controller were structered in the antecedent/defuzzication mix. That is, the antecedents were created in a way that the values tend to overlap so decisions aren't abrupt. Besides that, the defuzzification is a bit more confident, so, tends to higher speeds.

### Reinforcement Learning

The reinforcement approach showed to be an amazing approach for hard to note problems using artificial intelligence. As we saw on the charts of lenght and reward over the episodes, the idea of maximizing the reward obtained makes the robot learn with his mistakes, that is, the bad action take at some state. We didn't have time to train the robot over different scenarios, so he still not able, yet, to stay alive for a very long time in the enviroment. But, he can avoid some obstacles.

The rewards were modeled in a way that be close to an object has a negative return, a punishment. Also, make a move at the scene, walk, give a positive reward, a prize. By the end, the worst punish is die. Nevertheless, we believe that the way the state was defined wasn't the best possible and, possibly, is one of the reasons the results were not spectacular. An additional improvement that can be made in order to achieve better results is change the state for it to map only the front, left and right ultrassonic sensors.

Another problem faced was to determine when the robot had hit an object on the scene. The decision was check all the ultrassonic sensors and if any of then were ten centimeters or less from an object, we consider a hit. The decision fulfilled what was expected, but had some situations where wasn't clear if it really had hit something.

# Conclusions And Final Observations

In general, the work presented satisfactory results, especially considering that the objective of this work was to introduce the students to the concepts of fuzzy logic applies to robotics, being able to understand its operation and having the ability to evaluate solutions and know how to modify them for optimal results. It is also noteworthy that the results obtained by applying the solution to the proposed problem were significantly positive. Also, that was the first time ever working with reinforcement learning, that also presents very good results and it was really fun to work with.

Weaknesses of the work, which need to be improved in future iterations, are mostly related to how the fuzification criteria were defined. An improvement thought for the project is the use of genetic algorithms to choose the best ways to create the membership functions and to choose the fuzification method. For the reinforcement learning approach, we need to make a better model of the ```state-reward``` relation.


# Time-varying Convex Optimization

This notebook will provide implementation and examples from the paper [Time-varying Convex Optimization](https://arxiv.org/abs/1808.03994), Amir Ali Ahmadi and Bachir El Khadir, 2018.

*  bachir009@gmail.com
*  sindhwani@google.com

#### Copyright 2018 Google LLC.
Licensed under the Apache License, Version 2.0 (the "License");

```
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
```

>[Time-varying Convex Optimization](#scrollTo=cgvP6mUf5WJs)

>>>>[Copyright 2018 Google LLC.](#scrollTo=qDTiddF1Q8Iu)

>>[Install Dependencies](#scrollTo=_xLiNJfmORvW)

>>[Time Varying Semi-definite Programs](#scrollTo=6PuweE1NO-sZ)

>>[Some Polynomial Tools](#scrollTo=27St0x2TO7Eu)

>>[Examples: To Add.](#scrollTo=enYVtJrS5mCw)

## Install Dependencies

```
!pip install cvxpy
!pip install sympy

```

    Collecting cvxpy
      Using cached https://files.pythonhosted.org/packages/76/3c/4314c56be5b069f4d542046912d503a07c96b42c0b075ef0e32b48f8579f/cvxpy-1.0.10.tar.gz
    Collecting osqp (from cvxpy)
      Using cached https://files.pythonhosted.org/packages/43/f2/bbeb83c0da6fd89a6d835b98d85ec76c04f39a476c065e3c99b6b709c493/osqp-0.4.1-cp36-cp36m-manylinux1_x86_64.whl
    Collecting ecos>=2 (from cvxpy)
      Using cached https://files.pythonhosted.org/packages/b6/b4/988b15513b13e8ea2eac65e97d84221ac515a735a93f046e2a2a3d7863fc/ecos-2.0.5.tar.gz
    Collecting scs>=1.1.3 (from cvxpy)
      Using cached https://files.pythonhosted.org/packages/b3/fd/6e01c4f4a69fcc6c3db130ba55572089e78e77ea8c0921a679f9da1ec04c/scs-2.0.2.tar.gz
    Collecting multiprocess (from cvxpy)
      Using cached https://files.pythonhosted.org/packages/7a/ee/b9bf3e171f936743758ef924622d8dd00516c5532b00a1210a09bce68325/multiprocess-0.70.6.1.tar.gz
    Collecting fastcache (from cvxpy)
      Using cached https://files.pythonhosted.org/packages/fb/98/93f2d36738868e8dd5a8dbfc918169b24658f63e5fa041fe000c22ae4f8b/fastcache-1.0.2.tar.gz
    Requirement already satisfied: six in /usr/local/lib/python3.6/dist-packages (from cvxpy) (1.11.0)
    Requirement already satisfied: toolz in /usr/local/lib/python3.6/dist-packages (from cvxpy) (0.9.0)
    Requirement already satisfied: numpy>=1.14 in /usr/local/lib/python3.6/dist-packages (from cvxpy) (1.14.6)
    Requirement already satisfied: scipy>=0.19 in /usr/local/lib/python3.6/dist-packages (from cvxpy) (1.1.0)
    Requirement already satisfied: future in /usr/local/lib/python3.6/dist-packages (from osqp->cvxpy) (0.16.0)
    Requirement already satisfied: dill>=0.2.8.1 in /usr/local/lib/python3.6/dist-packages (from multiprocess->cvxpy) (0.2.8.2)
    Building wheels for collected packages: cvxpy, ecos, scs, multiprocess, fastcache
      Running setup.py bdist_wheel for cvxpy ... - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / - done
      Stored in directory: /root/.cache/pip/wheels/8b/af/aa/46570716431521ee92085f317c33b2f427e27f08fe4a8a738a
      Running setup.py bdist_wheel for ecos ... - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / done
      Stored in directory: /root/.cache/pip/wheels/50/91/1b/568de3c087b3399b03d130e71b1fd048ec072c45f72b6b6e9a
      Running setup.py bdist_wheel for scs ... - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / - \ | / - done
      Stored in directory: /root/.cache/pip/wheels/ff/f0/aa/530ccd478d7d9900b4e9ef5bc5a39e895ce110bed3d3ac653e
      Running setup.py bdist_wheel for multiprocess ... - \ | / done
      Stored in directory: /root/.cache/pip/wheels/8b/36/e5/96614ab62baf927e9bc06889ea794a8e87552b84bb6bf65e3e
      Running setup.py bdist_wheel for fastcache ... - \ done
      Stored in directory: /root/.cache/pip/wheels/b7/90/c0/da92ac52d188d9ebca577044e89a14d0e6ff333c1bcd1ebc14
    Successfully built cvxpy ecos scs multiprocess fastcache
    Installing collected packages: osqp, ecos, scs, multiprocess, fastcache, cvxpy
    Successfully installed cvxpy-1.0.10 ecos-2.0.5 fastcache-1.0.2 multiprocess-0.70.6.1 osqp-0.4.1 scs-2.0.2
    Requirement already satisfied: sympy in /usr/local/lib/python3.6/dist-packages (1.1.1)
    Requirement already satisfied: mpmath>=0.19 in /usr/local/lib/python3.6/dist-packages (from sympy) (1.0.0)

```
import numpy as np
import scipy as sp
```

## Time Varying Semi-definite Programs

The TV-SDP framework for CVXPY for imposing constraints of the form:

$$A(t) \succeq 0 \; \forall t \in [0, 1],$$

where $$A(t)$$ is a polynomial symmetric matrix, i.e. a symmetric matrix
whose entries are polynomial functions of time, and $$A(t) \succeq 0$$
means that all the eigen values of the matrix $$A(t)$$ are nonnegative.

```
def _mult_poly_matrix_poly(p, mat_y):
  """Multiplies the polynomial matrix mat_y by the polynomial p entry-wise.

  Args:
    p: list of size d1+1 representation the polynomial sum p[i] t^i.
    mat_y: (m, m, d2+1) tensor representing a polynomial
      matrix Y_ij(t) = sum mat_y[i, j, k] t^k.

  Returns:
    (m, m, d1+d2+1) tensor representing the polynomial matrix p(t)*Y(t).
  """

  mult_op = lambda q: np.convolve(p, q)
  p_times_y = np.apply_along_axis(mult_op, 2, mat_y)
  return p_times_y

def _make_zero(p):
  """Returns the constraints p_i == 0.

  Args:
    p: list of cvxpy expressions.

  Returns:
    A list of cvxpy constraints [pi == 0 for pi in p].
  """

  return [pi == 0 for pi in p]

def _lambda(m, d, Q):
  """Returns the mxm polynomial matrix of degree d whose Gram matrix is Q.

  Args:
    m: size of the polynomial matrix to be returned.
    d: degreen of the polynomial matrix to be returned.
    Q: (m*d/2, m*d/2)  gram matrix of the polynomial matrix to be returned.

  Returns:
    (m, m, d+1) tensor representing the polynomial whose gram matrix is Q.
    i.e. $$Y_ij(t) == sum_{r, s s.t. r+s == k}  Q_{y_i t^r, y_j t^s} t^k$$.
  """

  d_2 = int(d / 2)

  def y_i_j(i, j):
    poly = list(np.zeros((d + 1, 1)))
    for k in range(d_2 + 1):
      for l in range(d_2 + 1):
        poly[k + l] += Q[i + k * m, j + l * m]
    return poly

  mat_y = [[y_i_j(i, j) for j in range(m)] for i in range(m)]
  mat_y = np.array(mat_y)
  return mat_y

def _alpha(m, d, Q):
  """Returns t*Lambda(Q) if d odd, Lambda(Q) o.w.

  Args:
    m: size of the polynomial matrix to be returned.
    d: degreen of the polynomial matrix to be returned.
    Q: gram matrix of the polynomial matrix.

  Returns:
    t*Lambda(Q) if d odd, Lambda(Q) o.w.
  """

  if d % 2 == 1:
    w1 = np.array([0, 1])  # t
  else:
    w1 = np.array([1])  # 1
  mat_y = _lambda(m, d + 1 - len(w1), Q)
  return _mult_poly_matrix_poly(w1, mat_y)

def _beta(m, d, Q):
  """Returns (1-t)*Lambda(Q) if d odd, t(1-t)*Lambda(Q) o.w.

  Args:
    m: size of the polynomial matrix to be returned.
    d: degreen of the polynomial matrix to be returned.
    Q: gram matrix of the polynomial matrix.

  Returns:
    (1-t)*Lambda(Q) if d odd, t(1-t)*Lambda(Q) o.w.
  """

  if d % 2 == 1:
    w2 = np.array([1, -1])  # 1 - t
  else:
    w2 = np.array([0, 1, -1])  # t - t^2
  mat_y = _lambda(m, d + 1 - len(w2), Q)
  return _mult_poly_matrix_poly(w2, mat_y)

def make_poly_matrix_psd_on_0_1(mat_x):
  """Returns the constraint X(t) psd on [0, 1].

  Args:
    mat_x: (m, m, d+1) tensor representing a mxm polynomial matrix of degree d.

  Returns:
    A list of cvxpy constraints imposing that X(t) psd on [0, 1].
  """

  m, m2, d = len(mat_x), len(mat_x[0]), len(mat_x[0][0]) - 1

  # square matrix
  assert m == m2

  # build constraints: X == alpha(Q1) + beta(Q2) with Q1, Q2 >> 0
  d_2 = int(d / 2)
  size_Q1 = m * (d_2 + 1)
  size_Q2 = m * d_2 if d % 2 == 0 else m * (d_2 + 1)

  Q1 = cvxpy.Variable((size_Q1, size_Q1))
  Q2 = cvxpy.Variable((size_Q2, size_Q2))

  diff = mat_x - _alpha(m, d, Q1) - _beta(m, d, Q2)
  diff = diff.reshape(-1)

  const = _make_zero(diff)
  const += [Q1 >> 0, Q2 >> 0, Q1.T == Q1, Q2.T == Q2]

  return const
```

## Some Polynomial Tools

```
def integ_poly_0_1(p):
  """Return the integral of p(t) between 0 and 1."""

  return np.array(p).dot(1 / np.linspace(1, len(p), len(p)))

def spline_regression(x, y, num_parts, deg=3, alpha=.01, smoothness=1):
  """Fits splines with `num_parts` to data `(x, y)`.

  Finds a piecewise polynomial function `p` of degree `deg` with `num_parts`
  pieces that minimizes the fitting error sum |y_i - p(x_i)| + alpha |p|_1.

  Args:
    x: [N] ndarray of  input data. Must be increasing.
    y: [N] ndarray, same size as `x`.
    num_parts: int, Number of pieces of the piecewise polynomial function `p`.
    deg: int, degree of each polynomial piece of `p`.
    alpha: float, Regularizer.
    smoothness: int, the desired degree of smoothness of `p`, e.g.
      `smoothness==0` corresponds to a continuous `p`.

  Returns:
     [num_parts, deg+1] ndarray representing the piecewise polynomial `p`.
     Entry (i, j)  contains j^th coefficient of the i^th piece of `p`.
  """

  # coefficients of the polynomial of p.
  p = cvxpy.Variable((num_parts, deg + 1), name='p')

  # convert to numpy format because it is easier to work with.
  numpy_p = np.array([[p[i, j] for j in range(deg+1)] \
                    for i in range(num_parts)])

  regularizer = alpha * cvxpy.norm(p, 1)

  num_points_per_part = int(len(x) / num_parts)

  smoothness_constraints = []

  # cuttoff values
  t = []

  fitting_value = 0
  # split the data into equal `num_parts` pieces
  for i in range(num_parts):

    # the part of the data that the current piece fits
    sub_x = x[num_points_per_part * i:num_points_per_part * (i + 1)]
    sub_y = y[num_points_per_part * i:num_points_per_part * (i + 1)]

    # compute p(sub_x)
    # pow_x = np.array([sub_x**k for k in range(deg + 1)])
    # sub_p = polyval(sub_xnumpy_p[i, :].dot(pow_x)
    sub_p = eval_poly_from_coefficients(numpy_p[i], sub_x)

    # fitting value of the current part of p,
    # equal to sqrt(sum |p(x_i) - y_i|^2), where the sum
    # is over data (x_i, y_i) in the current piece.
    fitting_value += cvxpy.norm(cvxpy.vstack(sub_p - sub_y), 1)

    # glue things together by ensuring smoothness of the p at x1
    if i > 0:
      x1 = x[num_points_per_part * i]
      # computes the derivatives p'(x1) for the left and from the right of x1

      # x_deriv is the 2D matrix  k!/(k-j)! x1^(k-j) indexed by (j, k)
      x1_deriv = np.array(
          [[np.prod(range(k - j, k)) * x1**(k - j)
            for k in range(deg + 1)]
           for j in range(smoothness + 1)]).T

      p_deriv_left = numpy_p[i - 1].dot(x1_deriv)
      p_deriv_right = numpy_p[i].dot(x1_deriv)

      smoothness_constraints += [
          cvxpy.vstack(p_deriv_left - p_deriv_right) == 0
      ]
      t.append(x1)
  min_loss = cvxpy.Minimize(fitting_value + regularizer)
  prob = cvxpy.Problem(min_loss, smoothness_constraints)
  prob.solve(verbose=False)

  return _piecewise_polynomial_as_function(p.value, t)

def _piecewise_polynomial_as_function(p, t):
  """Returns the piecewise polynomial `p` as a function.

  Args:
    p: [N, d+1] array of coefficients of p.
    t: [N] array of cuttoffs.

  Returns:
    The function f s.t. f(x) = p_i(x) if t[i] < x < t[i+1].
  """

  def evaluate_p_at(x):
    """Returns p(x)."""

    pieces = [x < t[0]] + [(x >= ti)  & (x < ti_plusone) \
             for ti, ti_plusone in zip(t[:-1], t[1:])] +\
              [x >= t[-1]]

    # pylint: disable=unused-variable
    func_list = [
        lambda u, pi=pi: eval_poly_from_coefficients(pi, u) for pi in p
    ]

    return np.piecewise(x, pieces, func_list)

  return evaluate_p_at

def eval_poly_from_coefficients(coefficients, x):
  """Evaluates the polynomial whose coefficients are `coefficients` at `x`."""
  return coefficients.dot([x**i for i in range(len(coefficients))])
```

## Examples: To Add.