Minimum jerk trajectory generator has been fixed and the linear part of the right hand trajectory has been partially debugged. The orientation part needs further debugging.