
̂̐ڐGLCPւ̒莮

tazz

QƁF
http://www.diku.dk/undervisning/2005f/101/											


1. LCP (Linear Complimentarity Program)̒莮

LXgF

u		: ê̑xׂxNg
udot	: u̎Ԕ
w		: ڐG_ł̑x
J		: uw^郄Rrs. w = J u
M		: s
dt		: IC[ϕ̎ԃXebv
		: ڐG * dt
Fext	: O͂уRI


^
	M udot = J' /dt + Fext
IC[ϕɂ
	u(t+1) = u(t) + M^-1 J'  + dt M^-1 Fext
J
	w = J M^-1 J'  + J (u(t) + dt M^-1 Fext)
A = J M^-1 J'
b = J (u(t) + dt M^-1 Fext)
Ƃ
	w = A  + b

ɁCŔCÎ~CCCȂǂl邱Ƃɂ
ȉ̐𓾂
	_i = _lo_i				==> w_i >= 0
	_i = _hi_i				==> w_i <= 0
	_lo_i < _i < _hi_i	==> w_i = 0
̉ɂLCP(A, b, _hi, _lo)Ə

@:
A = D - NƂƂCKȃ[0]lƂāCQ
	[k+1] = LCP(D, -N [k] + b, _hi, _lo)
ɂ[k]XVĂD̂Ƃ
	[k+1] = [k]
藧Ȃ
	[k] = LCP(A, b, _hi, _lo)
ƂȂ茳̖肪ƂɂȂ
āCDΊpsȂ΁CLCP(D, -N [k] + b, _hi, _lo)̉
	[k+1] = min(max(_lo, D^-1(N [k] - b)), _hi)
œDvɂ̎JԂvZ΂悢D

ȉ͏ڍׂȎWJ
N : ̂̐
K : ڐG̐
lhs(i) : iԖڂ̐ڐǴuv̍̂̔ԍ
rhs(i) : iԖڂ̐ڐǴuEv̍̂̔ԍ
	lhs(i) < rhs(i)

[\m]
s̐C = A Bɂ
C(i,j) = Arow(i)' Bcol(j) = _k=1^n A(i,k) B(k,j)		(n = A̗ = B̍s)

[es3x3ubN̓e]
sA(i,j)Ԗڂ3x3sA(i,j)ƏD

size(M) = (6N, 6N)
M(i,j) =
	O		(i != j)
	m_k * 1	(i = 2k)	m_kkԖڂ̍̂̎, 13x3Pʍs
	I_k		(i = 2k+1)	I_kkԖڂ̍̂̊e\

size(J) = (3K, 6N)
J(i,j) =
	J_lin(i,lhs(i))		(j == 2lhs(i))
	J_ang(i,lhs(i))		(j == 2lhs(i)+1)
	J_lin(i,rhs(i))		(j == 2rhs(i))
	J_ang(i,rhs(i))		(j == 2rhs(i)+1)
	O					otherwise
Jlin(i, j)͍jvelocity (j̃[JW)ڐG_ȋxiڐG_Wjւ̃RrsC
Jang(i, j)͍jangular velocity (j̃[JW)ڐG_ȋxiڐG_Wjւ̃RrsD
܂u = Jlin * v + Jang * w (u:ڐG_xCv:̑xCw:̊px)

T := M^-1 J'
T(i,j)
	= _k=1^2N M^-1(i,k) J'(k,j)
	= M(i,i)^-1 J(j,i)'
	=	1/m_lhs(j)  J_lin(j,lhs(j))'	:= T_lin(j,lhs(j))	(i == 2lhs(j))
		I_lhs(j)^-1 J_ang(j,lhs(j))'	:= T_ang(j,lhs(j))	(i == 2lhs(j)+1)
		1/m_rhs(j)  J_lin(j,rhs(j))'	:= T_lin(j,rhs(j))	(i == 2rhs(j))
		I_rhs(j)^-1 J_ang(j,rhs(j))'	:= T_ang(j,rhs(j))	(i == 2rhs(j)+1)
		O							otherwise

A = J M^-1 J' = J T
size(A) = (3K, 3K)
A(i,j) = _k=1^2N J(i,k) T(k,j) =
	(lhs(i),lhs(j)) * J_lin(i,lhs(i)) T_lin(j,lhs(j)) + J_ang(i,lhs(i)) T_ang(j,lhs(j))) +
	(lhs(i),rhs(j)) * J_lin(i,lhs(i)) T_lin(j,rhs(j)) + J_ang(i,lhs(i)) T_ang(j,rhs(j))) +
	(rhs(i),lhs(j)) * J_lin(i,rhs(i)) T_lin(j,lhs(j)) + J_ang(i,rhs(i)) T_ang(j,lhs(j))) +
	(rhs(i),rhs(j)) * J_lin(i,rhs(i)) T_lin(j,rhs(j)) + J_ang(i,rhs(i)) T_ang(j,rhs(j)))
	=
	(lhs(i),lhs(j)) * (1/m_lhs(i) J_lin(i,lhs(i)) J_lin(j,lhs(j))' + J_ang(i,lhs(i)) I_lhs(i)^-1 J_ang(j,lhs(j))') +
	(lhs(i),rhs(j)) * (1/m_lhs(i) J_lin(i,lhs(i)) J_lin(j,rhs(j))' + J_ang(i,lhs(i)) I_lhs(i)^-1 J_ang(j,rhs(j))') +
	(rhs(i),lhs(j)) * (1/m_rhs(i) J_lin(i,rhs(i)) J_lin(j,lhs(j))' + J_ang(i,rhs(i)) I_rhs(i)^-1 J_ang(j,lhs(j))') +
	(rhs(i),rhs(j)) * (1/m_rhs(i) J_lin(i,rhs(i)) J_lin(j,rhs(j))' + J_ang(i,rhs(i)) I_rhs(i)^-1 J_ang(j,rhs(j))')
ɑΊpubN݂̂ɒڂ
A(i,i) =
	J_lin(i,lhs(i)) T_lin(i,lhs(i)) + J_ang(i,lhs(i)) T_ang(i,lhs(i)) +
	J_lin(i,rhs(i)) T_lin(i,rhs(i)) + J_ang(i,rhs(i)) T_ang(i,rhs(i))
	=
	1/m_lhs(i) J_lin(i,lhs(i)) J_lin(i,lhs(i))' + J_ang(i,lhs(i)) I_lhs(i)^-1 J_ang(i,lhs(i))' +
	1/m_rhs(i) J_lin(i,rhs(i)) J_lin(i,rhs(i))' + J_ang(i,rhs(i)) I_rhs(i)^-1 J_ang(i,rhs(i))'	

̓NlbJ[f^((i,j) == 1 if i == j or 0 otherwise)

b = J Vnc		(Vnc = u(t) + dt M^-1 Fext)
size(b) = 3K
b(i) = _k=1^2N J(i,k) Vnc(k) =
	J_lin(i,lhs(i)) Vnc(2lhs(i)) + J_ang(i,lhs(i)) Vnc(2lhs(i)+1) +
	J_lin(i,rhs(i)) Vnc(2rhs(i)) + J_ang(i,rhs(i)) Vnc(2rhs(i)+1)

ȉ͔vZ̏ڍ
As̑Ίpvf𔲂oΊpsDƂƁCˉex̔
	[k+1] = D^-1((D - A)[k] - b) = [k] - D^-1 A [k] - D^-1 b
ƂȂDeɒڂ
	[k+1][i] = [k][i] - (b[i] + Arow[i] [k]) / D[i,i]
Arow[i]AiԖڂ̍sDA = J M^-1 J'Arow[i] = Jrow[i] M^-1 J'D
ď㎮
	[k+1][i] = [k][i] - _b[i] - _Jrow[i] dV[k]
_b[i] = b[i]/D[i,i]C_Jrow[i] = Jrow[i]/D[i,i], dV[k] = M^-1 J' [k].
As͂̑Ίpvf̂݌vZ΂悢ƂɂȂD

/////////////////////////////////////////////////////////////////////////////////////////
Iterative LCP ̎ɊւāF
_IɌɂ͏Ă܂̂ň炸B

܂C
	find z s.t. z >= 0, (Az + b) >= 0, (Az + b)'z = 0
̉WLCP(A, b)ƏD
WƏ̂zӂƂ͌ȂD

LCP̔@Ƃ́CA = B + C Ȃ(B, C)lCKȏlz[0]ɂĔ
	z[k+1] = LCP(B, b + C z[k])
JԂ@łD
	z[k+1] >= 0, B z[k+1] + C z[k] + b >= 0, (B z[k+1] + C z[k] + b)'z[k+1] = 0
Ȃ̂ŁCz[k+1] = z[k]Ȃz[k+1]LCP(A, b)Ɋ܂܂C肪ƂɂȂD

BȂΊpvfΊpsƂ镪pƂCˉeRr@ƌĂԁD
	B = D, C = U + L
BȂΊpvfщOpvfsƂCˉeKEXUCf@ƌĂԁD
	B = L + D, C = U
萔ւɂāC
	B = L + ^-1 D, C = (1 - ^-1)D + U
ƕƂCSOR@ƌĂԁD

vfxł͂ȂCubNxœlɕƂ͎@̓ɃubN`tD

āC͏̔삪邩łD

********************************************************************************
AŝƂB - Csł邱ƂłD
********************************************************************************
A͐sƂD
SOR@ɂāCւ
	0 <  < 2
𖞂ƂCSOR@̔ɂ萶n͉ɎD
********************************************************************************


/////////////////////////////////////////////////////////////////////////////////////////
֐߂̓

ݒF
ڐGɊ֗^鍄̂̓C͊֐߂łȂĂD
֐ߌn
	J[0], J[1], ..., J[M-1]
ƂC֐J[i]ɂȂĂ鍄̂
	S[i][0], S[i][1], ..., S[i][N[i]]
ƂD
܂Cǂ̊֐ߌnɂȂĂȂ̂
	S[M][0], ..., S[M][N[M]]
ƂD
e֐ߌn̈ʉWq[i] (i = 0,1,...,M-1)ƁC
֐ߌnɂȂĂȂê̍Wq[M]ȂxNg
	q = [q[0]; q[1]; ...; q[M]]
ƂD
֐ߌnJ[i]̉^
	M(q[i]) q''[i] = h(q[i], q'[i]) + [i]
ƏD
