作業五
1.1~1.3：
程式body(畫手的資料已經包含在內)
-------------------------------------------------------------------------------------------------
function body(L1,L2,L3,theta1,theta2,theta3)
% 手臂長L1公分、手肘長L2公分、手掌長L3公分、角度算法詳見機動學作業網頁上的圖示
% 繪圖用的角度
p0=linspace(0.5*pi,2.5*pi,13);
p1=linspace(0.5*pi,1.5*pi,7);
p2=linspace(1.5*pi,2.5*pi,7);
p3=linspace(1.5*pi,1.75*pi,10);
p4=linspace(1.75*pi,2.75*pi,7);
% 我的肩膀寬10公分、手肘寬7公分、手腕厚4公分、手指粗2公分、手臂長30公分、手肘長24公分、手掌長19公分，本程式可自行輸入手長度。
% D1=10;
% D2=7;
% D3=4;
% D4=2;
% L1=30;
% L2=24;
% L3=19;
% 假設肩膀寬10公分、手肘寬7公分、手腕厚4公分、手指粗2公分
D1=10;
D2=7;
D3=4;
D4=2;
% 手臂形狀
arm1=[(D1/4)*cos(p0) 0 (D1/2)*cos(p1) L1 L1+(D2/2)*cos(p2) L1 L1+(D2/4)*cos(p0) L1 0 0 ;...
(D1/4)*sin(p0) (D1/2) (D1/2)*sin(p1) (-D2/2) (D2/2)*sin(p2) (D2/4) (D2/4)*sin(p0) (D2/2) (D1/2) (D1/4)];
% 手肘形狀
arm2=[ 0 (D2/2)*cos(p1) L2 L2+(D3/2)*cos(p2) L2 0 ;...
(D2/2) (D2/2)*sin(p1) (-D3/2) (D3/2)*sin(p2) (D3/2) (D2/2)];
% 手掌形狀（含上彎的手指）
palm=[L3/2 L3/2+(D4/2)*cos(p2) L3/2 L3/2+(D4/4)*cos(p0) L3/2 0 0 (D3/4)*cos(p0) 0 (D3/2)*cos(p1) L3/2 ...
L3/2+(D4/2)*cos(p3) L3/2+(D4/2)*cosd(315)+(L3/2)*cosd(45) L3/2+(L3/2)*cosd(45)+(D4/2)*cos(p4) L3/2 ;...
(-D4/2) (D4/2)*sin(p2) (D4/4) (D4/4)*sin(p0) (D4/2) (D3/2) (D3/4) (D3/4)*sin(p0) (D3/2) (D3/2)*sin(p1) (-D4/2) ...
(D4/2)*sin(p3) (D4/2)*sind(315)+(L3/2)*sind(45) (L3/2)*sind(45)+(D4/2)*sin(p4) D4/2];
% 把相對夾角換成絕對角度
phi1=-theta1;
phi2=phi1+(theta2-180);
phi3=phi2+(theta3-180);
% 手肘、手臂、手掌的旋轉矩陣
rotateArm1=[cosd(phi1) -sind(phi1); sind(phi1) cosd(phi1)];
rotateArm2=[cosd(phi2) -sind(phi2); sind(phi2) cosd(phi2)];
rotatePalm=[cosd(phi3) -sind(phi3); sind(phi3) cosd(phi3)];
% 執行旋轉動作
arm1=rotateArm1*arm1;
arm2=rotateArm2*arm2;
palm=rotatePalm*palm;
% 求出手臂和手肘、手肘和手掌之連接點（bearing）位置
bearing12=[L1*cosd(phi1) ; L1*sind(phi1)];
bearing23=[L1*cosd(phi1)+L2*cosd(phi2) ; L1*sind(phi1)+L2*sind(phi2)];
% 將手肘及手掌平移，把bearings連接好
arm2(1,:)=arm2(1,:)+bearing12(1,1);
arm2(2,:)=arm2(2,:)+bearing12(2,1);
palm(1,:)=palm(1,:)+bearing23(1,1);
palm(2,:)=palm(2,:)+bearing23(2,1);
% 轉置成x座標&y座標為column vector
arm1=arm1';
arm2=arm2';
palm=palm';
% 繪圖
axis equal;
line(arm1(:,1),arm1(:,2),'color','b','erasemode','background','linewidth',3);
line(arm2(:,1),arm2(:,2),'color','b','erasemode','background','linewidth',2);
line(palm(:,1),palm(:,2),'color','b','erasemode','background','linewidth',1);
----------------------------------------------------------------------------------------------------------------------------
程式bodyNew(畫手的資料已經包含在內)(用rotate指令重新寫過的)
-------------------------------------------------------------------------------------------------
function bodyNew(L1,L2,L3,theta1,theta2,theta3)
% 手臂長L1公分、手肘長L2公分、手掌長L3公分、角度算法詳見機動學作業網頁上的圖示
axis equal;
% 繪圖用的角度
p0=linspace(0.5*pi,2.5*pi,13);
p1=linspace(0.5*pi,1.5*pi,7);
p2=linspace(1.5*pi,2.5*pi,7);
p3=linspace(1.5*pi,1.75*pi,10);
p4=linspace(1.75*pi,2.75*pi,7);
% 我的肩膀寬10公分、手肘寬7公分、手腕厚4公分、手指粗2公分、手臂長30公分、手肘長24公分、手掌長19公分，本程式可自行輸入手長度。
% D1=10;
% D2=7;
% D3=4;
% D4=2;
% L1=30;
% L2=24;
% L3=19;
% 假設肩膀寬10公分、手肘寬7公分、手腕厚4公分、手指粗2公分
D1=10;
D2=7;
D3=4;
D4=2;
% 把相對夾角換成絕對角度
phi1=-theta1;
phi2=phi1+(theta2-180);
phi3=phi2+(theta3-180);
% 求出手臂和手肘、手肘和手掌之連接點（bearing）位置
bearing12=[L1*cosd(phi1) L1*sind(phi1)];
bearing23=[L1*cosd(phi1)+L2*cosd(phi2) L1*sind(phi1)+L2*sind(phi2)];
% 手臂形狀
arm1=[(D1/4)*cos(p0) 0 (D1/2)*cos(p1) L1 L1+(D2/2)*cos(p2) L1 L1+(D2/4)*cos(p0) L1 0 0 ;...
(D1/4)*sin(p0) (D1/2) (D1/2)*sin(p1) (-D2/2) (D2/2)*sin(p2) (D2/4) (D2/4)*sin(p0) (D2/2) (D1/2) (D1/4)];
% 將圖形存到handle裡
arm1=arm1';
arm1Fig=line(arm1(:,1),arm1(:,2),'color','b','linewidth',3);
% 手肘形狀
arm2=[ 0 (D2/2)*cos(p1) L2 L2+(D3/2)*cos(p2) L2 0 ;...
(D2/2) (D2/2)*sin(p1) (-D3/2) (D3/2)*sin(p2) (D3/2) (D2/2)];
% 把手肘放到連接點上
arm2(1,:)=arm2(1,:)+bearing12(1,1);
arm2(2,:)=arm2(2,:)+bearing12(1,2);
% 將圖形存到handle裡
arm2=arm2';
arm2Fig=line(arm2(:,1),arm2(:,2),'color','b','linewidth',2);
% 手掌形狀（含上彎的手指）
palm=[L3/2 L3/2+(D4/2)*cos(p2) L3/2 L3/2+(D4/4)*cos(p0) L3/2 0 0 (D3/4)*cos(p0) 0 (D3/2)*cos(p1) L3/2 ...
L3/2+(D4/2)*cos(p3) L3/2+(D4/2)*cosd(315)+(L3/2)*cosd(45) L3/2+(L3/2)*cosd(45)+(D4/2)*cos(p4) L3/2 ;...
(-D4/2) (D4/2)*sin(p2) (D4/4) (D4/4)*sin(p0) (D4/2) (D3/2) (D3/4) (D3/4)*sin(p0) (D3/2) (D3/2)*sin(p1) (-D4/2) ...
(D4/2)*sin(p3) (D4/2)*sind(315)+(L3/2)*sind(45) (L3/2)*sind(45)+(D4/2)*sin(p4) D4/2];
% 把手掌放到連接點上
palm(1,:)=palm(1,:)+bearing23(1,1);
palm(2,:)=palm(2,:)+bearing23(1,2);
% 將圖形存到handle裡
palm=palm';
palmFig=line(palm(:,1),palm(:,2),'color','b','linewidth',1);
axis off;
% 旋轉手肘以接在一起
rotate(arm1Fig,[0 0 1],phi1,[0 0 0]);
rotate(arm2Fig,[0 0 1],phi2,[bearing12 0]);
rotate(palmFig,[0 0 1],phi3,[bearing23 0]);
----------------------------------------------------------------------------------------------------------------------------
1.4：
程式homework5_1_4
-------------------------------------------------------------------------------------------------
% 人正面朝向+x軸
% 手臂長L1=30公分、手肘長L2=26公分、手掌長L3=19公分、角度算法詳見機動學作業網頁上的圖示
L1=30;
L2=26;
L3=19;
theta1=-90;
theta2=-45;
theta3=-30;
% 繪圖用的角度
p0=linspace(0.5*pi,2.5*pi,13);
p1=linspace(0.5*pi,1.5*pi,7);
p2=linspace(1.5*pi,2.5*pi,7);
p3=linspace(1.5*pi,1.75*pi,10);
p4=linspace(1.75*pi,2.75*pi,7);
% 假設肩膀寬10公分、手肘寬7公分、手腕厚4公分、手指粗2公分
D1=10;
D2=7;
D3=4;
D4=2;
% 手臂形狀
arm1=[(D1/4)*cos(p0) 0 (D1/2)*cos(p1) L1 L1+(D2/2)*cos(p2) L1 L1+(D2/4)*cos(p0) L1 0 0 ;...
(D1/4)*sin(p0) (D1/2) (D1/2)*sin(p1) (-D2/2) (D2/2)*sin(p2) (D2/4) (D2/4)*sin(p0) (D2/2) (D1/2) (D1/4)];
% 手肘形狀
arm2=[ 0 (D2/2)*cos(p1) L2 L2+(D3/2)*cos(p2) L2 0 ;...
(D2/2) (D2/2)*sin(p1) (-D3/2) (D3/2)*sin(p2) (D3/2) (D2/2)];
% 手掌形狀（含上彎的手指）
palm=[L3/2 L3/2+(D4/2)*cos(p2) L3/2 L3/2+(D4/4)*cos(p0) L3/2 0 0 (D3/4)*cos(p0) 0 (D3/2)*cos(p1) L3/2 ...
L3/2+(D4/2)*cos(p3) L3/2+(D4/2)*cosd(315)+(L3/2)*cosd(45) L3/2+(L3/2)*cosd(45)+(D4/2)*cos(p4) L3/2 ;...
(-D4/2) (D4/2)*sin(p2) (D4/4) (D4/4)*sin(p0) (D4/2) (D3/2) (D3/4) (D3/4)*sin(p0) (D3/2) (D3/2)*sin(p1) (-D4/2) ...
(D4/2)*sin(p3) (D4/2)*sind(315)+(L3/2)*sind(45) (L3/2)*sind(45)+(D4/2)*sin(p4) D4/2];
% 把相對夾角換成絕對角度
phi1=-theta1;
phi2=phi1+(theta2-180);
phi3=phi2+(theta3-180);
% 手肘、手臂、手掌的旋轉矩陣
rotateArm1=[cosd(phi1) -sind(phi1); sind(phi1) cosd(phi1)];
rotateArm2=[cosd(phi2) -sind(phi2); sind(phi2) cosd(phi2)];
rotatePalm=[cosd(phi3) -sind(phi3); sind(phi3) cosd(phi3)];
% 執行旋轉動作
arm1=rotateArm1*arm1;
arm2=rotateArm2*arm2;
palm=rotatePalm*palm;
% 求出手臂和手肘、手肘和手掌之連接點（bearing）位置
bearing12=[L1*cosd(phi1) ; L1*sind(phi1)];
bearing23=[L1*cosd(phi1)+L2*cosd(phi2) ; L1*sind(phi1)+L2*sind(phi2)];
% 將手肘及手掌平移，把bearings連接好，並存到畫圖專用的暫存變數內
tempArm1=arm1;
tempArm2=arm2;
tempPalm=palm;
tempArm2(1,:)=tempArm2(1,:)+bearing12(1,1);
tempArm2(2,:)=tempArm2(2,:)+bearing12(2,1);
tempPalm(1,:)=tempPalm(1,:)+bearing23(1,1);
tempPalm(2,:)=tempPalm(2,:)+bearing23(2,1);
% 轉置成x座標&y座標為column vector
tempArm1=tempArm1';
tempArm2=tempArm2';
tempPalm=tempPalm';
% 存進handle並繪圖
axis equal;
arm1Fig=line(tempArm1(:,1),tempArm1(:,2),'color','b','erasemode','xor');
arm2Fig=line(tempArm2(:,1),tempArm2(:,2),'color','b','erasemode','xor');
arm3Fig=line(tempPalm(:,1),tempPalm(:,2),'color','b','erasemode','xor');
drawnow;
pause;
axis equal;
% 進入for迴圈
for k=1:1:10
% 手肘、手臂、手掌的旋轉矩陣（重新定義rotateArm矩陣）
rotateArm1=[cosd(-1.5) -sind(-1.5); sind(-1.5) cosd(-1.5)]; %theta1一次變+1.5度，由line 30知應乘負號
rotateArm2=[cosd(-1.5+1) -sind(-1.5+1); sind(-1.5+1) cosd(-1.5+1)]; %theta2一次變+1度，由line 31知應乘正號
rotatePalm=[cosd(-1.5+1+2) -sind(-1.5+1+2); sind(-1.5+1+2) cosd(-1.5+1+2)]; %theta3一次變+2度，由line 32知應乘正號
% 執行旋轉動作
arm1=rotateArm1*arm1;
arm2=rotateArm2*arm2;
palm=rotatePalm*palm;
% 求出手臂和手肘、手肘和手掌之連接點（bearing）位置（重新定義bearing矩陣）
bearing12=[L1*cosd(phi1-1.5*k) ; L1*sind(phi1-1.5*k)];
bearing23=[L1*cosd(phi1-1.5*k)+L2*cosd(phi2+(-1.5+1)*k) ; L1*sind(phi1-1.5*k)+L2*sind(phi2+(-1.5+1)*k)];
% 將手肘及手掌平移，把bearings連接好，並存到畫圖專用的暫存變數內
tempArm1=arm1;
tempArm2=arm2;
tempPalm=palm;
tempArm2(1,:)=tempArm2(1,:)+bearing12(1,1);
tempArm2(2,:)=tempArm2(2,:)+bearing12(2,1);
tempPalm(1,:)=tempPalm(1,:)+bearing23(1,1);
tempPalm(2,:)=tempPalm(2,:)+bearing23(2,1);
% 轉置成x座標&y座標為column vector
tempArm1=tempArm1';
tempArm2=tempArm2';
tempPalm=tempPalm';
% 重新設定handle並繪圖
axis equal;
set(arm1Fig,'xdata',tempArm1(:,1),'ydata',tempArm1(:,2));
set(arm2Fig,'xdata',tempArm2(:,1),'ydata',tempArm2(:,2));
set(arm3Fig,'xdata',tempPalm(:,1),'ydata',tempPalm(:,2));
drawnow;
pause(1);
axis equal;
end;
% 由圖可知：手腕彎曲角度過大，實際上人手無法彎到如此地步。
----------------------------------------------------------------------------------------------------------------------------
程式homework5_1_4New(用rotate指令重寫過的)
-------------------------------------------------------------------------------------------------
axis equal;
axis([-10,40,-10,50]);
% 人正面朝向+x軸
% 手臂長L1=30公分、手肘長L2=26公分、手掌長L3=19公分、角度算法詳見機動學作業網頁上的圖示
L1=30;
L2=26;
L3=19;
theta1=-90;
theta2=-45;
theta3=-30;
% 繪圖用的角度
p0=linspace(0.5*pi,2.5*pi,13);
p1=linspace(0.5*pi,1.5*pi,7);
p2=linspace(1.5*pi,2.5*pi,7);
p3=linspace(1.5*pi,1.75*pi,10);
p4=linspace(1.75*pi,2.75*pi,7);
% 假設肩膀寬10公分、手肘寬7公分、手腕厚4公分、手指粗2公分
D1=10;
D2=7;
D3=4;
D4=2;
% 把相對夾角換成絕對角度
phi1=-theta1;
phi2=phi1+(theta2-180);
phi3=phi2+(theta3-180);
% 求出手臂和手肘、手肘和手掌之連接點（bearing）位置
bearing12=[L1*cosd(phi1) L1*sind(phi1)];
bearing23=[L1*cosd(phi1)+L2*cosd(phi2) L1*sind(phi1)+L2*sind(phi2)];
% 手臂形狀
arm1=[(D1/4)*cos(p0) 0 (D1/2)*cos(p1) L1 L1+(D2/2)*cos(p2) L1 L1+(D2/4)*cos(p0) L1 0 0 ;...
(D1/4)*sin(p0) (D1/2) (D1/2)*sin(p1) (-D2/2) (D2/2)*sin(p2) (D2/4) (D2/4)*sin(p0) (D2/2) (D1/2) (D1/4)];
% 將圖形存到handle裡
arm1=arm1';
arm1Fig=line(arm1(:,1),arm1(:,2),'color','b','linewidth',3);
% 手肘形狀
arm2=[ 0 (D2/2)*cos(p1) L2 L2+(D3/2)*cos(p2) L2 0 ;...
(D2/2) (D2/2)*sin(p1) (-D3/2) (D3/2)*sin(p2) (D3/2) (D2/2)];
% 把手肘放到連接點上
arm2(1,:)=arm2(1,:)+bearing12(1,1);
arm2(2,:)=arm2(2,:)+bearing12(1,2);
% 將圖形存到handle裡
arm2=arm2';
arm2Fig=line(arm2(:,1),arm2(:,2),'color','b','linewidth',2);
% 手掌形狀（含上彎的手指）
palm=[L3/2 L3/2+(D4/2)*cos(p2) L3/2 L3/2+(D4/4)*cos(p0) L3/2 0 0 (D3/4)*cos(p0) 0 (D3/2)*cos(p1) L3/2 ...
L3/2+(D4/2)*cos(p3) L3/2+(D4/2)*cosd(315)+(L3/2)*cosd(45) L3/2+(L3/2)*cosd(45)+(D4/2)*cos(p4) L3/2 ;...
(-D4/2) (D4/2)*sin(p2) (D4/4) (D4/4)*sin(p0) (D4/2) (D3/2) (D3/4) (D3/4)*sin(p0) (D3/2) (D3/2)*sin(p1) (-D4/2) ...
(D4/2)*sin(p3) (D4/2)*sind(315)+(L3/2)*sind(45) (L3/2)*sind(45)+(D4/2)*sin(p4) D4/2];
% 把手掌放到連接點上
palm(1,:)=palm(1,:)+bearing23(1,1);
palm(2,:)=palm(2,:)+bearing23(1,2);
% 將圖形存到handle裡
palm=palm';
palmFig=line(palm(:,1),palm(:,2),'color','b','linewidth',1);
axis off;
axis equal;
% 旋轉手肘以接在一起
rotate(arm1Fig,[0 0 1],phi1,[0 0 0]);
rotate(arm2Fig,[0 0 1],phi2,[bearing12 0]);
rotate(palmFig,[0 0 1],phi3,[bearing23 0]);
axis equal;
pause;
for k=1:1:10
axis equal;
% 求出新的手臂和手肘、手肘和手掌之連接點（bearing）位置
bearing12=[L1*cosd(phi1-1.5*k) L1*sind(phi1-1.5*k)];
bearing23=[L1*cosd(phi1-1.5*k)+L2*cosd(phi2+(-1.5+1)*k) L1*sind(phi1-1.5*k)+L2*sind(phi2+(-1.5+1)*k)]; % -1.5+1是因為角度要等於自己和所有之前的連桿角度相加
% 先動手臂(手肘手掌一起動)
rotate(arm1Fig,[0 0 1],(-1.5),[0 0 0]);
rotate(arm2Fig,[0 0 1],(-1.5),[0 0 0]);
rotate(palmFig,[0 0 1],(-1.5),[0 0 0]);
% 再動手肘(手臂已就定位，手掌一起動)
rotate(arm2Fig,[0 0 1],(1),[bearing12 0]);
rotate(palmFig,[0 0 1],(1),[bearing12 0]);
% 最後動手掌(手臂手肘都已就定位)
rotate(palmFig,[0 0 1],(2),[bearing23 0]);
pause(0.5);
end;
% 由圖可知：手腕彎曲角度過大，實際上人手無法彎到如此地步。
-----------------------------------------------------------------------------------------------------------------------
用rotate指令的優點是：可以使程式碼更簡潔，同時避免掉用handle畫圖時因為erasemode為'xor'產生的圖形抵銷。
----------------------------------------------------------------------------------------------------------------------
2.1~2.2：
程式limitOfFinger(有關題目的描述、分析、解釋，皆已寫入下列的程式碼中)
-------------------------------------------------------------------------------------------------
圖一
圖二
圖三
% 手指第一節固定在手掌上，故只有旋轉的自由度，自由度為1。
% 第二節跟第一節的銜接處可視為旋轉結，因旋轉節拘束度為2，故整個系統自由度加1，變成2。
% 第三節與第二節的情況亦同，故整個系統自由度再加1，變成3。
% 自由度為3告訴我們：手指頭可以移動到任意位置！且手指有3節恰好是構成自由度大於等於3的最少節數！
% 但實際上手指各節皆不是真正的旋轉結，無法作360度的旋轉。
% 手掌與第一節的旋轉結約可旋轉110度；第一節與第二節的旋轉節約可旋轉130度；第二節與第三節的旋轉節約可旋轉90度。(以我自己的手指作為測量物)
% 且手指有一定長度，故其只能在長度能到達且與手掌同向(與手背反向)的範圍內運動。
% 除了大拇指，其餘四指的情況皆可以上述模型與限制模擬之。
% 以上皆只考慮手指在平面上的運動，左右的移動不討論。
% 以圖一為初位置，圖二為末位置。
% 以指節間向量夾角為角度，逆時針為正。(第一節則取與x軸夾角)
% (定義：向量起點為該指節與前一指節的bearing，向量終點為該指節與下一指節的bearing)
% 我的手指長度：
% [第一節長度, 第二節長度, 第三節長度, 第一節初角度/末角度, 第二節初角度/末角度, 第三節初角度/末角度](長度：公分 / 角度：度)
% 食指：[6, 3.5, 2.5, -10/100, -110/20, -90/0]
% 中指：[7, 4, 2.5, -10/100, -110/20, -90/0]
% 無名指：[6, 3, 2.5, -10/100, -110/20, -90/0]
% 小指：[4.5, 2.5, 2, -10/100, -110/20, -90/0]
% 以下為手指極限位置程式
function limitOfFinger(L1,L2,L3,THETAi1,THETAi2,THETAi3,THETAf1,THETAf2,THETAf3)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 繪出手指頭能到達的區域 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 指頭最多能到多遠：手指總長度
L=L1+L2+L3;
% 手指全部伸直時可以到達的末角度：三節末角度加總
THETAf=THETAf1+THETAf2+THETAf3;
% 手指全部伸直時無法向下完全貼合在手掌，最遠情況如圖三：第一節初角度+第二節初角度+0度
THETAi=THETAi1+THETAi2;
% 做出指頭最遠距離的點
phi0=(THETAi+THETAi3):1:THETAi;
phi1=THETAi:1:THETAi1;
phi2=THETAi1:1:THETAf1;
phi3=THETAf1:1:THETAf;
phi0=phi0*(pi/180);
phi1=phi1*(pi/180);
phi2=phi2*(pi/180);
phi3=phi3*(pi/180);
temppts0=L3*exp(i*phi0);
temppts0(:)=temppts0(:)+L1*exp(i*THETAi1*(pi/180))+L2*exp(i*THETAi*(pi/180));
temppts1=(L2+L3)*exp(i*phi1);
temppts1(:)=temppts1(:)+L1*exp(i*THETAi1*(pi/180));
temppts2=(L2+L3)*exp(i*phi3);
temppts2(:)=temppts2(:)+L1*exp(i*THETAf1*(pi/180));
fingerTip=[0 temppts0 temppts1 L*exp(i*phi2) temppts2 0];
% 把點從極座標換成直角座標
rFinger=abs(fingerTip);
thetaFinger=angle(fingerTip);
[X,Y]=pol2cart(thetaFinger,rFinger);
% 填上黃色處即為手指頭可以到達的地方
fill(X',Y','Y');
axis equal;
grid on;
pause;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 繪出手指頭能到達的區域 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 沿著極限位置的運動情形 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 求出連接點（bearing）位置
bearing12=[L1*cosd(THETAi1) L1*sind(THETAi1)];
bearing23=[L1*cosd(THETAi1)+L2*cosd(THETAi) L1*sind(THETAi1)+L2*sind(THETAi)];
% 將手指圖形存在handle裡
finger1=linkpoints([0 0],[L1 0], 1);
finger1=finger1';
finger1Fig=line(finger1(:,1),finger1(:,2),'color','b','linewidth',1);
finger2=linkpoints([0 0],[L2 0], 1);
finger2(1,:)=finger2(1,:)+bearing12(1,1);
finger2(2,:)=finger2(2,:)+bearing12(1,2);
finger2=finger2';
finger2Fig=line(finger2(:,1),finger2(:,2),'color','b','linewidth',1);
finger3=linkpoints([0 0],[L3 0], 1);
finger3(1,:)=finger3(1,:)+bearing23(1,1);
finger3(2,:)=finger3(2,:)+bearing23(1,2);
finger3=finger3';
finger3Fig=line(finger3(:,1),finger3(:,2),'color','b','linewidth',1);
% 旋轉手肘以接在一起
rotate(finger1Fig,[0 0 1],THETAi1,[0 0 0]);
rotate(finger2Fig,[0 0 1],(THETAi1+THETAi2),[bearing12 0]);
rotate(finger3Fig,[0 0 1],(THETAi1+THETAi2+THETAi3),[bearing23 0]);
% 開始運動
% 轉第三節
for k=1:1:20
deltaTHETA3=(THETAf3-THETAi3)*0.05;
rotate(finger3Fig,[0 0 1],deltaTHETA3,[bearing23 0]);
pause(0.1);
end;
pause;
% 轉第二加第三節
for k=1:1:50
deltaTHETA2=(0-THETAi2)*0.02;
% 求出新的連接點（bearing）位置
bearing12=[L1*cosd(THETAi1) L1*sind(THETAi1)];
% 一起動第二第三節(第一節已就定位)
rotate(finger2Fig,[0 0 1],deltaTHETA2,[bearing12 0]);
rotate(finger3Fig,[0 0 1],deltaTHETA2,[bearing12 0]);
pause(0.05);
end;
pause;
% 三節一起轉
for k=1:1:50
deltaTHETA1=(THETAf1-THETAi1)*0.02;
% 三節一起動
rotate(finger1Fig,[0 0 1],deltaTHETA1,[0 0 0]);
rotate(finger2Fig,[0 0 1],deltaTHETA1,[0 0 0]);
rotate(finger3Fig,[0 0 1],deltaTHETA1,[0 0 0]);
pause(0.1);
end;
pause;
% 最後第二第三節一起轉
for k=1:1:10
deltaTHETA2=(THETAf2-0)*0.1;
% 求出新的連接點（bearing）位置
bearing12=[L1*cosd(THETAf1) L1*sind(THETAf1)];
% 一起動第二第三節(第一節已就定位)
rotate(finger2Fig,[0 0 1],deltaTHETA2,[bearing12 0]);
rotate(finger3Fig,[0 0 1],deltaTHETA2,[bearing12 0]);
pause(0.1);
end;
pause;
% 運動結束
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 沿著極限位置的運動情形 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 模擬手握拳張開時手指的運動情形 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 求出連接點（bearing）位置
bearing12=[L1*cosd(THETAi1) L1*sind(THETAi1)];
bearing23=[L1*cosd(THETAi1)+L2*cosd(THETAi) L1*sind(THETAi1)+L2*sind(THETAi)];
% 將手指圖形存在handle裡
finger1=linkpoints([0 0],[L1 0], 1);
finger1=finger1';
finger1Fig=line(finger1(:,1),finger1(:,2),'color','b','linewidth',1);
finger2=linkpoints([0 0],[L2 0], 1);
finger2(1,:)=finger2(1,:)+bearing12(1,1);
finger2(2,:)=finger2(2,:)+bearing12(1,2);
finger2=finger2';
finger2Fig=line(finger2(:,1),finger2(:,2),'color','b','linewidth',1);
finger3=linkpoints([0 0],[L3 0], 1);
finger3(1,:)=finger3(1,:)+bearing23(1,1);
finger3(2,:)=finger3(2,:)+bearing23(1,2);
finger3=finger3';
finger3Fig=line(finger3(:,1),finger3(:,2),'color','b','linewidth',1);
% 旋轉手肘以接在一起
rotate(finger1Fig,[0 0 1],THETAi1,[0 0 0]);
rotate(finger2Fig,[0 0 1],(THETAi1+THETAi2),[bearing12 0]);
rotate(finger3Fig,[0 0 1],(THETAi1+THETAi2+THETAi3),[bearing23 0]);
for k=1:1:100
deltaTHETA1=(THETAf1-THETAi1)*0.01;
deltaTHETA2=(THETAf2-THETAi2)*0.01;
deltaTHETA3=(THETAf3-THETAi3)*0.01;
% 求出新的連接點（bearing）位置
bearing12=[L1*cosd(THETAi1+deltaTHETA1*k) L1*sind(THETAi1+deltaTHETA1*k)];
bearing23=[L1*cosd(THETAi1+deltaTHETA1*k)+L2*cosd(THETAi+(deltaTHETA1+deltaTHETA2)*k) ...
L1*sind(THETAi1+deltaTHETA1*k)+L2*sind(THETAi+(deltaTHETA1+deltaTHETA2)*k)];
% 先動第一節(第二第三節一起動)
rotate(finger1Fig,[0 0 1],deltaTHETA1,[0 0 0]);
rotate(finger2Fig,[0 0 1],deltaTHETA1,[0 0 0]);
rotate(finger3Fig,[0 0 1],deltaTHETA1,[0 0 0]);
% 再動第二節(第一節已就定位，第三節一起動)
rotate(finger2Fig,[0 0 1],deltaTHETA2,[bearing12 0]);
rotate(finger3Fig,[0 0 1],deltaTHETA2,[bearing12 0]);
% 最後動第三節(第一第二節都已就定位)
rotate(finger3Fig,[0 0 1],deltaTHETA3,[bearing23 0]);
pause(0.1);
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 模擬手握拳張開時手指的運動情形 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 可看出兩次畫圖，最後手指頭會重合在一起！
% linkpoints(之前的作業，可以用來輸出畫link的點，對於這次把手指當成link是很有用的工具)
% Input the positions of the two ends & width , output points of the link.
function y=linkpoints(A,B,d) % A=[x1 y1] B=[x2 y2]
L=((B(1,1)-A(1,1))^2+(B(1,2)-A(1,2))^2)^0.5;
COStheta=(B(1,1)-A(1,1))/L;
SINtheta=(B(1,2)-A(1,2))/L;
rotationMatrix=[COStheta -SINtheta; SINtheta COStheta];
p0=linspace(0.5*pi,2.5*pi,12);
p1=linspace(0.5*pi,1.5*pi,6);
p2=linspace(1.5*pi,2.5*pi,6);
X=[(d/4)*cos(p0) 0 (d/2)*cos(p1) L L+(d/2)*cos(p2) L L+(d/4)*cos(p0) L 0 0 ];
Y=[(d/4)*sin(p0) (d/2) (d/2)*sin(p1) (-d/2) (d/2)*sin(p2) (d/4) (d/4)*sin(p0) (d/2) (d/2) (d/4)];
positions=[X;Y];
positions=rotationMatrix*positions;
X=positions(1,:)+A(1,1);
Y=positions(2,:)+A(1,2);
y=[X;Y];
-------------------------------------------------------------------------------------------------
2.3：
程式velocityAndAcceletrationOfFinger(有關題目的描述、分析、解釋，皆已寫入下列的程式碼中)
-------------------------------------------------------------------------------------------------
圖四
圖五
圖六
% 為了跟第一及第二小題使用相通的convention，我定義張開手時是final，握拳是initial，故程式中角度變化都是從final變成initial。
% 以指節間向量夾角為角度，逆時針為正。(第一節則取與x軸夾角)
% (定義：向量起點為該指節與前一指節的bearing，向量終點為該指節與下一指節的bearing)
% [L1第一節長度, L2第二節長度, L3第三節長度, THETAi1第一節初角度/THETAf1末角度,
% THETAi2第二節初角度/THETAf2末角度, THETAi3第三節初角度/THETAf3末角度, T手指下扣需要的時間](長度：公分 / 角度：度)
function velocityAndAccelerationOfFinger(L1,L2,L3,THETAi1,THETAi2,THETAi3,THETAf1,THETAf2,THETAf3,T)
% 我的手指從張開到握起來大約需要1/8秒，假設王建民需要1/10秒。
% 伸卡球的握法如圖3，只有食指、中指會往下扣，又投伸卡球時是食指比中指用力，故假設王建民的食指下扣需要9/100秒、中指需要1/10秒。
% 使球旋轉主要是靠手指頭的運動，假設下扣動作中，手指頭轉動初位置及末位置如圖五&圖六。
% 圖五&圖六中：THETAf1=90;THETAf2=-30;THETAf3=-20;THETAi1=0;THETAi2=-90;THETAi3=-90;
% 手指長度：我舉自己的中指為例：L1=7;L2=4;L3=2.5;
% 時間：用王建民的中指假設：1/10;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 模擬張開的手握拳時手指的運動情形 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
axis equal;
axis([-20 20 -20 20]);
axis manual;
% 求出連接點（bearing）位置
bearing12=[L1*cosd(THETAf1) L1*sind(THETAf1)];
bearing23=[L1*cosd(THETAf1)+L2*cosd(THETAf1+THETAf2) L1*sind(THETAf1)+L2*sind(THETAf1+THETAf2)];
% 將手指圖形存在handle裡
finger1=linkpoints([0 0],[L1 0], 1);
finger1=finger1';
finger1Fig=line(finger1(:,1),finger1(:,2),'color','b','linewidth',1);
finger2=linkpoints([0 0],[L2 0], 1);
finger2(1,:)=finger2(1,:)+bearing12(1,1);
finger2(2,:)=finger2(2,:)+bearing12(1,2);
finger2=finger2';
finger2Fig=line(finger2(:,1),finger2(:,2),'color','b','linewidth',1);
finger3=linkpoints([0 0],[L3 0], 1);
finger3(1,:)=finger3(1,:)+bearing23(1,1);
finger3(2,:)=finger3(2,:)+bearing23(1,2);
finger3=finger3';
finger3Fig=line(finger3(:,1),finger3(:,2),'color','b','linewidth',1);
% 旋轉手肘以接在一起
rotate(finger1Fig,[0 0 1],THETAf1,[0 0 0]);
rotate(finger2Fig,[0 0 1],(THETAf1+THETAf2),[bearing12 0]);
rotate(finger3Fig,[0 0 1],(THETAf1+THETAf2+THETAf3),[bearing23 0]);
pause;
% 算出w1、w2、w3
w1=(THETAf1-THETAi1)*(pi/180)*(1/T);
w2=(THETAf2-THETAi2)*(pi/180)*(1/T);
w3=(THETAf3-THETAi3)*(pi/180)*(1/T);
% 設第一節與第二節間的bearing(稱作A點)的位置向量為r1，第二節與第三節間的bearing(稱作B點)的位置向量為r1+r2，手指頭的位置向量(稱作C點)為r1+r2+r3。
r1=L1*exp(i*(THETAf1*(pi/180)));
r2=L2*exp(i*((THETAf1+THETAf2)*(pi/180)));
r3=L3*exp(i*((THETAf1+THETAf2+THETAf3)*(pi/180)));
% 對r1、r1+r2、r1+r2+r3作微分得到ABC三點的速度向量
v1=i*(-w1)*r1;
v2=i*(-w1)*r1+i*(-w1-w2)*r2;
v3=i*(-w1)*r1+i*(-w1-w2)*r2+i*(-w1-w2-w3)*r3;
% 對v1、v1+v2、v1+v2+v3作微分得到ABC三點的加速度向量(在此題中我假設角加速度為0)
a1=-((w1)^2)*r1;
a2=-((w1)^2)*r1-((w1+w2)^2)*r2;
a3=-((w1)^2)*r1-((w1+w2)^2)*r2-((w1+w2+w3)^2)*r3;
% 將複數平面轉成xy座標
r1=[real(r1) imag(r1)];
r2=[real(r2) imag(r2)];
r3=[real(r3) imag(r3)];
v1=[real(v1) imag(v1)];
v2=[real(v2) imag(v2)];
v3=[real(v3) imag(v3)];
a1=[real(a1) imag(a1)];
a2=[real(a2) imag(a2)];
a3=[real(a3) imag(a3)];
% 將速度及加速度線存成handle(速度為綠線、加速度為紅線)
v1handle=line([r1(1,1);r1(1,1)+v1(1,1)],[r1(1,2);r1(1,2)+v1(1,2)],'color','G','linewidth',1);
v2handle=line([r1(1,1)+r2(1,1);r1(1,1)+r2(1,1)+v2(1,1)],[r1(1,2)+r2(1,2);r1(1,2)+r2(1,2)+v2(1,2)],'color','G','linewidth',1);
v3handle=line([r1(1,1)+r2(1,1)+r3(1,1);r1(1,1)+r2(1,1)+r3(1,1)+v3(1,1)],[r1(1,2)+r2(1,2)+r3(1,2);r1(1,2)+r2(1,2)+r3(1,2)+v3(1,2)],'color','G','linewidth',1);
a1handle=line([r1(1,1);r1(1,1)+a1(1,1)],[r1(1,2);r1(1,2)+a1(1,2)],'color','R','linewidth',1);
a2handle=line([r1(1,1)+r2(1,1);r1(1,1)+r2(1,1)+a2(1,1)],[r1(1,2)+r2(1,2);r1(1,2)+r2(1,2)+a2(1,2)],'color','R','linewidth',1);
a3handle=line([r1(1,1)+r2(1,1)+r3(1,1);r1(1,1)+r2(1,1)+r3(1,1)+a3(1,1)],[r1(1,2)+r2(1,2)+r3(1,2);r1(1,2)+r2(1,2)+r3(1,2)+a3(1,2)],'color','R','linewidth',1);
% 開始動作
for t=0.001:(0.001):T
% A點更新速度及加速度線
r1=L1*exp(i*(-w1*t+THETAf1*(pi/180)));
v1=i*(-w1)*r1;
a1=-((w1)^2)*r1;
r1=[real(r1) imag(r1)];
v1=[real(v1) imag(v1)];
a1=[real(a1) imag(a1)];
set(v1handle,'xdata',[r1(1,1);v1(1,1)],'ydata',[r1(1,2);v1(1,2)]);
set(a1handle,'xdata',[r1(1,1);a1(1,1)],'ydata',[r1(1,2);a1(1,2)]);
% B點更新速度及加速度線
r2=L2*exp(i*(-(w1+w2)*t+(THETAf1+THETAf2)*(pi/180)));
v2=i*(-w1)*r1+i*(-w1-w2)*r2;
a2=-((w1)^2)*r1-((w1+w2)^2)*r2;
r2=[real(r2) imag(r2)];
v2=[real(v2) imag(v2)];
a2=[real(a2) imag(a2)];
set(v2handle,'xdata',[r1(1,1)+r2(1,1);v2(1,1)],'ydata',[r1(1,2)+r2(1,2);v2(1,2)]);
set(a2handle,'xdata',[r1(1,1)+r2(1,1);a2(1,1)],'ydata',[r1(1,2)+r2(1,2);a2(1,2)]);
% C點更新速度及加速度線
r3=L3*exp(i*(-(w1+w2+w3)*t+(THETAf1+THETAf2+THETAf3)*(pi/180)));
v3=i*(-w1)*r1+i*(-w1-w2)*r2+i*(-w1-w2-w3)*r3;
a3=-((w1)^2)*r1-((w1+w2)^2)*r2-((w1+w2+w3)^2)*r3;
r3=[real(r3) imag(r3)];
v3=[real(v3) imag(v3)];
a3=[real(a3) imag(a3)];
set(v3handle,'xdata',[r1(1,1)+r2(1,1)+r3(1,1);v3(1,1)],'ydata',[r1(1,2)+r2(1,2)+r3(1,2);v3(1,2)]);
set(a3handle,'xdata',[r1(1,1)+r2(1,1)+r3(1,1);a3(1,1)],'ydata',[r1(1,2)+r2(1,2)+r3(1,2);a3(1,2)]);
% 更新圖
refresh;
deltaTHETA1=w1*(180/pi);
deltaTHETA2=w2*(180/pi);
deltaTHETA3=w3*(180/pi);
% 求出新的連接點（bearing）位置
bearing12=[L1*cosd(THETAf1-deltaTHETA1*t) L1*sind(THETAf1-deltaTHETA1*t)];
bearing23=[L1*cosd(THETAf1-deltaTHETA1*t)+L2*cosd((THETAf1+THETAf2)-(deltaTHETA1+deltaTHETA2)*t) ...
L1*sind(THETAf1-deltaTHETA1*t)+L2*sind((THETAf1+THETAf2)-(deltaTHETA1+deltaTHETA2)*t)];
% 先動第一節(第二第三節一起動)
rotate(finger1Fig,[0 0 1],-deltaTHETA1*0.001,[0 0 0]);
rotate(finger2Fig,[0 0 1],-deltaTHETA1*0.001,[0 0 0]);
rotate(finger3Fig,[0 0 1],-deltaTHETA1*0.001,[0 0 0]);
% 再動第二節(第一節已就定位，第三節一起動)
rotate(finger2Fig,[0 0 1],-deltaTHETA2*0.001,[bearing12 0]);
rotate(finger3Fig,[0 0 1],-deltaTHETA2*0.001,[bearing12 0]);
% 最後動第三節(第一第二節都已就定位)
rotate(finger3Fig,[0 0 1],-deltaTHETA3*0.001,[bearing23 0]);
pause(0.2);
end;
% 結束動作
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 模擬張開的手握拳時手指的運動情形 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% linkpoints(之前的作業，可以用來輸出畫link的點，對於這次把手指當成link是很有用的工具)
% Input the positions of the two ends & width , output points of the link.
function y=linkpoints(A,B,d) % A=[x1 y1] B=[x2 y2]
L=((B(1,1)-A(1,1))^2+(B(1,2)-A(1,2))^2)^0.5;
COStheta=(B(1,1)-A(1,1))/L;
SINtheta=(B(1,2)-A(1,2))/L;
rotationMatrix=[COStheta -SINtheta; SINtheta COStheta];
p0=linspace(0.5*pi,2.5*pi,12);
p1=linspace(0.5*pi,1.5*pi,6);
p2=linspace(1.5*pi,2.5*pi,6);
X=[(d/4)*cos(p0) 0 (d/2)*cos(p1) L L+(d/2)*cos(p2) L L+(d/4)*cos(p0) L 0 0 ];
Y=[(d/4)*sin(p0) (d/2) (d/2)*sin(p1) (-d/2) (d/2)*sin(p2) (d/4) (d/4)*sin(p0) (d/2) (d/2) (d/4)];
positions=[X;Y];
positions=rotationMatrix*positions;
X=positions(1,:)+A(1,1);
Y=positions(2,:)+A(1,2);
y=[X;Y];
1 則留言:
一，你花了很多功夫在這道題目上，而且分析的相當好。
二，手臂的部份原先設計是只要上臂，下臂及手掌就可以，現在似乎你將手掌的部份又分成兩部份。
You did a very good job!
張貼留言