stop (intop1(x)>xb)*(intop1(y)<0) Stop criterion
Tsh Tsh_max*tanh(lambda*t) Applied shoulder torque
Ta min(Ta_max,Ta_rate*t) Applied arm torque
Tw Tw_max*tanh(lambda*(t-tw)) Applied wrist torque