# 2.47 Bio-Inspired Robotics Final Project

Proposal [link.](https://docs.google.com/document/d/1dNlEJe5-szEFqpwIeSgpptBZx4WYQ4iAs7nnBplzilc/edit)

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

---
## Simulation

$$
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:
$$
ctrl_1=[T_1,T_2,..T_n] \\
ctrl_2=[T_1,T_2,..T_n] \\
n=6 \\
2<T_i<2
$$

Starting Conditions:

$$
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

$$
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:
    $$
    apex \ height = h' \\
    x_{end}=x' \\
    $$ 
    and minimized cost of transport by minimizing:
    $$
    E/(m*g*d)
    $$ 
    which is the similar as minimizing sum of torque squared as m, g and d is fixed




---
## Results

###First Hop

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


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


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


---