# Pose Rotation Order

**URL:** <https://forum.universal-robots.com/t/pose-rotation-order/223>\
**Category:** URScript\
**Created:** [September 9, 2016, 2:31pm UTC](https://forum.universal-robots.com/t/pose-rotation-order/223 "2016-09-09T14:31:41Z")\
**Posts on this page:** 4\
**Page:** 1

<div class="post-metadata">

**Author:** ![anon48882232](https://avatars.discourse-cdn.com/v4/letter/a/e480ec/32.png) [@anon48882232](https://forum.universal-robots.com/u/anon48882232)\
**Post date:** [September 9, 2016, 2:31pm UTC](https://forum.universal-robots.com/t/pose-rotation-order/223/1 "2016-09-09T14:31:42Z")

</div>

In the description of a Pose, it says the last 3 values are “an axis-angle”, but if you look at a Waypoint, it shows a Rotation Vector as RX, RY, RZ. I am used to working with compound Euler angles (Yaw, Pitch, Roll), but haven’t worked with a rotation vector before.

Is there a way to convert between RPY and a Rotation Vector, either through math or a built-in script function, or a way a pose can be defined directly using YPR? I can’t find any examples in the manual or an explanation any further than pointing to a wikipedia page.

---

<div class="post-metadata">

**Author:** ![jbm](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/jbm/32/614_2.png) [@jbm](https://forum.universal-robots.com/u/jbm)\
**Post date:** [September 9, 2016, 2:42pm UTC](https://forum.universal-robots.com/t/pose-rotation-order/223/2 "2016-09-09T14:42:07Z")

</div>

You can use the URScript codes `rotvec2rpy(rotation_vector)` and `rpy2rotvec(rpy_vector)` to convert the rotation vectors to RPY and vice versa in URScript.  
They are both explained in the [script manual](http://www.universal-robots.com/download/?option=22197#section21968).

Below are some example formulas that can be used in URScript before the above functions were implemented (3.3.0.145), other methods might work just as fine.

For conversion from RPY to rotation vector:

```auto
  def rpy2rv(roll,pitch,yaw):
  
  alpha = yaw
  beta = pitch
  gamma = roll
  
  ca = cos(alpha)
  cb = cos(beta)
  cg = cos(gamma)
  sa = sin(alpha)
  sb = sin(beta)
  sg = sin(gamma)
  
  r11 = ca*cb
  r12 = ca*sb*sg-sa*cg
  r13 = ca*sb*cg+sa*sg
  r21 = sa*cb
  r22 = sa*sb*sg+ca*cg
  r23 = sa*sb*cg-ca*sg
  r31 = -sb
  r32 = cb*sg
  r33 = cb*cg
  
  theta = acos((r11+r22+r33-1)/2)
  sth = sin(theta)
  kx = (r32-r23)/(2*sth)
  ky = (r13-r31)/(2*sth)
  kz = (r21-r12)/(2*sth)
  
  rv[0] = theta*kx
  rv[1] = theta*ky
  rv[2] = theta*kz
  
  return rv
  
  end

```

For conversion from rotation vector to RPY:

```auto
  def rv2rpy(rx,ry,rz):
  
  theta = sqrt(rx*rx + ry*ry + rz*rz)
  kx = rx/theta
  ky = ry/theta
  kz = rz/theta
  cth = cos(theta)
  sth = sin(theta)
  vth = 1-cos(theta)
  
  r11 = kx*kx*vth + cth
  r12 = kx*ky*vth - kz*sth
  r13 = kx*kz*vth + ky*sth
  r21 = kx*ky*vth + kz*sth
  r22 = ky*ky*vth + cth
  r23 = ky*kz*vth - kx*sth
  r31 = kx*kz*vth - ky*sth
  r32 = ky*kz*vth + kx*sth
  r33 = kz*kz*vth + cth
  
  beta = atan2(-r31,sqrt(r11*r11+r21*r21))
  
  if beta > d2r(89.99):
  beta = d2r(89.99)
  alpha = 0
  gamma = atan2(r12,r22)
  elif beta < -d2r(89.99):
  beta = -d2r(89.99)
  alpha = 0
  gamma = -atan2(r12,r22)
  else:
  cb = cos(beta)
  alpha = atan2(r21/cb,r11/cb)
  gamma = atan2(r32/cb,r33/cb)
  end
  
  rpy[0]= gamma
  rpy[1]= beta
  rpy[2]= alpha
  
  return rpy
  
  end

```

Additionally, check out [this article](http://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/rpy-tofrom-rotation-vector-18958/).

---

<div class="post-metadata">

**Author:** ![anon48882232](https://avatars.discourse-cdn.com/v4/letter/a/e480ec/32.png) [@anon48882232](https://forum.universal-robots.com/u/anon48882232)\
**Post date:** [September 9, 2016, 2:47pm UTC](https://forum.universal-robots.com/t/pose-rotation-order/223/3 "2016-09-09T14:47:41Z")

</div>

Thanks so much for the help. I’m currently using 3.2 so that explains why I couldn’t find anything. This is exactly what I was looking for

---

<div class="post-metadata">

**Author:** ![androidflorent](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.universal-robots.com/androidflorent/32/3314_2.png) [@androidflorent](https://forum.universal-robots.com/u/androidflorent)\
**Post date:** [September 16, 2021, 4:41am UTC](https://forum.universal-robots.com/t/pose-rotation-order/223/4 "2021-09-16T04:41:46Z")

</div>

This code is a good approach, but beware of gimbal lock.  
For example `rv2rpy(0,180,180)` leads to a division by zero.

Another solution is to use quaternions, like in this library: [Convert position types - UnderAutomation](https://underautomation.com/fr/universal-robots/documentation/tools)

```auto
Pose p = new Pose(x, y, z, rx, ry, rz);

Pose rpy = p.FromRotationVectorToRPY();

Pose rotVect = p.FromRPYToRotationVector();

```