예제 #1
0
파일: pose.py 프로젝트: niberger/joysix 
def dexp(v):
	r = v[0:3]
	dexpr = quat.dexp(r)
	MT = mt(v)
	return np.bmat([[dexpr,MT],[np.zeros((3,3)),dexpr]])
예제 #2
0
파일: pose.py 프로젝트: niberger/joysix 
def exp(v):
	r = v[0:3]
	t = v[3:6]
	q = quat.exp(r)
	D = quat.dexp(r)
	return Pose(q, D * t)