Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- function R = myinvd(robot,J,dJ,ddJ,cnf)
- %
- % myinvd(robot,J,dJ,ddJ,cnf) - inverse dynamic
- %
- % Input parameters:
- % robot: struct ... description of the manipulator. You can ignore and
- % use description and parameters from your variant.
- % J: [2 x n] ... joint coordinates. Each column J(:,i) contains
- % 2 joint coordinates (angles and or displacements).
- % dJ: [2 x n] ... joint velocities. Each column dJ(:,i) contains
- % 2 joint velocities (angular velocity).
- % ddJ: [2 x n] ... joint accelerations. Each column dJ(:,i) contains
- % 2 joint accelerations (angular acceleration).
- % cnf: [1 x n] ... robot configuration sign. Configuration of final two
- % links is defined by position of end point in
- % relation to line defined by their oposit ends.
- %
- % Output parameters:
- % R: [2 x n] ... torques and forces provided by drives for each input
- % configuration which is defined by J and cnf. Each
- % column of R must contain torques for corresponding
- % input configuration. If there is no solution for
- % input configuration, the output value for the
- % configuration should be NaN.
- %
- % All angles are in radians !
- % preallocation
- R = zeros(2, size(J,2));
- R(:) = NaN;
- for i = 1:size(J,2)
- R(:,i) = my_myinvd(robot,J(:,i),dJ(:,i),ddJ(:,i),cnf(i));
- end
- % R(:,4:end)=NaN;
- % R(:,1:2)=NaN;
- end
- function output = my_myinvd(robot,J,dJ,ddJ,cnf)
- %% SET CONSTANTS %%
- phi4 = robot.rotations(1);
- P1 = [robot.origins(1);
- robot.origins(2)]/10;
- P2 = [robot.origins(3);
- robot.origins(4)]/10;
- P1x = P1(1);
- P1y = P1(2);
- P2x = P2(1);
- P2y = P2(2);
- L1 = robot.links(1,2)/20;
- L2 = robot.links(2,2)/20;
- L3 = robot.links(2,1)/20;
- C=mydkt(robot, J);
- C1=C{1};
- C2=C{2};
- d=J(1)/10;
- dd = dJ(1)/10;
- ddd = ddJ(1)/10;
- phi1 = J(2);
- phi1d = dJ(2);
- phi1dd = ddJ(2);
- Ax=d*cos(phi4)+P1x;
- Ay=d*sin(phi4)+P1y;
- Bx=L1*2*cos(phi1)+P2x;
- By=L1*2*sin(phi1)+P2y;
- %% DECIDE SOLUTION %%
- Wx1=C1(1)/10;
- Wy1=C1(2)/10;
- Wx2=C2(1)/10;
- Wy2=C2(2)/10;
- K=(By-Ay)*Wx1-(Bx-Ax)*Wy1 +Ay*(Bx-Ax)-Ax*(By-Ay)
- if (abs(sign(K)-cnf) > 1e-3)
- Wx=Wx2;
- Wy=Wy2;
- else
- Wx=Wx1;
- Wy=Wy1;
- end
- if isnan(Wx) || isnan(Wy)
- output = [NaN;NaN];
- return
- end
- %% VECTOR METHOD %%
- phi2 = atan2(Wy-By, Wx-Bx);
- phi3 = atan2(Ay-Wy, Ax-Wx);
- qd = [dd; phi1d];
- qdd = [ddd; phi1dd];
- Jq = [-0.5*cos(phi4) -L1*sin(phi1);-0.5*sin(phi4) L1*cos(phi1)];
- Jz = [-L2*sin(phi2) -L3*sin(phi3); L2*cos(phi2) L3*cos(phi3)];
- zd = -(Jz\Jq)*qd;
- phi2d = zd(1);
- phi3d = zd(2);
- Jqd = [0 -L1*cos(phi1)*phi1d; 0 -L1*sin(phi1)*phi1d];
- Jzd = [-L2*cos(phi2)*phi2d -L3*cos(phi3)*phi3d; -L2*sin(phi2)*phi2d -L3*sin(phi3)*phi3d];
- zdd = -inv(Jz)*(Jq*qdd + Jzd*zd + Jqd*qd);
- phi2dd = zdd(1);
- phi3dd = zdd(2);
- %% WORLD COORDINATES %%
- Axdd = ddd*cos(phi4);
- Aydd = ddd*sin(phi4);
- S1xdd = -L1*(cos(phi1)*phi1d^2 + sin(phi1)*phi1dd);
- S1ydd = L1*(-sin(phi1)*phi1d^2 + cos(phi1)*phi1dd);
- S2xdd = 2*S1xdd - L2*(cos(phi2)*phi2d^2 + sin(phi2)*phi2dd);
- S2ydd = 2*S1ydd + L2*(-sin(phi2)*phi2d^2 + cos(phi2)*phi2dd);
- S3xdd = Axdd + L3*(cos(phi3)*phi3d^2 + sin(phi3)*phi3dd);
- S3ydd = Aydd - L3*(-sin(phi3)*phi3d^2 + cos(phi3)*phi3dd);
- f1= [ -1 0 1 0 0 0 0 0 0 0];
- f2= [ 0 -1 0 1 0 0 0 0 0 0];
- f3= [-L1*sin(phi1) L1*cos(phi1) -L1*sin(phi1) L1*cos(phi1) 0 0 0 0 0 1];
- f4= [ 0 0 -1 0 1 0 0 0 0 0];
- f5= [ 0 0 0 -1 0 1 0 0 0 0];
- f6= [ 0 0 -L2*sin(phi2) L2*cos(phi2) -L2*sin(phi2) L2*cos(phi2) 0 0 0 0];
- f7= [ 0 0 0 0 -1 0 1 0 0 0];
- f8= [ 0 0 0 0 0 -1 0 1 0 0];
- f9= [ 0 0 0 0 -L3*sin(phi3) L3*cos(phi3) -L3*sin(phi3) L3*cos(phi3) 0 0];
- f10=[ 0 0 0 0 0 0 -cos(phi4) -sin(phi4) 1 0];
- D = [f1;f2;f3;f4;f5;f6;f7;f8;f9;f10]';
- %% INVERZNI DYNAMICKA ULOHA &&
- ro = 2;
- ma = 0.6;
- m1 = L1*ro*2;
- m2 = L2*ro*2;
- m3 = L3*ro*2;
- I1 = m1*4*L1^2/12;
- I2 = m2*4*L2^2/12;
- I3 = m3*4*L3^2/12;
- M = diag([m1 m1 I1 m2 m2 I2 m3 m3 I3 ma]);
- % Q = [0 0 ma 0 0 0 0 0 0 cos(phi4)*F sin(phi4)*F 0];
- a = [S1xdd S1ydd phi1dd S2xdd S2ydd phi2dd S3xdd S3ydd phi3dd ddd]';
- R = D'\(M*a);
- % lambda = R(1:10);
- Moment = R(9);
- F = R(10);
- output = [Moment; F];
- end
- % here assign values to R for each input configuration
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement