first phase speedup completed 
added jacobian dot mex
updated robot