halcon实现rpy转换旋转矢量函数

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 ()