円環と接触する指のPR運動　EdgeContactPR2Motion 　2023-5-15

（「詳説ロボットの運動学9.4.4(1)PR接触の指の腹による操りの逆運動学」209pの修正）

ソースファイル：FinPRManip2Source, 　実行ファイル：FinPRManip2exe

・原著では微小時間内の接触点の移動を直線運動と仮定してPR運動の条件を設定しているが，その結果は指とWorkの接触が保証されない．特に円筒端面の円環と接触するようなWorkの境界線を辿るPR操り運動に顕著である．

PR接触運動の指上の接触点の速度vfc[ ]と回転ωf[ ]は

vfc[ ] = vw[ ] + ωw[ ]×rwc[ ] (PR1-1)

(ωf[ ], nc[ ]) = (ωw[ ], nc[ ]) (PR1-2)

であり，Work上の接触点Cwと指面上の接触点Cfが同じ位置に移動することを表している．しかし実際には両曲面は相対的に転がり運動をし，接触点は別の位置になる．

別の表現をすれば，上記４つのPR条件式から4つの指関節角速度ν[ ]が得られ，微小時間dt後にdq[ ]=ν[ ]*dt, 関節角q2[ ]=q1[ ]+dq[ ]となり，q2[ ]のときの指はWorkに対する相対ポーズが変わるので新しい接触点で交わることはあっても接触することは期待できない．

・上記をPR1運動とし，PR条件を保ちつつ新しい接触点を求めるためにPR2運動を追加する．これがPR接触操り運動の修正である．

PR2運動

・円環に沿った円筒指のPR運動は，「接触点Cにおける指と円環の速度が等しい（滑りがない）こと，及びC点回りの回転速度の法線成分は等しい（ツイストがない）こと」である．

vfc[ ] = vwc[ (PR1-1)]

, (ωf[ ], nc[ ]) = (ωw[ ], nc[ ]) (PR1-2)

・ワークの曲面上でのPR1運動と同様に上記の運動の微小時間後に指とワークが接触するとは限らないので，PR2運動を追加する．

　指の追加運動をvwc[ ]軸回りの回転(捻り)ωf2[ ]とする．

　これはPR1運動の条件を満足している．但し捻り運動によって指はvwc[ ]回りの回転する．この回転によって指円筒と円環を接触させる．

　PR条件は微小時間dt後に

dq[ ] = ν2[ ]*dt (PR2-1)

q2[ ] = q1[ ] + dq[ ] (PR2-2)

(1) 指円筒が円環と接触する．D = f(q2[ ]) → 0

(2) 指円筒上の転がり量と円環上の転がり量が等しい．Sf – Sw → 0

(3) ωf2[ ]は接触点の法線と直角 (ωf2[ ], nc[ ]) = 0

(4) ωf2[ ]は円環半径方向と直角 (ωf2[ ], nr[ ]) = 0

(3), (4)はωf2[ ]がvwc[ ]方向（円環に沿う方向）であることを意味する．

　この4条件を満足するdq[ ]=ν2[ ]*dtを求める．

・運動前のparameter

　q1[ ], (Lf1, φf1, φw1) (PR2-3)

接触点C1=Cf1=Cw1

rfc1[ ] = JA1[ ] + Lf1*ezf1[ ] + af*(exf1[ ]*cφf1 + eyf1[ ]*sφf1) (PR2-4)

rw1[ ] = rw1[ ] + aw*(exw1[ ]*cφw1 + eyw1[ ]*sφw1) (PR2-5)

・PR1運動後のC1の行き先

q11[ ], (Lf1, φf1, φw1) (PR2-6)

ωf1[ ] = ΣAX[k][ ]*ν[k] AX[ ]：関節軸方向 (PR2-7)

vfc1[ ] = ΣLC[k][ ]*ν[k] LC[k][ ] = AX[k][ ]×L[k][ ], L[k] = rfc1[ ] – J[k][ ] (PR2-8)

vwc[ ] = vw[ ] + ωw[ ]×(aw*nc1[ ]), nc1[ ] = (rwc1[ ] – rw1[ ])/aw：C1点の法線 (PR2-9)

　　　omwn = (ωw[ ], nc1[ ]) (PR2-10)

omfn1 = (ωf1[ ], nc[ ]) (PR2-11)

・PR1条件：dt後

(vwc[ ], omwn) = (vfc1[ ], omfn1)

= JB41[ ][k]*ν[k] (PR2-21)

→ ν[k] = IJB41[k][ ]*(vwc[ ], omwn) (PR2-22)

dq11[ ] = ν[ ]*dt (PR2-23)

q21[ ] = q1[ ] + dq11[ ] (PR2-24)

・PR1後のCf1の行き先Cf4

q21[ ], (Lf1, φf1, φw1) (PR2-25)

q21[ ]のときの指各諸元

nJ関節位置JA21[ ]

指座標 exf21[ ], eyf21[ ], ezf21[ ]

rfc4[ ] = JA21[ ] + Lf1*ezf21[ ] + af*(exf21[ ]*cφf1 + eyf21[ ]*sφf1) (PR2-26)

・PR1後の円環上のCw1の行き先Cw4

ωw[ ]を円環座標系で表す．

　　 ωw[ ] = omx*exw1[ ] + omy*eyw1[ ] + omz*ezw1[ ] (PR2-27)

　（omx*dt, omy*dtの回転による円環の姿勢の変換

exw1[ ], eyw1[ ], ezw1[ ]→exw41[ ], eyw41[ ], ezw41[ ]

ezw41[ ]回りのomz*dtによる姿勢変換

dφw = omz*dt (PR2-28)

exw4[ ] = exw41[ ]*c(dφw) + eyw41[ ]*s(dφw) (PR2-29)

eyw4[ ] =-exw41[ ]*s(dφw) + eyw41[ ]*c(dφw) (PRE2-30)

Cw1点の移動先C41

φw2 = φw1 + dφw (PR2-31)

rwc4[ ] =rw2[ ] + aw*(exw4[ ]*cφw2 + eyw4[ ]*sφw2) (PR2-32)

・PR1後のPR2運動：

　接触点C2を基準に考える．

　C2点で指と円環が接触する状態の指円環parameterをq2[ ], (Lf2, φf2, φw2)とする． (PR2-33)

q2[ ] = q21[ ] + dq2[ ]

qq2[ ]のときのnJ関節位置JA2[ ], 指座標系を(exf2[ ], eyf2[ ], ezf2[ ])とする．

　　 Cf2 : rfc2[ ] = JA2[ ] + Lf2*ezf2[ ] + aw*( exf2[ ]*cφf2 + eyf2[ ]*sφf2 ) (PR2-34)

exw2[ ] = exw4[ ], eyw2[ ] = eyw4[ ], ezw2[ ] = ezw4[ ]

Cw2 : rwc2[ ] = rw2[ ] + aw*( exw2[ ]*cφw2 + eyw2[ ]*sφw2 ) (Pr2-35)

Cf1(Cf4)の行き先

rfc3[ ] = JA2[ ] + Lf2*ezf2[ ] + af*( exf2[ ]*cφf1 + eyw2[ ]*sφf1 ) (PR2-36)

Cf1(Cf4)の行き先

rwc3[ ] = rw2[ ]] + aw*( exw2[ ]*cφw2 + eyw2[ ]*sφw2 ) (PR2-37)

PR2運動の条件は

(1) 指と円環は接触している．間隔D

D = abs(rfc2[ ] – rwc2[ ]) → 0 (PR2-38)

(2) 指上と円環上の転がり量は等しい..

sf[ ] = rfc2[ ] – rfc3[ ], Sf = abs( sf[ ] ) (PR2-39)

sw[ ] = rwc2[ ] – rwc3[ ], Sw = abs( sw[ ] ) (PR2-40)

dS = Sf – Sw　→ 0 (PR2-41)

(3), (4) ωf2[ ]は円環接線方向にある（法線方向，円環半径方向成分=0）

ωf2[ ]*dt = ΣAX[k][ ]*dq2[k] (PR2-42)

omfn2 = (ωf2[ ], nc2[ ]) → 0 (PR2-43)

omfr2 = (ωf2[ ], nr2[ ]) → 0, nr2[ ] = nc2[ ]×tw2[ ] (PR2-44)

掌円板とWrk円筒との干渉計算

・掌は半径rr, 厚さaaとし，中心はO(0,0,0)とする．

　掌面Hpは＋Y方向に向いており，面はyp=aa/2の(X,Z)面内にある．その中心をA(0, aa/2, 0)とする．

・両端の円筒軸位置でHP面に近い方をA;ra[ ], 遠い方をB:rb[ ]とし，A→B方向をmw[ ]とする．

円筒軸式 : 　r[ ] = rL[ ] + L*ezw[ ]

上端位置 : 　 rU[ ] = rL[ ] + Lw*ezw[ ]

rL[y]<rU[y] : ra[ ] = rL[ ], mw[ ] = ezw[ ]

rL[y]>rU[y] : ra[ ] = rU[ ], mw[ ] =-ezw[ ]

　改めて円筒軸の式

r[ ] = ra[ ] + L*mw[ ]

・円筒がHp面(X,Z)，Y=aa/2を切る曲線は楕円となる．

　楕円の中心Oe

ra[y] + Le*mw[y] = aa/2

（１）mw[y]≠0：円筒軸がHp面と非平行の場合

Le =(aa/2 - ra[y]) / mw[y]

・楕円中心Oe

re[ ] = ra[ ] + Le*mw[ ]

・円筒軸のY軸からの傾き角をβとすると楕円の長短径aw, bwは

aw = aw, bw = aw / sinβ

　HP面(X,Z)内で軸の方向をαとすると

α = atan2( mw[z], mw[x] )

・短径はX軸からZ軸の方向にαだけ傾いている．

・楕円の中心を原点とし，長径をZ軸とする座標系Σmに変換する．

楕円の式

(x/aw)^2 + (z/bw)^2 = 1

掌円板の式

(x – Xp)^2 + (z – Zp)^2 = rr^2

変換した円の中心Op(Xp, Zp)はOe回りに原点を右に-α回転して得られる．

Xp = (0-ra[x])*cos(-α) – (0-ra[z])*sin(-α)

Zp = (0-ra[x])*sin(-α) + (0-ra[z])*cos(-α)

・楕円と円の正規化　1/aw :

u = bw / aw

xp = Xp / aw

zp = Zp / aw;

ap = rr / aw

x^2 + (z/u)^2 = 1

(x – xp)^2 + (z – zp)^2 = ap^2

・楕円と円の交点ある場合，円内にある楕円部分の各点をに対応する円筒断面の円の中心をOcとすると

　AからOcまでの軸長Lcが有限軸長内にあれば干渉が生じ，有限軸長外なら非干渉である．

・楕円と円の交点C1(xc1, yc1), C2(xc2, yc2)

楕円式より

z = u*sqrt( 1 – x^2 )

円の式に代入

(x – xp)^2 + { u*sqrt(1-x^2) – zp }^2 = ap^2

= (x – xp)^2 + {u^2*(1-x^2) - 2*u*zp*sqrt(1 – x^2) + zp^2 } = ap^2

2*u*zp*sqrt(1 – x^2) = (x – xp)^2 + { u^2*(1-x^2) + zp^2 } - ap^2

両辺2乗：

4*u^2*zp^2*(1 – x^2)

= { (x –xp}^2 +u^2*(1-x^2) + zp^2 – ap^2 }^2

= { (1-u^2)*x^2 - (2*xp)*x + xp^2 + u^2 + zp^2– ap^2 ) }^2

→

≡A*(1- x^2) = { B*x^2 + C*x + D }^2

A≡4*u^2*zp^2, B≡1-u^2, C≡-2*xp, D≡xp^2 +u^2 + zp^2 – ap^2

上式は

A*(1- x^2) = B^2*x^4 + 2*B*C*x^3 + (2*B*D + C^2)*x^2 + 2*C*D*x + D^2

≡H4*x^4 + H3*x^3 + H2*x^2 + H1*x + H0 = 0

H4≡B^2, H3≡2*B*C, H2≡2*B*D + C^2) + A, H1≡2*C*D, H0≡D^2 - A

・xの4次方程式である．

・解がなければ干渉はない．

・解があれば4通りがあり得る．解を

x = xc

　とすると

z = zc = u*sqrt(1 – xc^2)

・楕円と円の交点C(xc, zc)に相当する円筒の断面をとり，その中心Ocまでの軸長Lcを求める．

　Lcが有限円筒内にあれば干渉があり，外なら非干渉である．

　　 |C-Oe| = zc

|Oc-Oe| = Lce = ze+sinβ

|OA-Oc| = Lc = Le - Lce

・各交点Cにおいて

0<Lc<Lw

があれば円筒と掌円板は干渉する．

（２）mw[y]=0：円筒軸がHp面と平行の場合

・掌面と平行な円筒を軸がZ方向に向くように円筒を回転する．

　円筒の基点rw[ ]を回転してrA[ ]とする．

rA[x] = rw[x])*cosα1 – rw[z]*sinα1 (c2-10)

　　　rA[z] = rw[x]*sinα1 + rw[2]*cosα1 (c2-11)

rA[y] = rw[y]

α = atan2( mw[z], mw[x] ) α1=π/2 – α

・円筒面が最も掌面に近いのは（X,Y）面内(Z=0)の断面円Ocである．

　Ocが有限円筒内のときは断面円が掌円板と交われば干渉する．

　Ocが有限円筒外のときは，円筒上下面の円が掌円板との干渉を調べる．

・円筒軸が（X,Y）面を切る点Ocの位置

rc0[x] =rA[x], rc0[y] = rA[y], rc0[z] = 0

・円筒基点OAからOcまでの軸長Lc

Lc = -rA[z]

(2-1)　Ocは有限円筒外の場合

Lc<0 or Lw<Lc

ならば干渉しない．

(2-2) Ocは有限円筒内の場合

0<Lc<Lw

・Ocを中心とする円筒断面の円が掌面のy=aa/2と交わる点C1, C2

円 : (x - rA[x])^2 + (y – rA[y])^2 = aw^2

D = aw^2 - (aa/2 – rA[y])^2

(2-2-1) D<0 :

交点がない．円筒と掌面は非干渉

(2-2-2) D>0:

xc1,2 = rA[x] ±sqrt( D )

小さい方をxc1, 大きい方をxc2とする．

・C1, C2が+X側でrr以上

xc1>rr, and xc2>rr ：干渉なし

-rr<xc1<rr or -rr<xc2<rr ：干渉あり

xc1<-rr, and rr<xc2 ：円筒断面は掌円を包含する．干渉あり

・ 0 < Lc < Lwを含めて上記が干渉の条件である．

（３）mw[y]=1：円筒軸はHp面と垂直な場合

・円筒面の式は

r[ ] = rw[ ] + L*ezw[ ] + aw*(exw[ ]*cosφ+eyw[ ]*sinφ) (c01)

円筒下端面の円板の座標は

r[ ] = rwL[ ] + aw*(exw[ ]*cosφ+eyw[ ]*sinφ), rwL[ ] = rw[ ] (c02)

円筒上端面の円板の座標は

r[ ] = rwU[ ] + aw*(exw[ ]*cosφ+eyw[ ]*sinφ), rwU[ ] = rw[ ] + Lw*ezw[ ] (c03)

・掌面と交わる円筒はその(X,Z)面内で楕円となる．

　その楕円と掌円との最短位置をrc[ ], 最短距離をDD, rc[ ]のときの円筒軸長をLcとする．

・円筒面・円環と掌面との干渉は，

DD = rc[y) - aa/2 < 0 (c04)

dR = sqrt(r[x]^2+r[z]^2) – rr < 0 (c05)

0<Lc<Lw (c06)

　のときに生じる．

計算：

・円筒面がy=aa/2を切るときの(L,φ)：

r[y] = rw[y] + L*ezw[y] + aw*(exw[y]*cosφ+eyw[y]*sinφ) = aa/2 (c1-1)

(1) ezw[y]=0：円筒軸が掌面と平行のとき：

・軸線が掌面以下の距離で掌円板内のあれば干渉

・軸直線の式

rs[ ] = rw[ ] + L*ezw[ ] (c1-2)

(1-1) 円筒軸と掌面との距離

DD = rw[y] – aa/2 < 0 (C1-1)

(1-2)，有限軸長が掌半径内にある

・円筒軸を含む平面の原点をB:rb[ ]とする．

rb[ ] = (0, rw[y], 0)

・軸直線のBからの距離 Rb

Rb^2 = ra[x]^2 + ra[z]^2

= rw[x]^2 + rw[z]^2 + 2*L*( rw[x]*ezw[x] + rw[z]*ezw[z] ) + L^2*( ezw[x]^2 + ezw[z]^2 )

< rr^2 (c1-3)

→ A*L^2 + 2*B*L + C < 0 (c1-4)

A≡ ezw[x]^2 + ezw[z]^2 (c1-5)

B≡ rw[x]*ezw[x] + rw[z]*ezw[z] (c1-6)

C≡ rw[x]^2 + rw[z]^2 – rr^2 (c1-7)

　　　D≡ B^2 – A*C (c1-8)

・D<0：

　軸線は半径rr内二存在しない → 干渉なし

・D≧0

L1,2 = ( -B±√(B^2 – A+C) ) / A (c1-9)

軸線が半径rr内にある条件：

L1≦L≦L2 (C1-2)

・円筒軸が掌面と平行のときお干渉条件は(C1-1), (C1-2)である．

(2) ezw[y]≠0のとき

・簡単のため円筒軸は+Y側に向いているとする．

　（掌面に近い方に円筒基点rw[ ]を定める．ezw[y]>0）

・円筒軸の式

rs[ ] = rw[ ] + L*ezw[ ] (c2-1)

・円筒面の式

r[ ] = rs[ ] + aw*( exw[ ]*cosφ+eyw[ ]*sinφ) (c2-2)

・掌面：中心O(0,0,0), 半径rr, 厚さaa, +Y方向を法線とする．

・円筒の掌面y=aa/2の切り口は楕円となる．

この楕円の掌面内原点(0,aa/2,0)から最近点Cまでの距離DD<rr+awのとき干渉する．

円筒軸がy=aa/2と交わる点：

rs[y] = rw[y] + L*ezw[y] = aa/2 (c2-3)

L = Le = (aa/2 – rw[y] ) / ezw[y] (c2-4)

・円筒軸の(X,Z)座標から見たEuler角(α,β)：

β = acos( ezw[y] ) (c2-5)

α = atan2( ezw[z], ezw[x] ) (c2-6)

・楕円の短径a, 長径bは

a = aw (c2-7)

b = aw / ezw[y] ( ezw[y]>0 ) (c2-8)

　短長径を軸とする楕円の式

(x/a)^2 + (y/b)^2 = 1 (c2-9)

(X,Z)面内の楕円長径はZ軸からX軸に向けてαの方向にあるとする．

(X,Z)を（X,Y）と読み替える．

・上記楕円を正規楕円とし掌円を座標変換する．

掌円の中心Op:：

　 X = (0-ra[x])*cosα - (0-ra[z])*sinα (c2-10)

　　　Y = (0-ra[x])*sinα + (0-ra[z])*cosα (c2-11)

(x – X)^2 + (y – Y)^2 = rr^2 (c2-12)

・楕円輪郭曲線の座標変換した掌円からの最近点をC(xc, zc)とする．

・楕円のC点の接線t とn=PCは直交する．

　 2(x/a^2)*dx + 2(y/b^2)*dy = 0 (c2-13)

t = dy/dx =- (x*b^2) / (y*a^2) (c2-14)

n = (Y – y ) / (X - x) (c2-15)

t*n = -(x*b^2,)*(Y – y) / { (y*a^2)*(X – x) } = -1

→ -(x*b^2,)*(Y – y) + (y*a^2)*(X – x) = 0

x*y*(b^2 – a^2) – (b^2*Y)*x + (a^2*X)*y = 0

= x*y*BA – By*x + Ax*y = 0 (c2-16)

・楕円の式(c2-9)と接線・法線直交の式(c16)からC(xc, zc)が得られる．

　この連立方程式のyを消去すればxの4次方程式となり，解析的に解を得る．

(2-1) X≠0, Y≠0の場合：

・(c2-16)より

y = b^2*Y*x / ( x*(b^2-a^2) + a^2*X )

≡By:* x / (BA*x + Ax)　　　　　　　　　 (c2-17)

(c2-9)に代入

x^2 / A + { By*x / ( BA*x +Ax) }^2 / B = 1

= x^2 * [ 1/A + { By / ( BA*x + Ax ) }^2 ] / B (c2-19)

両辺×( BA*x + Ax ) }^2 * B * A

→ x^2 * [ (BA*x +Ax)^2 }*B + By^2*A ] = ( BA*x + Ax)^2*B*A

= BA^2*B*x^4 + 2*BA*Ax*B*x^3 + (Ax^2*B + By^2*A)*x^2

- { BA^2*A*B*x^2 + 2*BA*Ax*A*B*x + Ax^2*A*B } = 0

= H4*x^4 + H3*x^3 + H2*x^2 + H1*x + H0 = 0 (c2-20)

H4≡BA^2*B (c2-21)

　　H3≡2*BA*Ax*B (c2-22)

H2≡(Ax^2*B + By^2*A) – (BA^2*A*B) (c2-23)

H1≡-2*BA*Ax*A*B (c2-24)

　　H0≡- Ax^2*A*B (c2-25)

この式からxが求まる．

・(c2-17)からyを求める．

y = By:* x / (BA*x + Ax)　　　　　　　　　 (c2-17)

・（X,Z）面上の原点とWrk円筒の距離は

Opが楕円の外にあるとき

DP = sqrt( (x-X)^2 + (y-Y)^2 ) (c2-26)

Opが楕円の内にあるとき

DP =-sqrt( (x-X)^2 + (y-Y)^2 ) (c2-27)

干渉の第1条件は

DD = DD – rr<0 (C2-1)

・最近点は

rcm[ ]=(x,y)　　 (c2-28)

である．

元の座標系に戻してRC[ ]とする．円筒面上のRC[ ]は

RC[ ] = rw[ ] + Lc*ezw[ ] + aw*( exw[ ]*cosφc + eyw[ ]*sinφc ) (c2-29)

・干渉の第2条件は

0< Lc < Lw (C2-2)

　Lcの計算は(3)へ

(2-2) Y=0の場合(OpがX軸上)

(C2-16)より

y = 0 (c2-17)’

X>0のとき x = a, DP = X – a, DD = DP - ap

　X<0のとき　 x =-a DP =-(X + a), DD = DP – ap

(2-3) X=0の場合(OpがY軸上)

x = 0

Y>0のとき y = b, DP = Y – b, DD = DP - ap

Y<0のとき y =-b, DP =-(Y + b), DD = DP - ap

(3) 円筒と掌円との干渉

・上記楕円と掌円とが重ならなければ（DD>0）干渉しない．

・重なるとき(DD<0)は掌円内にある楕円の最近点Cが有限円筒内にあるとき干渉，なければ非干渉である．

・C点を含む円筒を輪切りにした円環の中心をOcとする．円筒基点O_AからOcまでの軸長さをLcとすると

干渉条件は

0<Lc<Lw

・掌面内楕円の座標変換した式は

(x/a)^2 + (y/b)^2 = 1, a=aw, b=aw/cosβ

・最近点Cは(c2-20), (C2-17)の(x, y)で与えられ，

rcm[ ] = (x, y) = (xcm, ycm)

　とする．　

　Cは楕円中心Oeから長径方向にycmだけ離れた位置にある．

　Cを円筒軸に投影した点OcとOe間の距離Lcmは

Lcm = |ycm|*sinβ

・Oe点の軸方向距離（円筒基点O_Aからの距離）はLeであるからOc点の軸方向距離Lcは

Lc = Le - Lcm

干渉の第2の条件は

0<Lc<Lw (C3-1)