procedure rpy2rotvec (: : vec: rx, ry, rz)
tuple_cos (vec, Cos)
tuple_sin (vec, Sin)
create_matrix (3, 3, [Cos[2], -Sin[2],0,Sin[2],Cos[2],0,0,0,1], Y)
create_matrix (3, 3, [Cos[1],0, Sin[1],0,1,0, -Sin[1],0, Cos[1]], P)
create_matrix (3, 3, [1,0,0,0,Cos[0], -Sin[0],0,Sin[0],Cos[0]], R)
mult_matrix (Y, P, 'AB', YP)
mult_matrix (YP, R, 'AB', r)
get_full_matrix (r, Values)
tuple_acos ((Values[0]+Values[4]+Values[8]-1)/2, theta)
tuple_sin (theta, sint)
multi := 1/(2*sint)*theta
rx := multi*(Values[7]-Values[5])
ry := multi*(Values[2]-Values[6])
rz := multi*(Values[3]-Values[1])
return ()
halcon实现rpy转换旋转矢量函数
最新推荐文章于 2024-04-23 19:14:22 发布