4/11-第五次作業

作業五 b94611040 廖婉婷

5-1-1
以下為繪製上下手臂外形之函式:
參考講義上連桿之linkshape函式
joint處修正為僅畫外圓

function arm(A,B,d)
d=abs(d);
AB=(B(1)+j*B(2))-(A(1)+j*A(2));
D=abs(AB);th=angle(AB);
t=linspace(pi/2,2.5*pi,20);
Cout=max(d/2,0.2)*exp(j*t');Cin=Cout/2;
if d>0,
    P=[Cout;Cout(1:10);D+Cout(11:20);D+Cout;D+Cout(20);Cout(1)];
else
    P=[Cin;0;D;D+Cin];
end;
xx=real(P);yy=imag(P);
x=xx*cos(th)-yy*sin(th)+A(1);
y=xx*sin(th)+yy*cos(th)+A(2);
line(x,y);
axis equal;

以下為繪製手掌外形之函式:
參考之前講義linkshape函式
手指的部份另外繪出
故手指長度需為其中一項input

function draw_palm(A,B,d,f)
d=abs(d);
AB=(B(1)+j*B(2))-(A(1)+j*A(2));
D=abs(AB);th=angle(AB);
t=linspace(pi/2,2.5*pi,17);
Cout=max(d/2,0.2)*exp(j*t');Cin=Cout/2;
if d>0,
    P=[Cout;Cout(1:9);
        D+Cout(9);D+Cout(9)+f;D+Cout(9);D+Cout(9:11);
        D+Cout(11);D+Cout(11)+f;D+Cout(11);D+Cout(11:13);
        D+Cout(13);D+Cout(13)+f;D+Cout(13);D+Cout(13:15);
        D+Cout(15);D+Cout(15)+f;D+Cout(15);D+Cout(15:17);
        D+Cout(17);D+Cout(17)+f;D+Cout(17);
        D+Cout;D+Cout(9);D+Cout(17);Cout(1)];
else
    P=[Cin;0;D;D+Cin];
end;
xx=real(P);yy=imag(P);
x=xx*cos(th)-yy*sin(th)+A(1);
y=xx*sin(th)+yy*cos(th)+A(2);
line(x,y);
axis equal;

以下為主繪圖指令:

axis([0 100 -50 50]);
axis equal;
L1 = 30;      %假設上手臂,下手臂,手掌長為30cm,25cm,15cm
L2 = 25;
L3 = 15;
theta1 = 45;  %假設手臂,手掌間角度之值
theta2 = 235;
theta3 = 200;
a = L1*cosd(theta1);
b = -L1*sind(theta1);
c = a+L2*cosd(theta2-theta1-180);
d = b+L2*sind(theta2-theta1-180);
theta4 = theta3+theta2-theta1-360;
e = c+L3*cosd(theta4)/2;
f = d+L3*sind(theta4)/2;
arm1 = [0 0;a b]; %上手臂之座標組合
arm2 = [a b;c d]; %下手臂之座標組合
palm = [c d;e f]; %手掌之座標組合
arm(arm1(1,:), arm1(2,:), 7); %繪出上手臂
arm(arm2(1,:), arm2(2,:), 5); %繪出下手臂
finger =L3/2;
%由於function draw_palm是將手掌/手指長度分別採用繪出
%故需另外計算手指長度
%此處假設手指長約等於整個手掌的一半
draw_palm(palm(1,:), palm(2,:), 8,finger);
%繪出整個手掌

<圖>以自己尺寸輸入之手臂圖




5-1-2
此題是將上一小題之主繪圖指令置於function body中:

function body(L1,L2,L3,theta1,theta2,theta3)
axis equal;
a = L1*cosd(theta1);
b = -L1*sind(theta1);
c = a+L2*cosd(theta2-theta1-180);
d = b+L2*sind(theta2-theta1-180);
theta4 = theta3+theta2-theta1-360;
e = c+L3*cosd(theta4)/2;
f = d+L3*sind(theta4)/2;
%由輸入之手臂手掌尺寸,計算出各端點座標位置
arm1 = [0 0;a b];  %手臂之座標連結
arm2 = [a b;c d];
palm = [c d;e f];  %手掌之座標連結
arm(arm1(1,:), arm1(2,:), 7);
arm(arm2(1,:), arm2(2,:), 5);
%呼叫前一小題function arm繪出手臂
finger =L3/2;
draw_palm(palm(1,:), palm(2,:), 8,finger);
%呼掉函式繪出手掌
%繪出整支手臂之function結束

5-1-3

axis([0 100 -50 50]);
body(30,25,15,90,-45,-30);

<圖>由給定長度,角度繪出之手臂位置




5-1-4

for i = 1:6,        % 動畫重複六次
    for k = 1:1:10,
        clf;        %清除畫面,繼續指令
        theta1i = -90 + 1.5*k;
       %theta1i在-90~-75度間分十等分運動 因此每次動1.5度
        theta2i = -45 + k;
       %theta2i在-45~-35度間分十等分運動 因此每次動1度
        theta3i = -30 + 4*k;
       %theta3i在-30~10度間分十等分運動 因此每次動4度
        body(30, 25, 15, theta1i, theta2i, theta3i);
       %分別繪出每一等份之手臂位置
        pause(0.2);
    end;
end;

<動畫>由給定範圍手臂運動之情形



5-2-1
首先,手指的三個關節可視為旋轉節,連接相當於連桿之手指及手掌
其中指尖部份沒有再作連接動作,因此整個手指可視為一開放型運動結
至於自由度部份,平面上每個運動物體最多會有3個自由度
由於每個旋轉節會造成兩個拘束度
因此總自由度為3*3-2*3 = 3
以古魯伯公式計算M = 3(4-1)-(3*3-3) = 3 結果相同
此三個自由度分別代表三個關節的旋轉





5-2-2
手指採用與前面function arm的繪製方法相同
但為了固定住顯示範圍
故另外複製成fingerpart函式作修改

function finger_part(A,B,d)
d=abs(d);
AB=(B(1)+j*B(2))-(A(1)+j*A(2));
D=abs(AB);th=angle(AB);
t=linspace(pi/2,2.5*pi,20);
Cout=max(d/2,0.2)*exp(j*t');Cin=Cout/2;
if d>0,
    P=[Cout;Cout(1:10);D+Cout(11:20);D+Cout;D+Cout(20);Cout(1)];
else
    P=[Cin;0;D;D+Cin];
end;
xx=real(P);yy=imag(P);
x=xx*cos(th)-yy*sin(th)+A(1);
y=xx*sin(th)+yy*cos(th)+A(2);
line(x,y);
axis([-1 12 -12 12]);
%axis equal;

由於手指四指之運動方式皆相同
故以一指作代表,以下為繪出手指外形之函式:

function finger(L1,L2,L3,theta1,theta2,theta3)
axis equal;
a = L1*cosd(theta1-90);  %計算出每一指間joint的座標位置
b = L1*sind(theta1-90);
c = a + L2*sind(360-theta2-theta1);
d = b + L2*cosd(360-theta2-theta1);
e = c + L3*sind(theta3-360+theta2+theta1);
f = d - L3*cosd(theta3-360+theta2+theta1);
finger_part([0 0],[a b],1);  %分別繪出每一節指頭
finger_part([a b],[c,d],1);
finger_part([c d],[e f],1);

以下為展示手指極限位置之方程式:

L1 = 4; L2 = 2.5; L3 = 2; %假設各節長度以自己尺寸輸入
for i = 0:5:90,
    clf;
    finger(L1, L2, L3, 180-i, 180, 180);
    %第一關節先作轉動 為極限範圍180~90度
    pause(0.05);
end;
for i = 0:5:90,
    clf;
    finger(L1, L2, L3, 90, 180-i, 180);
    %再換第二關節轉動 範圍同樣180~90度
    pause(0.05);
end;
for i = 0:5:90,
    clf;
    finger(L1, L2, L3, 90, 90, 180-i);
    %第三關節轉動 範圍180~90度
    pause(0.05);
end;

<動畫>依照指定角度範圍做運動之手臂



5-2-3
以下為繪出手指各節速度/加速度之函式:

function [vec,dyadata] = dyad(rho,theta,td,tdd)
theta=theta(:);rho=rho(:);
n=length(rho);
if nargin<4, tdd="zeros(size(rho));" nargin="=" td="ones(size(rho));" td="ones(size(rho))*td;end;" tdd="ones(size(rho))*tdd;end;" td="td(:);tdd=" d2g="pi/180;" tt="exp(i*theta*d2g);" pp="rho.*tt;vv=" aa="-pp.*td.^2+i*pp.*tdd;" dyadata="[pp" vec="[abs(sum(dyadata));angle(sum(dyadata))/d2g];">

以下為繪出手指各節速度/加速度之函式:
function dyad_draw(rho,theta,td,tdd)
% Inputs: rho:length of links
%         theta:incling angles, deg.
%         td:angular velocity, rad/s
%         tdd:angular acceleration, rad/s^2

clf;
[vec,data] = dyad(rho,theta,td,tdd);
x=[0;cumsum(real(data(:,1)))];y=[0;cumsum(imag(data(:,1)))];
for i=1:length(x)-1
  linkshape([x(i) y(i)],[x(i+1) y(i+1)],1);
end
for k=1:length(rho)
   x0=x(k+1);y0=y(k+1);
   vx=x0+real(data(k,2));vy=y0+imag(data(k,2));
   ax=x0+real(data(k,3));ay=y0+imag(data(k,3));
   line([x0 vx],[y0 vy],'marker','s','linewidth',2);
   line([x0 ax],[y0 ay],'marker','s','color','r','linewidth',3)
end
sdata=sum(data);
ss=[real(sdata(1)) imag(sdata(1))];
vv=[real(sdata(2)) imag(sdata(2))];
aa=[real(sdata(3)) imag(sdata(3))];
line([0 ss(1)],[0 ss(2)],'linestyle',':','linewidth',2)
axis equal;grid on

dyad_draw([4 2.5 2],[90 135 160],[.5 .5 .5],0)
%此處手指長度以自己尺寸為輸入
%設每一角度與座標軸(逆時鐘為正)呈90,135,160度,且角速度相等
一物體作曲線運動時,與其路徑相切會有切線速度/加速度,改變其運動快慢
同樣在與路徑垂直方向上會有法線速度/加速度,用於改變其運動方向
由於手指自由度為3,只能作旋轉運動,故只存在法線速度/加速度
由圖上即可見紅色為各桿之法線加速度，藍色為法線速度

<圖>手臂運動之速度,加速度分析圖