# 2.47 Bio-Inspired Robotics Final Project


## Presentation

- [Link to the google slides presentation.](https://docs.google.com/presentation/d/1jrJuXlVjGZLsMKcDKoz_kq29gHqd7rpQDm3EZizCAnw/edit)

## Proposal 

- [Link to the google docs.](https://docs.google.com/document/d/1dNlEJe5-szEFqpwIeSgpptBZx4WYQ4iAs7nnBplzilc/edit)

<img src="./img/leg.png" width="50%" /><br></br>

---
## Simulation

```math
q   = [\theta_1  ; \theta_2; x; y]; \\
\dot{q}   = [\dot{\theta}_1  ; \dot{\theta}_2; \dot{x}; \dot{y}];
```
---

## Control

### simulation/optimization

All torque control with bezier curves

### hardware control

Get Bezier curve path of the leg from simulation/optimization and  have an Impedance control (flight stage) and torque control (stance phase)

---
## Optimization 

### Variables

Two Bezier curves for torques:
```math
ctrl_1=[T_1,T_2,..T_n] \\
ctrl_2=[T_1,T_2,..T_n] \\
n=6 \\
-0.85<T_i<085
```

Starting Conditions:

```math
ground \ height = -0.164; \\
\theta_1=-36*\pi/180; \\
\theta_2=90*\pi/180;\\
x=y=0\\
\dot{\theta}_1=\dot{\theta}_2= 0\\
\dot{x}=\dot{y}= 0\\
```

### Constraints

```math
x^{end}>=x^{start} \\
q_1^{end}=q_1^{start} \\
q_2^{end}=q_2^{start} \\
y^{end}=y^{start} \\

\dot{q}_1^{end}=\dot{q}_1^{start} \\
\dot{q}_2^{end}=\dot{q}_2^{start} \\
\dot{x}^{end}=\dot{x}^{start} \\
\dot{y}^{end}=\dot{y}^{start} \\
```

### Objective Function

1. First objective: maximize height $`h'`$ to push it to go up, got the max $`x'`$
2. Then I added to the constraints:
    ```math
    apex \ height = h' \\
    x_{end}=x' \\
    ``` 
    and minimized cost of transport by minimizing:
    ```math
    E/(m*g*d)
    ``` 
    which is the similar as minimizing sum of torque squared as m, g and d is fixed

---
## Updates and Results
## 5 December
<!-- <img src="./img/24_nov/gait.gif" width="80%" /><br></br> -->


<img src="./img/5_dec_3/6.1_phases.png" width="40%" />
<img src="./img/5_dec_3/6.1_phases.png" width="40%" /><br></br>

<img src="./img/5_dec_3/6.8_cntrl.png" width="40%" />
<img src="./img/5_dec_3/6.19_cntrl.png" width="40%" /><br></br>


<img src="./img/5_dec_3/6.1_ks.png" width="40%" />
<img src="./img/5_dec_3/6.12_ks.png" width="40%" /><br></br>


<img src="./img/5_dec_3/6.1_ks_zoom.png" width="40%" />
<img src="./img/5_dec_3/6.12_ks_zoom.png" width="40%" /><br></br>

<img src="./img/5_dec_3/6.1_torques_thetast.png" width="40%" />
<img src="./img/5_dec_3/6.12_torques_thetast.png" width="40%" /><br></br>

<img src="./img/5_dec_3/6.1_torques_with_without.png" width="40%" />
<img src="./img/5_dec_3/6.12_torques_with_without.png" width="40%" /><br></br>

<img src="./img/5_dec_3/6.1_torques_with_without_squared.png" width="40%" />
<img src="./img/5_dec_3/6.12_torques_with_without_squared.png" width="40%" /><br></br>

## 2 December
<!-- <img src="./img/24_nov/gait.gif" width="80%" /><br></br> -->


<img src="./img/2_dec/6.16_cntrl.png" width="40%" />
<img src="./img/2_dec/6.17_cntrl.png" width="40%" /><br></br>


<img src="./img/2_dec_1/6.1_ks.png" width="40%" />
<img src="./img/2_dec_1/6.12_ks.png" width="40%" /><br></br>


<img src="./img/2_dec_1/6.1_ks_zoom.png" width="40%" />
<img src="./img/2_dec_1/6.12_ks_zoom.png" width="40%" /><br></br>

<img src="./img/2_dec/6.1_torques_thetast.png" width="40%" />
<img src="./img/2_dec/6.12_torques_thetast.png" width="40%" /><br></br>

<img src="./img/2_dec/6.1_torques_with_without.png" width="40%" />
<img src="./img/2_dec/6.12_torques_with_without.png" width="40%" /><br></br>



## November 23
- having trouble finding a gait
- Added starting $`\dot{q_1}^{start}`$,$`\dot{q_2}^{start}`$,$`\dot{x}^{start}`$,$`\dot{y}^{start}`$ to the optimization variables
- added  $`q_1^{start}`$ and $`q_2^{start}`$ to the optimization variables
- recently added tf as it changed the dynamics
- Moving to casadi instead of fmincon (maybe the derivatives will help)
  - lots has to be changed as it does not support branching statements they have discontinuous derivatives, that affect the two functions `discrete_impact_contact` and `joint_limit_constraint` in `simulate_leg.m`
  - first the angle constraints has to be added as constraints
  - second the simulation has to be divided into three stages: stance flight and stance again. The tf of each of these stages will be come an optimization variable as well 
  
<img src="./img/24_nov/gait.gif" width="80%" /><br></br>


<img src="./img/24_nov/6.16_cntrl.png" width="40%" />
<img src="./img/24_nov/6.17_cntrl.png" width="40%" /><br></br>

<img src="./img/24_nov/6.1_torques_thetast.png" width="40%" />
<img src="./img/24_nov/6.12_torques_thetast.png" width="40%" /><br></br>

<img src="./img/24_nov/6.1_torques_with_without.png" width="40%" />
<img src="./img/24_nov/6.12_torques_with_without.png" width="40%" /><br></br>

<img src="./img/24_nov/6.1_ks.png" width="40%" />
<img src="./img/24_nov/6.12_ks.png" width="40%" /><br></br>





### November 12: First Hop

<img src="./img/first_hop.gif" width="80%" /><br></br>

<img src="./img/6.1_cntrl.png" width="40%" />
<img src="./img/6.3_cntrl.png" width="40%" /><br></br>

---
## TODO and Questions
- [x] script to calculate before and after adding spring 
- [ ] Optimize with springs
- [x] get real physical variables
  - [x] angle limits
  - [x] masses
  - [x] torques limits
  - [ ] the friction coefficients
- [x] try other starting conditions
- [x] is velocity calculation correct?
- [x] what if it torque not enough
- [x] for the control should $T_1 = T_n$ (cyclical control?)

- [ ] change bezier to lower dimensions to allow for sudden changes
- [ ] check if angle calculation is correct
- [ ] from last to to first 
- [ ] adding springs 
---