Added detection for reachable workspace

The inv kine function now checks for reachable workspace by checking
whether the triangles it uses to compute thetas are degenerate.  The
logic is that we are outside the reachable workspace, the triangles will
be degenerate (have one side longer than the sum of the other two).