0
$\begingroup$

Hi I've got a problem with the function tf.euler_from_quaternion

I've got an end-effector's orientation using moveit_commander

and I want to convert quaternion to euler

When quaternion value is 0 0 0 1, euler_from_quaternion function convert very well (rpy:000)

But, when quaternion value is 0 0.707 0 0.707, I was expecting that rpy is 0 90 0, however uler_from_quaternion function gave me 82 89 80

when I inserted (0, 0.7, 0, 0.7 ), convert to 0 90 0 but this function couldn't convert (0, 0.707, 0, 0.707) I think there was a very little difference

I don't know why... Someone else tell me the problem and how to fix this problem??

This is my code

def print_rpy(self): global stop_loop group = self.group

current_pose = group.get_current_pose().pose
init_x = current_pose.orientation.x
init_y = current_pose.orientation.y
init_z = current_pose.orientation.z
init_w = current_pose.orientation.w
print("init qua: ",init_x,init_y,init_z,init_w)

init_q_list = [init_x, init_y, init_z, init_w]
(init_roll, init_pitch, init_yaw) = euler_from_quaternion(init_q_list)
init_roll = round(init_roll * 180/pi,2)
init_pitch = round(init_pitch * 180/pi,2)
init_yaw = round(init_yaw * 180/pi,2)
print("init rpy : ", init_roll, init_pitch, init_yaw)

while (1):
  with stop_lock:
    if stop_loop:
      break

  current_pose = group.get_current_pose().pose
  current_x = current_pose.orientation.x
  current_y = current_pose.orientation.y
  current_z = current_pose.orientation.z
  current_w = current_pose.orientation.w
  print("current qua : ", current_x,current_y,current_z,current_w)

  q_list = [current_x, current_y, current_z, current_w]
  (r, p, y) = euler_from_quaternion(q_list, axes='sxyz')
  r = round(r * 180/pi,2)
  p = round(p * 180/pi,2)
  y = round(y * 180/pi,2)
  
  print("current rpy : ", r,p,y)
  error_r = init_roll-r
  error_p = init_pitch-p
  error_y = init_yaw-y
  
  print("error roll : ", error_r)
  print("error pitch : ", error_p)
  print("error yaw : ", error_y)

  rpy=str(error_r)+','+str(error_p)+','+str(error_y)
  # print(rpy)
  
  try:
    ser.write(rpy.encode('utf-8'))
    # print(ser.readline())
    print("connected", rpy)
  except:
    print("connect error")
$\endgroup$
2
  • $\begingroup$ Please edit your post to show us complete code that can be run. Are you using the euler_from_quaternion() function provided by ros? $\endgroup$
    – Mike973
    Oct 14, 2023 at 13:53
  • $\begingroup$ I've edited the question. Thank you $\endgroup$
    – 정현재
    Oct 17, 2023 at 8:56

2 Answers 2

1
$\begingroup$

RPY angles have a singularity for pitch values of 90° and -90°. This means that, for a given orientation that yields a pitch of 90°, it is not possible to uniquely define the roll and yaw angles.

This is due to the X and Z axis being parallel, so there is no distinction between rotating around X or rotating around Z. I.e. the same orientation quaternion results from:

  • rotating the full angle around X and zero around Z, or
  • full angle around Z and zero around X, or
  • any combination in between.

This is also called 'gimbal lock', see this nice animation on Wikipedia.

As a consequence, Quaternion to RPY conversion is ill-conditioned in the neighbourhood of a pitch value of +/- 90°. Depending on the numerical precision, you can get varying results.

and how to fix this problem??

This depends on your use case.

For my application, I simply rotated the reference frame of my end effector by 90°, so that the end-effector X-axis is never parallel to the world Z-axis in the motion range of my robot task. So I avoid that the RPY representation of {end effector} wrt {world} would be singular.

Alternatively, an option might be to use a different representation, e.g. ZXZ-Euler angles. But it is important to realise that each representation of an orientation by 3 variables has its singularities.

How about to set my robot's orientation to 0, 89, 0 or 0, 91, 0??

A pitch angle of 89° or 91° corresponds to a different orientation than a pitch angle of 90°. So if you command your robot end effector to move to an orientation with a pitch of 89° or 91°, it will have a different physical orientation than for a pitch of 90°. Whether that is or is not acceptable for your task is something only you can assess.

$\endgroup$
1
  • $\begingroup$ Ah.. I've already known 'gimbal lock' phenomenon, but i didn't know that gimbal lock is applied to this conversion.. Think you for teaching me. Then what should I do to solve this problem? How about to set my robot's orientation to 0, 89, 0 or 0, 91, 0?? $\endgroup$
    – 정현재
    Oct 17, 2023 at 9:18
0
$\begingroup$

I get the results I expect, which look very different from yours:

>>> import tf
>>> tf.transformations.euler_from_quaternion((0, 0.707, 0, 0.707))
(-0.0, 1.5707963267948966, 0.0)
>>> tf.transformations.euler_from_quaternion((0, 0.70, 0, 0.70))
(-0.0, 1.5707963267948961, 0.0)
$\endgroup$
2
  • $\begingroup$ Yes. that was just example. I'll show you my real example tf.transformations.euler_from_quaternion((0.001347211885, 0.706929032, 0.000874635079, 0.7072826612883255)) (1.4132943121198136, 1.567614609686458, 1.412625986733099) This is my problem $\endgroup$
    – 정현재
    Oct 17, 2023 at 8:57
  • $\begingroup$ I added some suggestions to my answer. $\endgroup$
    – JRTG
    Oct 17, 2023 at 11:50

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.