Basic figures.h

From HotDec

Pseudo-Code Description

path_init_for_h() {

  Assign current pose to the reference control input vector
  Assign distance from start pose to target pose as part of the reference input

}

path_line() {

  As far as I can tell, this is just a function that calculates a weighted distance 
from starting pose to target pose based on time elapsed and relative current pose.

  It uses this weighted distance to calculate the error vector.

}