Пример #1
0
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
def exp(v):
	r = v[0:3]
	t = v[3:6]
	q = quat.exp(r)
	D = quat.dexp(r)
	return Pose(q, D * t)