function T = WP_move(T_a,av,del_t)
%

WHEEL_RADIUS = 1;
L = 2;

% robot state
theta = atan2(T_a(2,1),T_a(1,1));
x = T_a(1,4);
y = T_a(2,4);

V_l = av(1)*WHEEL_RADIUS;
V_r = av(2)*WHEEL_RADIUS;
w = (V_r-V_l)/L;
if V_l==0&V_r==0
    return
end
if V_l==V_r
    p = [x+V_l*cos(theta)*del_t;y+V_l*sin(theta)*del_t;theta];
elseif V_l==-V_r
    p = [x;y;theta+2*V_l*del_t/L];
else
    R = (L/2)*(V_l+V_r)/(V_r-V_l);
    ICC = [x-R*sin(theta);y+R*cos(theta)];
    wdt = w*del_t;
    A = [cos(wdt),-sin(wdt),0;sin(wdt),cos(wdt),0;0 0 1];
    p = A*[x-ICC(1); y-ICC(2); theta] + [ICC(1); ICC(2); wdt];
end
R = [cos(p(3)),-sin(p(3)),0;sin(p(3)),cos(p(3)),0;0 0 1];
T(1:3,1:3) = R;
T(1,4) = p(1);
T(2,4) = p(2);
tch = 0;
