腕の順運動計算

x,y,z J1〜J4,r,p,tは肩関節原点。胴体姿勢は正姿勢。

■座標

m = Rot(x,J1)･Rot(y,J2)･Rot(z,J3)･(Rot(y,J4)･ml2+ml1)

	  0
ml1 =	  0
	-al1

	  0
ml2 =	  0
	-al2

	x
m =	y
	z

m:
-c2c3s4al2-s2c4al2-s2al1        		0       0
-s1s2c3s4al2-c1s3s4al2+s1c2c4al2+s1c2al1        0       0
c1s2c3s4al2-s1s3s4al2-c1c2c4al2-c1c2al1         0       0

■姿勢
姿勢は、肩と手先を結ぶ線を基準に考える。
つまり、手先を原点とした肩の姿勢を考え、その後、肩関節原点とした数値に変換する。

r4321 = Rot(y,a-J4)･Rot(z,-J3)･Rot(y,-J2)･Rot(x,-J1)

sin(a) = al1 * sin(J4) / l;

一方肩関節姿勢は

rrpt = Rot(z,-r)･Rot(y,-p)･Rot(x,-t)

r4321:
ca4c3c2+sa4s2   ca4s3c1+ca4c3s2s1-sa4c2s1       ca4s3s1-ca4c3s2c1+sa4c2c1
-s3c2   	c3c1-s3s2s1     		c3s1+s3s2c1
-sa4c3c2+ca4s2  -sa4s3c1-sa4c3s2s1-ca4c2s1      -sa4s3s1+sa4c3s2c1+ca4c2c1

rrpt:
crcp    srct+crspst     srst-crspct
-srcp   crct-srspst     crst+srspct
sp      -cpst   cpct

	  -(-srcp)         s3c2
tan(r) = ---------- = ----------------
	     crcp      ca4c3c2+sa4s2

	  -(-cpst)     -(-sa4s3c1-sa4c3s2s1-ca4c2s1)
tan(t) = ---------- = ------------------------------
	    cpct       -sa4s3s1+sa4c3s2c1+ca4c2c1

	           sp                                    -sa4c3c2+ca4s2
tan(p) = --------------------- = ---------------------------------------------------------------
	  -st(-cpst)+ct(cpct)     -st(-sa4s3c1-sa4c3s2s1-ca4c2s1)+ct(-sa4s3s1+sa4c3s2c1+ca4c2c1)


■姿勢2
手首関節の姿勢は

r1234 = Rot(x,J1)･Rot(y,J2)･Rot(z,J3)･Rot(y,J4)

手首関節の姿勢を極座標で表すと、

rtpr = Rot(x,t)･Rot(y,p)･Rot(z,r)

r1234:
c2c3c4-s2s4     		-c2s3   	c2c3s4+s2c4
s1s2c3c4+c1s3c4+s1c2s4  	-s1s2s3+c1c3    s1s2c3s4+c1s3s4-s1c2c4
-c1s2c3c4+s1s3c4-c1c2s4         c1s2s3+s1c3     -c1s2c3s4+s1s3s4+c1c2c4

rtpr:
cpcr    	-cpsr   	sp
stspcr+ctsr     -stspsr+ctcr    -stcp
-ctspcr+stsr    ctspsr+stcr     ctcp

	  -(-cpsr)        c2s3
tan(r) = ---------- = -------------
	    cpcr       c2c3c4-s2s4

	  -(-stcp)     -(s1s2c3s4+c1s3s4-s1c2c4)
tan(t) = ---------- = ----------------------------
	    ctcp       -c1s2c3s4+s1s3s4+c1c2c4

	           sp                                    c2c3s4+s2c4
tan(p) = --------------------- = ----------------------------------------------------------
	  -st(-stcp)+ct(ctcp)     -st(s1s2c3s4+c1s3s4-s1c2c4)+ct(-c1s2c3s4+s1s3s4+c1c2c4)