took out some variables that were equal to zero. Changed some trig to be more consistent
returns two points. p0 is the xyz of the elbow. p1 is the xyz of the jaw
takes xyz coordinate of jaw and outputs desired angles for the each joint