Partial3.m

%% 逕ｨ莠手ｮ｡邂怜刈蜈･蝓ｺ蝮先�ｳｻ蛻ｰCMM蝮先�ｳｻ霓ｬ謐｢遏ｩ髦ｵ譬｡蜃�噪讓｡蝙句￥蠕ｮ蛻?
function [dC] = Partial2(SphereTrans)
%% 隶｡邂苓ｽｬ謐｢遏ｩ髦ｵ逧�￥蠕ｮ蛻�洸髦ｵ
% 霎灘�荳ｺ霓ｬ謐｢遏ｩ髦ｵ蜈ｳ莠主推荳ｪ蜈ｳ闃ら噪蝗帑ｸｪ蜿よ焚逧�￥蠕ｮ蛻
%% 蛛丞ｾｮ蛻�ｮ｡邂鈴｡ｺ蠎丈ｸｺp,a,t,d,蜀嶺ｽ吝盾謨ｰ荳ｺd2�慧6蜥荊6
syms a1 p1 d1 t1;
syms a2 p2 d2 t2;
syms a3 p3 d3 t3;
syms a4 p4 d4 t4;
syms a5 p5 d5 t5;
syms a6 p6 d6 t6;
syms x y z a b g;
% 蛻�悪荳ｺa�径lpha�慧�荊heta

%縲?桷蟒ｺ莉擦MM蝮先�ｳｻ蛻ｰ蝓ｺ蝮先�ｳｻ逧�ｽｬ謐｢遏ｩ髦?
T0 = Rotz(a)*Roty(b)*Rotx(g)+transl(x,y,z)-...
    [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; 
T1 = transl(0,0,d1)*Rotz(t1)*transl(a1,0,0)*Rotx(p1);
T2 = transl(0,0,d2)*Rotz(t2)*transl(a2,0,0)*Rotx(p2);
T3 = transl(0,0,d3)*Rotz(t3)*transl(a3,0,0)*Rotx(p3);
T4 = transl(0,0,d4)*Rotz(t4)*transl(a4,0,0)*Rotx(p4);
T5 = transl(0,0,d5)*Rotz(t5)*transl(a5,0,0)*Rotx(p5);
T6 = transl(0,0,d6)*Rotz(t6)*transl(a6,0,0)*Rotx(p6);
T7 = SphereTrans;

% 隶｡邂佑MM蝮先�ｳｻ蛻ｰ蟾･蜈ｷ逅�攝譬�ｳｻ逧�ｽｬ謐｢遏ｩ髦?
T = T0*T1*T2*T3*T4*T5*T6*T7;

% 霓ｬ蛹紋ｸｺ��,y,z,psi,theta,phi�牙錘蛻�悪豎ょ￥蟇?
[C] = CalPos(T,0);

% 隶｡邂怜ｯｹ蜷�ｸｪ蜿よ焚逧�￥蟇ｼ
% mﾃ溶,m譏ｯ蝮先�盾謨ｰ�系譏ｯ豈丈ｸｪ霑樊揉逧ДH蜿よ焚
dC = jacobian(C,[p1;a1;t1;d1;p2;a2;t2;p3;a3;t3;d3;p4;a4;t4;d4;p5;a5;t5;d5;p6;a6;x;y;z;a;b;g]);
end