% Force Control % Tip Position Tip = [ 1.0 ; 0.0 ]; % Joint Angles Q = InverseRR(Tip) % Desired Tip Force Ref = [1 ; 2]; % Compute the Jacobian J = [-sin(Q(1)) - sin(Q(1)+Q(2)), -sin(Q(1)+Q(2)) ; cos(Q(1)) + cos(Q(1)+Q(2)), cos(Q(1)+Q(2)) ]; % Initial Values T = [0; 0]; t = 0; dt = 0.001; % plotting data Fref = []; Ftip = []; while(t < 2) % Compute the tip forces given the joint torques F2 = J' * T; % the error in the tip forces dF = Ref - F2; % convert to the error in the joint torques dT = inv(J')*dF; % integrate with a gain of 10 T = T + 10*dT*dt; t = t + dt; % Save the tip forces for plotting Fref = [Fref, Ref]; Ftip = [Ftip, F2]; end t = [1:length(Ftip)] * dt; plot(t,Fref,t,Ftip); xlabel('Time (seconds)'); ylabel('Tip Force (N)');