fixed InvKin, but not sure why the fixes I made fixed it. Also, trajopt is up and running.