Home Programming

Discussion

Left ArrowBack to discussions page
adhecht24adhecht24 Posts: 7 Apprentice
Hello,

I would need the mathematical formula for the function "PoseTrans".
Background: Position preset by a controller.

And then I still need the conversion from the Rotationsvetoren to the RPY values.

Can anybody help me further ?

Comments

  • mkardasinskimkardasinski Posts: 34 Handy
    This is code I use for machine vision camera which provides data in °RPY
      
      Cx = cos(°R / 2)
      Sx = sin(°R / 2)
      Cy = cos(°P / 2)  
      Sy = sin(°P / 2)
      Cz = cos(°Y / 2)
      Sz = sin(°Y / 2)  
    
      x = Sx*Cy*Cz - Cx*Sy*Sz
      y = Cx*Sy*Cz + Sx*Cy*Sz
      z = -Sx*Sy*Cz + Cx*Cy*Sz
    
      Norm = sqrt(x^2 + y^2 + z^2)
      θ = 2 * acos(Cx*Cy*Cz + Sx*Sy*Sz)
    
      VectorX = x * (θ / Norm)
      VectorY = y * (θ / Norm)
      VectorZ = z * (θ / Norm)
    

Sign In or Register to comment.
Left ArrowBack to discussions page