├── pose_state.m ├── imgs ├── comparison.png ├── reachable.png ├── fixed_location.png ├── fixed_orientation.png └── all_range_orientation.png ├── roty.m ├── rotx.m ├── rotz.m ├── README.md ├── IK.m ├── LICENSE ├── rdl2bph.m ├── FK.m ├── example_fixed_orientation_ws.m ├── example_fixed_location_ws.m ├── DistBetween2Segment.m ├── example_all_range_orientation_ws.m ├── example_reachable_ws.m └── example_inverse_dynamics.m /pose_state.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/houchangmeng/Stewart-Platform-Related-Code/HEAD/pose_state.m -------------------------------------------------------------------------------- /imgs/comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/houchangmeng/Stewart-Platform-Related-Code/HEAD/imgs/comparison.png -------------------------------------------------------------------------------- /imgs/reachable.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/houchangmeng/Stewart-Platform-Related-Code/HEAD/imgs/reachable.png -------------------------------------------------------------------------------- /imgs/fixed_location.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/houchangmeng/Stewart-Platform-Related-Code/HEAD/imgs/fixed_location.png -------------------------------------------------------------------------------- /imgs/fixed_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/houchangmeng/Stewart-Platform-Related-Code/HEAD/imgs/fixed_orientation.png -------------------------------------------------------------------------------- /imgs/all_range_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/houchangmeng/Stewart-Platform-Related-Code/HEAD/imgs/all_range_orientation.png -------------------------------------------------------------------------------- /roty.m: -------------------------------------------------------------------------------- 1 | function R = roty(t, deg) 2 | 3 | assert((isreal(t) & isscalar(t)) | isa(t, 'sym'), ... 4 | 'RTB:roty:badarg', 'theta must be a real scalar'); 5 | 6 | if nargin > 1 && strcmp(deg, 'deg') 7 | t = t *pi/180; 8 | end 9 | 10 | ct = cos(t); 11 | st = sin(t); 12 | 13 | % make almost zero elements exactly zero 14 | if ~isa(t, 'sym') 15 | if abs(st) < eps 16 | st = 0; 17 | end 18 | if abs(ct) < eps 19 | ct = 0; 20 | end 21 | end 22 | 23 | % create the rotation matrix 24 | R = [ 25 | ct 0 st 26 | 0 1 0 27 | -st 0 ct 28 | ]; 29 | end -------------------------------------------------------------------------------- /rotx.m: -------------------------------------------------------------------------------- 1 | function R = rotx(t, deg) 2 | 3 | assert((isreal(t) & isscalar(t)) | isa(t, 'sym'), ... 4 | 'RTB:rotx:badarg', 'theta must be a real scalar'); 5 | 6 | if nargin > 1 && strcmp(deg, 'deg') 7 | t = t *pi/180; 8 | end 9 | 10 | ct = cos(t); 11 | st = sin(t); 12 | 13 | % make almost zero elements exactly zero 14 | if ~isa(t, 'sym') 15 | if abs(st) < eps 16 | st = 0; 17 | end 18 | if abs(ct) < eps 19 | ct = 0; 20 | end 21 | end 22 | 23 | % create the rotation matrix 24 | R = [ 25 | 1 0 0 26 | 0 ct -st 27 | 0 st ct 28 | ]; 29 | end -------------------------------------------------------------------------------- /rotz.m: -------------------------------------------------------------------------------- 1 | function R = rotz(t, deg) 2 | 3 | assert((isreal(t) & isscalar(t)) | isa(t, 'sym'), ... 4 | 'RTB:rotz:badarg', 'theta must be a real scalar or symbolic'); 5 | 6 | if nargin > 1 && strcmp(deg, 'deg') 7 | t = t *pi/180; 8 | end 9 | 10 | ct = cos(t); 11 | st = sin(t); 12 | 13 | % make almost zero elements exactly zero 14 | if ~isa(t, 'sym') 15 | if abs(st) < eps 16 | st = 0; 17 | end 18 | if abs(ct) < eps 19 | ct = 0; 20 | end 21 | end 22 | 23 | % create the rotation matrix 24 | R = [ 25 | ct -st 0 26 | st ct 0 27 | 0 0 1 28 | ]; 29 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Stewart-Platform-Related-Code 2 | 3 | Stewart Platform Related Code，such as forward kinematics, inverse kinematics, inverse dynamics, workspace etc. 4 | 5 | ## Run code 6 | 7 | run example_*.m file. 8 | 9 | ## Workspace pictures 10 | 11 | #### Fixed Orentation - Location Workspace. 12 | 13 | ![fixed_orientation](./imgs/fixed_orientation.png) 14 | 15 | #### Fixed Location - Orentation Workspace. 16 | 17 | ![fixed_location](./imgs/fixed_location.png) 18 | 19 | #### Reachable Workspace. 20 | 21 | ![reachable](./imgs/reachable.png) 22 | 23 | #### All range orientation. 24 | 25 | ![all_range_orientation](./imgs/all_range_orientation.png) 26 | 27 | #### Comparison between all range, fixed orientation and reachable 28 | 29 | From inside to outside. 30 | 31 | ![comparison](./imgs/comparison.png) 32 | 33 | 34 | -------------------------------------------------------------------------------- /IK.m: -------------------------------------------------------------------------------- 1 | % Author: Changmeng Hou(Harbin Engineering University) 2 | 3 | % Inverse Kinematics. 4 | 5 | function [nowLmbL,Q,L,R_PB,Jac] = IK(pose,B,P) 6 | 7 | if ~( (isequal(length(pose),6))&&(isequal(size(B),[3,6]))&&(isequal(size(P),[3,6])) ) 8 | error('Check your input!') 9 | end 10 | 11 | Trans = pose(1:3); 12 | Euler = pose(4:6); 13 | 14 | % % calculate the output 15 | 16 | T = Trans(:); 17 | R_PB = rotz(Euler(3))*roty(Euler(2))*rotx(Euler(1)); % rotation (zyx) 18 | P_B = R_PB*P; 19 | Q = T+P_B; 20 | L = Q-B; 21 | 22 | if ~isa(pose,'sym') 23 | nowLmbL = zeros(1,6); 24 | s = zeros(3,6); 25 | Jac = zeros(6,6); 26 | end 27 | 28 | for i = 1:6 29 | nowLmbL(i) = norm(L(:,i)); 30 | s(:,i) = L(:,i)/nowLmbL(i); 31 | Jac(i,:) = [s(:,i).' cross(P_B(:,i),s(:,i)).']; 32 | end 33 | 34 | detJ = det(Jac); 35 | 36 | end 37 | 38 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Changmeng Hou 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /rdl2bph.m: -------------------------------------------------------------------------------- 1 | % Author: Changmeng Hou(Harbin Engineering University) 2 | 3 | % Generate all joints location from platform radius, joint distance and limb length. 4 | 5 | function [B,P,H] = rdl2bph(RDL) 6 | 7 | assert( ( ( (RDL(3)<=RDL(1)) && (RDL(4)<=RDL(2)) )||(RDL(5)<=RDL(6)) ),... 8 | 'Error Joint Distance!') 9 | 10 | rb = RDL(1); 11 | rp = RDL(2); 12 | db = RDL(3); 13 | dp = RDL(4); 14 | 15 | etap = pi/3 -asin(dp/2/rp); 16 | etab = asin(db/2/rb); 17 | etaP = [etap 2/3*pi-etap 2/3*pi+etap 4/3*pi-etap 4/3*pi+etap -etap]; 18 | etaB = [etab 2/3*pi-etab 2/3*pi+etab 4/3*pi-etab 4/3*pi+etab -etab]; 19 | wL = [0 0 0]'; 20 | wB = [0 0 0]'; 21 | P = [rp*cos(etaP);rp*sin(etaP);[0 0 0 0 0 0]] + wL; 22 | B = [rb*cos(etaB);rb*sin(etaB);[0 0 0 0 0 0]] + wB; 23 | 24 | 25 | limbMin = RDL(5); 26 | limbMax= RDL(6); 27 | 28 | syms h 29 | R_PB = eye(3); 30 | equ1 = norm([0 0 h].'+R_PB*P(:,1)-B(:,1)) == limbMin; 31 | equ2 = norm([0 0 h].'+R_PB*P(:,1)-B(:,1)) == limbMax; 32 | Hmin = double(solve(equ1,h)); 33 | Hmax = double(solve(equ2,h)); 34 | clear h 35 | if isempty(Hmin), Hmin = 0; end 36 | H = [Hmin;Hmax]; 37 | 38 | end 39 | 40 | 41 | -------------------------------------------------------------------------------- /FK.m: -------------------------------------------------------------------------------- 1 | % Author: Changmeng Hou(Harbin Engineering University) 2 | 3 | % Forward Kinematics. 4 | 5 | function [pose,isSingular] = FK(lmbLength,B,P) 6 | 7 | if (~isequal(size(P),[3,6]))||(~isequal(size(B),[3,6])) 8 | error('Check the paramaters P or B!') 9 | end 10 | if (~isequal(length(lmbLength),6)) 11 | error('Check the paramaters limblength') 12 | end 13 | 14 | % initial position 15 | initPose = [0,0,mean(lmbLength),0,0,0].'; 16 | 17 | finalLmbL = lmbLength; 18 | 19 | poseTemp = initPose(:); 20 | [nowLmbL,~,~,~,J] = IK(poseTemp,B,P); 21 | deltaLmb = finalLmbL(:)-nowLmbL(:); 22 | Jmodi = getJmodi(poseTemp,J); 23 | pose = poseTemp+Jmodi\deltaLmb; 24 | 25 | iteraIdx = 0; 26 | 27 | while (norm(pose - poseTemp)>1e-3) && (iteraIdx < 50) 28 | 29 | poseTemp = pose; 30 | [nowLmbL,~,~,~,J] = IK(poseTemp,B,P); 31 | % delta limb 32 | deltaLmb = finalLmbL(:)-nowLmbL(:); 33 | Jmodi = getJmodi(poseTemp,J); 34 | % singular judge 35 | poseVel = Jmodi\deltaLmb; 36 | isSingular = norm(poseVel,'inf')>(100*mean(finalLmbL)); 37 | if isSingular 38 | break 39 | end 40 | pose = poseTemp+poseVel; 41 | iteraIdx = iteraIdx +1; 42 | end 43 | 44 | function Jmodi = getJmodi(pose,J) 45 | 46 | be = pose(5); 47 | ga = pose(6); 48 | sb = sin(be); 49 | sg = sin(ga); 50 | cb = cos(be); 51 | cg = cos(ga); 52 | 53 | U = [cg*cb -sg 0; 54 | sg*cb cg 0; 55 | -sb 0 1]; 56 | Jmodi = J*[ eye(3), zeros(3); 57 | zeros(3), U ]; 58 | end 59 | end -------------------------------------------------------------------------------- /example_fixed_orientation_ws.m: -------------------------------------------------------------------------------- 1 | % Author: Changmeng Hou(Harbin Engineering University) 2 | 3 | % Fixed Orientation - Location Workspace. 4 | 5 | clc 6 | clear 7 | tic 8 | 9 | RDL = [5 3 .5 .3 7.2 10]; 10 | [B,P,H] = rdl2bph(RDL); 11 | limbPara = RDL(5:6); 12 | jointCons = [0,pi/4]; 13 | 14 | 15 | zmin = H(1); 16 | zmax = H(2); 17 | zRange = [zmin,zmax]; 18 | euler = [0 0 0]; 19 | numz = round((zRange(2)-zRange(1))/0.1); 20 | numTheta = 90; 21 | z = linspace(zRange(1),zRange(2),numz); 22 | 23 | theta = linspace(0,2*pi,numTheta); 24 | 25 | % theta = distributed(theta); 26 | deltaz = (zRange(2)-zRange(1))/numz; 27 | deltaTheta = 2*pi/numTheta; 28 | deltaRho = 0.02; 29 | 30 | rhoTemp = zeros(numz,numTheta); 31 | subVoli = 0; 32 | subVol = zeros(numz*numTheta,1); 33 | V = 0; 34 | 35 | for i =1:numz 36 | for j = 1:numTheta 37 | rho = 0; 38 | inWorkspace = 1; 39 | while (rho >= 0&& inWorkspace) 40 | 41 | [xc,yc,zc] = pol2cart(theta(j),rho,z(i)); 42 | transTemp = [xc,yc,zc]; 43 | eulerTemp = [0 0 0]; 44 | poseTemp = [transTemp,eulerTemp]; 45 | [inWorkspace,isSingular,inBound] = pose_state(poseTemp,jointCons,limbPara,B,P); 46 | rho = rho + deltaRho; 47 | end 48 | rhoTemp(i,j) = rho ; 49 | subV = 1/2*(rho^2)*deltaz*deltaTheta; 50 | V =V+subV; 51 | end 52 | end 53 | 54 | 55 | [row] = find(all(rhoTemp ~= 0,2)); 56 | if (max(row) ~= numz)&&(min(row) ~= 1) 57 | row = [min(row)-1;row;max(row)+1]; 58 | elseif (max(row) == numz)&&(min(row) ~= 1) 59 | row = [min(row)-1;row]; 60 | elseif (max(row) ~= numz)&&(min(row) == 1) 61 | row = [row;max(row)+1]; 62 | else 63 | 64 | end 65 | 66 | RHO = rhoTemp(row,:); 67 | z = z(row); 68 | [THETA,Z] = meshgrid(theta,z); 69 | [XC,YC,ZC] = pol2cart(THETA,RHO,Z); 70 | mesh(XC,YC,ZC) 71 | 72 | 73 | toc 74 | 75 | -------------------------------------------------------------------------------- /example_fixed_location_ws.m: -------------------------------------------------------------------------------- 1 | % Author: Changmeng Hou(Harbin Engineering University) 2 | 3 | % Fixed Location - Orientation Workspace. 4 | 5 | clc 6 | clear 7 | tic 8 | 9 | RDL = [5 3 .5 .3 7.2 10]; 10 | [B,P,H] = rdl2bph(RDL); 11 | limbPara = RDL(5:6); 12 | jointCons = [0,pi/4]; 13 | 14 | 15 | gammarange = [-pi/2,pi/2]; 16 | 17 | pose = FK(repmat(mean(limbPara),[1,6]),B,P); 18 | transTemp = pose(1:3).'; 19 | 20 | numGamma = round((pi/0.05)); 21 | numTheta = 90; 22 | gamma = linspace(-pi/2,pi/2,numGamma); 23 | theta = linspace(0,2*pi,numTheta); 24 | 25 | deltaGamma = pi/numGamma; 26 | deltaTheta = 2*pi/numTheta; 27 | deltaRho = 0.005; 28 | 29 | rhoTemp = zeros(numGamma,numTheta); 30 | subVoli = 0; 31 | subVol = zeros(numGamma*numTheta,1); 32 | for i =1:numGamma 33 | for j = 1:numTheta 34 | rho = 0; 35 | while (rho >= 0) 36 | 37 | [alc,bec,gac] = pol2cart(theta(j),rho,gamma(i)); 38 | % transTemp = [0 0 .5*(H(1)+H(2))]; 39 | eulerTemp = [alc,bec,gac]; 40 | poseTemp = [transTemp,eulerTemp]; 41 | [inWorkspace,isSingular,inBound] = pose_state(poseTemp,jointCons,limbPara,B,P); 42 | if inWorkspace 43 | rho = rho + deltaRho; 44 | else 45 | rhoTemp(i,j) = rho ; 46 | subVoli = subVoli + 1; 47 | subVol(subVoli) = 1/2*(rho^2)*deltaGamma*deltaTheta; 48 | break 49 | end 50 | end 51 | end 52 | end 53 | 54 | 55 | [row] = find(all(rhoTemp ~= 0,2)); 56 | if (max(row) ~= numGamma)&&(min(row) ~= 1) 57 | row = [min(row)-1;row;max(row)+1]; 58 | elseif (max(row) == numGamma)&&(min(row) ~= 1) 59 | row = [min(row)-1;row]; 60 | elseif (max(row) ~= numGamma)&&(min(row) == 1) 61 | row = [row;max(row)+1]; 62 | else 63 | 64 | end 65 | 66 | RHO = rhoTemp(row,:); 67 | gamma = gamma(row); 68 | [THETA,Z] = meshgrid(theta,gamma); 69 | [XC,YC,ZC] = pol2cart(THETA,RHO,Z); 70 | 71 | mesh(XC,YC,ZC) 72 | % COWVolume = sum(subVol) 73 | % XCdeg = XC.*180./pi; 74 | % YCdeg = YC.*180./pi; 75 | % ZCdeg = ZC.*180./pi; 76 | % mesh(XCdeg,YCdeg,ZCdeg) 77 | 78 | toc 79 | 80 | 81 | -------------------------------------------------------------------------------- /DistBetween2Segment.m: -------------------------------------------------------------------------------- 1 | % Compute the shortest distance between two line. 2 | 3 | function [distance varargout] = DistBetween2Segment(p1, p2, p3, p4) 4 | 5 | u = p1 - p2; 6 | v = p3 - p4; 7 | w = p2 - p4; 8 | 9 | a = dot(u,u); 10 | b = dot(u,v); 11 | c = dot(v,v); 12 | d = dot(u,w); 13 | e = dot(v,w); 14 | D = a*c - b*b; 15 | sD = D; 16 | tD = D; 17 | 18 | SMALL_NUM = 0.00000001; 19 | 20 | % compute the line parameters of the two closest points 21 | if (D < SMALL_NUM) % the lines are almost parallel 22 | sN = 0.0; % force using point P0 on segment S1 23 | sD = 1.0; % to prevent possible division by 0.0 later 24 | tN = e; 25 | tD = c; 26 | else % get the closest points on the infinite lines 27 | sN = (b*e - c*d); 28 | tN = (a*e - b*d); 29 | if (sN < 0.0) % sc < 0 => the s=0 edge is visible 30 | sN = 0.0; 31 | tN = e; 32 | tD = c; 33 | elseif (sN > sD)% sc > 1 => the s=1 edge is visible 34 | sN = sD; 35 | tN = e + b; 36 | tD = c; 37 | end 38 | end 39 | 40 | if (tN < 0.0) % tc < 0 => the t=0 edge is visible 41 | tN = 0.0; 42 | % recompute sc for this edge 43 | if (-d < 0.0) 44 | sN = 0.0; 45 | elseif (-d > a) 46 | sN = sD; 47 | else 48 | sN = -d; 49 | sD = a; 50 | end 51 | elseif (tN > tD) % tc > 1 => the t=1 edge is visible 52 | tN = tD; 53 | % recompute sc for this edge 54 | if ((-d + b) < 0.0) 55 | sN = 0; 56 | elseif ((-d + b) > a) 57 | sN = sD; 58 | else 59 | sN = (-d + b); 60 | sD = a; 61 | end 62 | end 63 | 64 | % finally do the division to get sc and tc 65 | if(abs(sN) < SMALL_NUM) 66 | sc = 0.0; 67 | else 68 | sc = sN / sD; 69 | end 70 | 71 | if(abs(tN) < SMALL_NUM) 72 | tc = 0.0; 73 | else 74 | tc = tN / tD; 75 | end 76 | 77 | % get the difference of the two closest points 78 | dP = w + (sc * u) - (tc * v); % = S1(sc) - S2(tc) 79 | 80 | distance = norm(dP); 81 | outV = dP; 82 | 83 | varargout(1) = {outV}; % vector connecting the closest points 84 | varargout(2) = {p2+sc*u}; % Closest point on object 1 85 | varargout(3) = {p4+tc*v}; % Closest point on object 2 86 | 87 | end -------------------------------------------------------------------------------- /example_all_range_orientation_ws.m: -------------------------------------------------------------------------------- 1 | % Author: Changmeng Hou(Harbin Engineering University) 2 | 3 | % All Range Orientation Workspace. 4 | 5 | clc 6 | clear 7 | tic 8 | 9 | RDL = [5 3 .5 .3 7.2 10]; 10 | [B,P,H] = rdl2bph(RDL); 11 | limbPara = RDL(5:6); 12 | jointCons = [0,pi/4]; 13 | 14 | euler = [0 0 0]; 15 | almax = pi/18; 16 | bemax = pi/18; 17 | gamax = pi/18; 18 | % disStep = 0.02; 19 | disStep = 0.04; 20 | eu_i = 1; 21 | euler(eu_i,:) = [0,0,0]; 22 | for al = -almax:disStep:almax 23 | for be = -bemax:disStep:bemax 24 | for ga = -gamax:disStep:gamax 25 | eu_i = eu_i + 1; 26 | euler(eu_i,:) = [al,be,ga]; 27 | end 28 | end 29 | end 30 | 31 | 32 | zmin = H(1); 33 | zmax = H(2); 34 | % numz = 50; 35 | % numtheta = 90; 36 | numz = 40; 37 | numtheta = 60; 38 | z = linspace(zmin,zmax,numz); 39 | theta = linspace(0,2*pi,numtheta); 40 | 41 | deltaz = (zmax-zmin)/numz; 42 | deltatheta = 2*pi/numtheta; 43 | deltarho = 0.02; 44 | 45 | % Z = zeros(numz,numtheta); 46 | Rhotemp = zeros(numz,numtheta); 47 | % Theta = zeros(numz,numtheta); 48 | % XC = zeros(numz,numtheta); 49 | % YC = zeros(numz,numtheta); 50 | % ZC = zeros(numz,numtheta); 51 | tic 52 | 53 | subvi = 0; 54 | subv = zeros(numz*numtheta,1); 55 | for i =1:numz 56 | for j = 1:numtheta 57 | rho = 0; 58 | while (rho >= 0) 59 | [xc,yc,zc] = pol2cart(theta(j),rho,z(i)); 60 | transTemp = [xc,yc,zc]; 61 | for ei = 1:eu_i 62 | eulerTemp = euler(ei,:); 63 | posTemp = [transTemp,eulerTemp]; 64 | [inWorkspace,isSingular,inBound] = pose_state(posTemp,jointCons,limbPara,B,P); 65 | if ~inWorkspace 66 | break 67 | end 68 | 69 | end 70 | if ~inWorkspace 71 | Rhotemp(i,j) = rho; 72 | subvi = subvi + 1; 73 | subv(subvi) = 1/2*(rho^2)*deltaz*deltatheta; 74 | break 75 | end 76 | rho = rho + deltarho; 77 | 78 | end 79 | end 80 | toc 81 | end 82 | 83 | 84 | [row] = find(all(Rhotemp ~= 0,2)); 85 | if (max(row) ~= numz)&&(min(row) ~= 1) 86 | row = [min(row)-1;row;max(row)+1]; 87 | elseif (max(row) == numz)&&(min(row) ~= 1) 88 | row = [min(row)-1;row]; 89 | elseif (max(row) ~= numz)&&(min(row) == 1) 90 | row = [row;max(row)+1]; 91 | else 92 | 93 | end 94 | 95 | Rho = Rhotemp(row,:); 96 | z = z(row); 97 | [Theta,Z] = meshgrid(theta,z); 98 | [XC,YC,ZC] = pol2cart(Theta,Rho,Z); 99 | mesh(XC,YC,ZC); 100 | workspaceV = sum(subv) 101 | toc 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /example_reachable_ws.m: -------------------------------------------------------------------------------- 1 | % Author: Changmeng Hou(Harbin Engineering University) 2 | 3 | % Reachable Workspace in Orientation Range. 4 | 5 | clc 6 | clear 7 | tic 8 | 9 | RDL = [5 3 .5 .3 7.2 10]; 10 | [B,P,H] = rdl2bph(RDL); 11 | limbPara = RDL(5:6); 12 | jointCons = [0,pi/4]; 13 | 14 | zmin = H(1); 15 | zmax = H(2); 16 | zRange = [zmin,zmax]; 17 | rhoMax = 4; 18 | 19 | alMax = pi/12; 20 | beMax = pi/12; 21 | gaMax = pi/12; 22 | step = 8; 23 | discreteStep = alMax/step; 24 | eu_i = 0; 25 | % initEulerGroup(eu_i,:) = [0 0 0]; 26 | for al = -alMax:discreteStep:alMax 27 | for be = -beMax:discreteStep:beMax 28 | for ga = -gaMax:discreteStep:gaMax 29 | eu_i = eu_i + 1; 30 | initEulerGroup(eu_i,:) = [al,be,ga]; 31 | end 32 | end 33 | end 34 | NewEulerGroup = [[0 0 0];initEulerGroup]; 35 | numz = 30; 36 | numTheta = 30; 37 | numDelta = 30; 38 | z = linspace(zRange(1),zRange(2),numz); 39 | theta = linspace(0,2*pi,numTheta); 40 | 41 | deltaz = (zRange(2)-zRange(1))/numz; 42 | deltaTheta = 2*pi/numTheta; 43 | deltaRho = rhoMax/numDelta; 44 | 45 | rhoTemp = zeros(numz,numTheta); 46 | subVoli = 0; 47 | subVol = zeros(numz*numTheta,1); 48 | tic 49 | zSearchPoint = 0; 50 | V = 0; 51 | for i =1:numz 52 | for j = 1:numTheta 53 | rho = 0; 54 | while (rho < rhoMax) 55 | 56 | [xc,yc,zc] = pol2cart(theta(j),rho,z(i)); 57 | transTemp = [xc,yc,zc]; 58 | % NewEulerGroup = getEulerGroup(theta(j),zSearchPoint,initEulerGroup); 59 | 60 | for ei = 1:eu_i 61 | eulerTemp = NewEulerGroup(ei,:); 62 | posTemp = [transTemp,eulerTemp]; 63 | [inWorkspace,isSingular,inBound] = pose_state(posTemp,jointCons,limbPara,B,P); 64 | if inWorkspace 65 | break 66 | end 67 | end 68 | if ~inWorkspace 69 | rhoTemp(i,j) = rho ; 70 | subV = 1/2*(rho^2)*deltaz*deltaTheta; 71 | V =V+subV; 72 | % subVoli = subVoli + 1; 73 | % subVol(subVoli) = 1/2*(rho^2)*deltaz*deltaTheta; 74 | break 75 | end 76 | rho = rho + deltaRho; 77 | end 78 | end 79 | % i 80 | toc 81 | end 82 | toc 83 | 84 | [row] = find(all(rhoTemp ~= 0,2)); 85 | if (max(row) ~= numz)&&(min(row) ~= 1) 86 | row = [min(row)-1;row;max(row)+1]; 87 | elseif (max(row) == numz)&&(min(row) ~= 1) 88 | row = [min(row)-1;row]; 89 | elseif (max(row) ~= numz)&&(min(row) == 1) 90 | row = [row;max(row)+1]; 91 | else 92 | % do nothing 93 | end 94 | 95 | RHO = rhoTemp(row,:); 96 | z = z(row); 97 | [THETA,Z] = meshgrid(theta,z); 98 | [XC,YC,ZC] = pol2cart(THETA,RHO,Z); 99 | mesh(XC,YC,ZC) 100 | V 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /example_inverse_dynamics.m: -------------------------------------------------------------------------------- 1 | % Author: Changmeng Hou(Harbin Engineering University) 2 | % Reference : Lung-Wen Tsai 3 | % Solving the Inverse Dynamics of a Stewart-Gough Manipulator 4 | % by the Principle of Virtual Work 5 | 6 | clc 7 | clear 8 | tic 9 | % % Initial condition % % 10 | 11 | P = [0.17 0.595 -0.4; -0.6 0.15 -0.4; -0.6 -0.15 -0.4; 0.17 -0.595 -0.4;0.43 -0.445 -0.4;0.43 0.445 -0.4]'; 12 | B = [-2.12 1.374 0; -2.38 1.224 0; -2.38 -1.224 0;-2.12 -1.374 0;0 -0.15 0; 0 0.15 0]'; 13 | mp = 1.5; 14 | m1 = 0.1; 15 | m2 = 0.1; 16 | g = [0 0 -9.80665]'; 17 | I_P = diag([0.08 0.08 0.08]); 18 | I1_i = diag([1/160 1/160 1/160]); 19 | I2_i = diag([1/160 1/160 1/160]); 20 | e1 = 0.5; 21 | e2 = 0.5; 22 | initL = [0 0 0]; 23 | 24 | % input forces exerted at the center of mass of the platform 25 | fe = [0 0 0]'; 26 | ne = [0 0 0]'; 27 | 28 | syms t; 29 | assume(t,'Real') 30 | assumeAlso(t>0) 31 | 32 | w = 3; % unit:rad/s 33 | 34 | % % Motion pattern 1 35 | Trans = [-1.5+0.2*sin(w*t),0.2*sin(w*t),1+0.2*sin(w*t)].'; 36 | Euler = [sym(0),sym(0),sym(0)].'; 37 | 38 | % % Motion pattern 2 39 | % Trans = [-1.5,0,1]'; 40 | % Euler = [sym(0),sym(0),pi/6*sin(w*t)]'; 41 | 42 | % 43 | % % Motion pattern 3 44 | % Trans = [-1.5+0.2*sin(w*t),0,1]'; 45 | % Euler = [sym(0),sym(0),sym(0)]'; 46 | 47 | % % Motion pattern 4 48 | % Trans = [-1.5,0,1].'; 49 | % Euler = [0,0,0].'; 50 | 51 | T = Trans; % position of the platform's mass center 52 | vt = diff(T,t); % velocity of the platform's mass center 53 | at = diff(vt,t); % acceleration of the platform's mass center 54 | 55 | R_PB = rotz(Euler(3))*roty(Euler(2))*rotx(Euler(1)); 56 | % Rotation matrix of frame platform relative to frame base(zyx) 57 | P_B = R_PB*P; % position of (i)st upper ball joint in the fixed frame 58 | L = T + P_B - B; % postion inverse solution 59 | 60 | % % % Allocate space % % % 61 | 62 | d = cell(1,6); % limb length 63 | s = cell(1,6); % unit vector of limb from fixed frame to moving frame 64 | R_iB = cell(1,6); % rotation matrix of limb frame i relative to fixed frame 65 | R_Bi = cell(1,6); % rotation matrix of fixed frame relative to limb frame i 66 | 67 | % % velocity % % 68 | 69 | vp_B = cell(1,6); % velocity of upper joint Pi 70 | vp_i = cell(1,6); % velocity of upper joint Pi in the ith limb frame 71 | vL_i_z = cell(1,6); % limb's velocity in the limb frame(z axis) 72 | omegaL_i = cell(1,6); % angular velocity of the ith limb in the ith limb frame 73 | v1_i = cell(1,6); % velocity of the under limb's in the ith limb frame 74 | v2_i = cell(1,6); % velocity of the upper limb's in the ith limb frame 75 | 76 | % % acceleration % % 77 | 78 | [omegat,alphat] = euler2omealpvec(Euler); 79 | % Euler angular velocity is transformed into angular velocity vector and angular acceleration vector 80 | % omega_p: angular velocity of the mass center of moving platform 81 | % alpha_p: angular acceleration of the mass center of moving platform 82 | 83 | ap_B = cell(1,6); % acceleration of the upper joint 84 | ap_i = cell(1,6); % acceleration of the upper joint in the limb frame 85 | aL_i_z = cell(1,6); % acceleration of the ith limb in the limb frame(z-axis) 86 | alphaL_i = cell(1,6); % angular acceleration of the ith limb in the limb frame 87 | a1_i = cell(1,6); % acceleration of the under limb's in the limb frame 88 | a2_i = cell(1,6); % acceleration of the upper limb's in the limb frame 89 | 90 | % % Jacobi % % 91 | 92 | Jb= cell(1,6); % Jacobi of the ith joint 93 | Jb_i= cell(1,6); % Jacobi of the ith limb 94 | 95 | J1_i = cell(1,6); % Jacobi of the ith under limb 96 | J2_i = cell(1,6); % Jacobi of the ith upper limb 97 | 98 | JP = sym('JL',[6 6]); 99 | Jx = sym('Jx',[6 6]); % Jacobi of the ith limbs(limbframe x axis) 100 | Jy = sym('Jy',[6 6]); % Jacobi of the ith limbs(limbframe y axis) 101 | 102 | % % Force and Movement % % 103 | 104 | f1 = cell(1,6); % resulant of applied and inertia forces exerted at the ith under limb 105 | n1 = cell(1,6); % resulant of applied and inertia moments exerted at the ith under limb 106 | F1_i = cell(1,6); % wrenches F1 = [f1;n1] 107 | 108 | f2 = cell(1,6); % resulant of applied and inertia forces exerted at the ith upper limb 109 | n2 = cell(1,6); % resulant of applied and inertia moments exerted at the ith upper limb 110 | F2_i = cell(1,6); % wrenches F2 = [f2;n2] 111 | 112 | I_B = R_PB*I_P*R_PB'; 113 | fp = fe+mp*g-mp*at; % resulant of applied and inertia forces exerted at the mass center of platform 114 | np = ne-I_B*alphat-cross(omegat,(I_B*omegat)); 115 | % resulant of applied and inertia moments exerted at the mass center of platform 116 | Fp = [ fp ; 117 | np ]; 118 | 119 | Fx = sym('Fx',[6 1]); % the forces in the x-axis of all limbs in the limb frame 120 | Fy = sym('Fy',[6 1]); % the forces in the y-axis of all limbs in the limb frame 121 | Fz = sym('Fz',[6 1]); % the forces in the z-axis of all limbs in the limb frame 122 | 123 | for i = 1:6 124 | 125 | % limb length 126 | d{i} = norm(L(:,i)); 127 | % unit vector of limb 128 | s{i} = L(:,i)/d{i}; 129 | 130 | % rotation matrix of limb frame i relative to fixed frame 131 | ct = s{i}(3); 132 | st = sqrt(s{i}(1)^2+s{i}(2)^2); 133 | sf = s{i}(2)/st; 134 | cf = s{i}(1)/st; 135 | R_iB{i} = [ 136 | -cf*ct -sf cf*st; 137 | sf*ct cf sf*st; 138 | -st 0 ct ; 139 | ]; 140 | R_Bi{i} = R_iB{i}.'; 141 | 142 | % % velocity % % 143 | 144 | % velocity of the ith upper joint 145 | vp_B{i} = vt+cross(omegat,P_B(:,i)); 146 | vp_i{i} = R_Bi{i}*vp_B{i}; 147 | % velocity of the ith limb in the ith limb frame (z-axis) 148 | vL_i_z{i} = vp_i{i}(3); 149 | % angular velocity of the ith limb in the ith limb frame 150 | omegaL_i{i} = 1/d{i}*(cross([0 0 1]',vp_i{i})); 151 | % velocity of the under limb's in the ith limb frame 152 | v1_i{i} = e1*cross(omegaL_i{i},[0,0,1]'); 153 | % velocity of the upper limb's in the ith limb frame 154 | v2_i{i} = (d{i} - e2)*cross(omegaL_i{i},[0,0,1]')+vL_i_z{i}*[0 0 1]'; 155 | 156 | % % acceleration % % 157 | 158 | % acceleration of the ith upper joint 159 | ap_B{i} = at + cross(alphat,P_B(:,i))+cross(omegat,cross(omegat,P_B(:,i))); 160 | ap_i{i} = R_Bi{i}*ap_B{i}; 161 | % acceleration of the ith limb in the ith limb frame (z-axis) 162 | aL_i_z{i} = diff(vL_i_z{i},t) + d{i}*dot(omegaL_i{i},omegaL_i{i}); 163 | % angular acceleration of the ith limb in the limb frame 164 | alphaL_i{i} = 1/d{i}*cross([0 0 1]',ap_i{i})... 165 | -2*vL_i_z{i}/d{i}*omegaL_i{i}; 166 | % acceleration of the ith under limb's in the limb frame 167 | a1_i{i} = e1*cross(alphaL_i{i},[0 0 1]') + e1*cross(omegaL_i{i},cross(omegaL_i{i},[0 0 1]')); 168 | % acceleration of the ith upper limb's in the limb frame 169 | a2_i{i} = aL_i_z{i}*[0 0 1]'+(d{i} - e2)*cross(alphaL_i{i},[0 0 1]')+... 170 | (d{i}-e2)*cross(omegaL_i{i},cross(omegaL_i{i},[0 0 1]'))+... 171 | 2*vL_i_z{i}*cross(omegaL_i{i},[0 0 1]'); 172 | 173 | % % force moments wrenches %% 174 | 175 | % resulant of applied and inertia forces exerted at the ith under limb 176 | f1{i} = m1*R_Bi{i}*g - m1*a1_i{i}; 177 | % % moments 178 | n1{i} = -I1_i*alphaL_i{i} - cross(omegaL_i{i},(I1_i*omegaL_i{i})); 179 | % % wrenches 180 | F1_i{i} = [ 181 | f1{i}; 182 | n1{i} 183 | ]; 184 | % resulant of applied and inertia forces exerted at the ith upper limb 185 | f2{i} = m2*R_Bi{i}*g - m2*a2_i{i}; 186 | % % moments 187 | n2{i} = -I2_i*alphaL_i{i} - cross(omegaL_i{i},(I2_i*omegaL_i{i})); 188 | % % wrenches 189 | F2_i{i} = [ 190 | f2{i}; 191 | n2{i} 192 | ]; 193 | % the forces in the x-axis of all limbs in the limb frame 194 | Fx(i,1) = 1/d{i}*(e1*f1{i}(1)+(d{i}-e2)*f2{i}(1)+n1{i}(2)+n2{i}(2)); 195 | % % y-axis 196 | Fy(i,1) = 1/d{i}*(e1*f1{i}(2)+(d{i}-e2)*f2{i}(2)+n1{i}(1)+n2{i}(1)); 197 | % % z-axis 198 | Fz(i,1) = f2{i}(3); 199 | 200 | % % Jacobi % % 201 | 202 | % Jacobi of the ith upper joint 203 | Jb{i} = [eye(3),-skew(P_B(:,i))]; 204 | % Jacobi of the ith limb(in the limb frame) 205 | Jb_i{i} = R_Bi{i}*Jb{i}; 206 | % Jacobi of manipulator 207 | JP(i,:) = Jb_i{i}(3,:); 208 | % Jacobi of the ith limbs(limb frame x-axis) 209 | Jx(i,:) = Jb_i{i}(1,:); 210 | % % y-axis 211 | Jy(i,:) = Jb_i{i}(2,:); 212 | 213 | end 214 | toc 215 | 216 | % % plot figure % % 217 | 218 | f = figure('Position',[560 75 475 680]); 219 | % 220 | timeInterval = [0,4*pi/w]; 221 | % 222 | subplot(4,1,1); 223 | fplot(d,timeInterval); 224 | legend('d_1','d_2','d_3','d_4','d_5','d_6'); 225 | title('Leg length') 226 | xlabel('t(s)') 227 | ylabel('length(m)') 228 | 229 | subplot(4,1,2) 230 | fplot(vL_i_z,timeInterval); 231 | legend('v_1','v_2','v_3','v_4','v_5','v_6'); 232 | title('Velocity') 233 | xlabel('t(s)') 234 | ylabel('velocity(m/s)') 235 | 236 | subplot(4,1,3) 237 | fplot(aL_i_z,timeInterval); 238 | legend('a_1','a_2','a_3','a_4','a_5','a_6'); 239 | title('Accleration') 240 | xlabel('t(s)') 241 | ylabel('accleration(m/s^2)') 242 | 243 | toc 244 | % the first dynamics inverse solution 245 | 246 | % Symbol calculation takes too long, so convert it to numerical value to speed up the calculation 247 | % tau1 = -Fz - (Jz.')\(Fp+(Jx.')*Fx+(Jy.')*Fy); % Fmula 248 | 249 | n = 20; 250 | T = linspace(0,timeInterval(2),n); 251 | tau1 = []; 252 | for tt = T 253 | 254 | FxTemp = double(subs(Fx,t,tt)); 255 | FyTemp = double(subs(Fy,t,tt)); 256 | FzTemp = double(subs(Fz,t,tt)); 257 | FpTemp = double(subs(Fp,t,tt)); 258 | 259 | JxTemp = double(subs(Jx,t,tt)); 260 | JyTemp = double(subs(Jy,t,tt)); 261 | JpTemp = double(subs(JP,t,tt)); 262 | 263 | % Calculate the force at the current moment using the same formula as noted above 264 | tauTemp = -FzTemp - (JpTemp.')\(FpTemp+(JxTemp.')*FxTemp+(JyTemp.')*FyTemp); 265 | tau1 = [tau1,tauTemp]; 266 | end 267 | toc 268 | 269 | subplot(4,1,4) 270 | plot(T,tau1.'); 271 | legend('f_1','f_2','f_3','f_4','f_5','f_6') 272 | title('Dynamics') 273 | xlabel('t(s)') 274 | ylabel('Force(N)') 275 | 276 | function [omega_p,alpha_p] = euler2omealpvec(euler) 277 | % This function convert Euler to omega and alpha vectors 278 | % Syntax : 279 | % [omega_p,alpha_p] = euler2omealpvec(Euler) 280 | % Input : 281 | % Euler - the euler angle 282 | % Output : 283 | % omega_p - the angle velocity vector in this Euler 284 | % alpha_p - the angle accleration vector in this Euler 285 | % Reference ： Liu's Book 286 | 287 | syms t; 288 | assume(t,'Real') 289 | assumeAlso(t>0) 290 | 291 | alpha = euler(1); 292 | beta = euler(2); 293 | gamma = euler(3); 294 | 295 | cb = cos(beta); 296 | cg = cos(gamma); 297 | 298 | sb = sin(beta); 299 | sg = sin(gamma); 300 | 301 | da = diff(alpha,t); 302 | db = diff(beta,t); 303 | dg = diff(gamma,t); 304 | 305 | dda = diff(da,t); 306 | ddb = diff(db,t); 307 | ddg = diff(dg,t); 308 | 309 | 310 | U = [ 311 | cg*cb -sg 0; 312 | sg*cb cg 0; 313 | -sb 0 1 314 | ]; 315 | omega_p = U*[da db dg]'; 316 | W = [ 317 | -dg*sg*cb-db*cg*sb -dg*cg 0; 318 | dg*cg*cb-db*sg*sb -dg*sg 0; 319 | -db*cb 0 0 320 | ]; 321 | alpha_p = W*[da db dg]'+U*[dda ddb ddg]'; 322 | 323 | end 324 | 325 | function S = skew(v) 326 | if isvector(v) 327 | if (length(v)==3) 328 | % SO(3) case 329 | S = [ 0 -v(3) v(2) 330 | v(3) 0 -v(1) 331 | -v(2) v(1) 0]; 332 | elseif (length(v)==2) 333 | % SO(2) case 334 | S = [0 -v; v 0]; 335 | else 336 | error('SMTB:skew:badarg', 'argument must be a 1- or 3-vector'); 337 | end 338 | else 339 | error('SMTB:skew:badarg', 'argument must be a 1- or 3-vector'); 340 | end 341 | end 342 | --------------------------------------------------------------------------------