* Solved reset pose bug
* Increased wrist P gain
* Small changes to improve FWD seq

Signed-off-by: Darpa Tech 3 <darpa3@SMILe03.(none)>