UR5 how to convert axis angles to quaternions

asked 2022-02-18 12:09:28 -0500

zahid990170
135 ●30 ●43 ●48

updated 2022-02-21 05:50:37 -0500

Hi,

We have a UR5 robotic arm. We use Moveit move_group_interface to send simple pose and joint-space goals to this arm. In particular for the pose message of the type,

  geometry_msgs::Pose pose_msg;

the position component is reached correctly. But, I cannot correctly find the orientation component. In UR5, what we read on PolyScope is axis-angle representation given as a rotation vector [rx, ry, rz]. But, for ROS side, we need quaternion representation. I have followed some help online, but still cannot get the right orientation of the tool point. For example, this suggested an approach, which I have followed,

  std::vector<double> axis_angles{ang_a, ang_b, ang_c};  // as read on the polyscope
  double theta = GetNorm(axis_angles);     // sqrt(ang_a² + ang_b² + ang_c²) 

  double ux, uy, uz;

  ux = ang_a/theta;
  uy = ang_b/theta;
  uz = ang_c/theta;

  tf::Vector3 unit_vector(ux, uy, uz);
 tf::Quaternion quat;
tf::Quaternion quat_inv(tfScalar(-0.5),tfScalar(0.5),tfScalar(-0.5),tfScalar(-0.5));
quat.setRotation(unit_vector,theta);
quat *= quat_inv;

pose_msg.orientation.x = quat.getX();
  pose_msg.orientation.y = quat.getY();
  pose_msg.orientation.z = quat.getZ();
  pose_msg.orientation.w = quat.getW();

However, the final orientation of the last link is inverted (facing up). I would like it to be so that its perpendicular to a table surface below (facing down), so that an attached gripper can pick things from the table.

I also tried the following

tf2::Vector3 axis(ux, uy, uz);
  tf2::Quaternion Quat(axis, theta);

Still cannot get the the orientation to what I encode in the pose message.

How can I correctly specify in ROS the orientation for a UR robotic arm given the axis angles.

*EDIT

So following the suggestions given by @fvd, I have made test on UR5 robotic arm. I attach the picture from three cases below.

(a) Using the method above that I described, but discarding the quat *= quat_inv; part. In a few tests performed, I could not see valid motions, and usually the motion of stopped due to the arm violating some safety boundary set on the teach pendant.

(b) Using the method above, and keeping quat *= quat_inv; part. image description

teach pendant reads.

X = 138.28
Y = -539.49
Z = 394.44
RX = 1.9577
RY = -2.5844
RZ = -0.0577

teach pendant reads.

X = 137.80
Y = -548.39
Z = 391.40
RX = 0.3176
RY = -3.3345
RZ = -2.7655

This was the pose string that was given to the motion planner.

std::string s = "[-0.14527758743934377, 0.5449976892506504, 0.38858309510149047, -0.15787628477528537, 0.2747166458598104, 0.5450830459979051]";

(Note that, x and y coordinates of the pose are opposite signs between ROS MoveIt and teach pendant, in ROS the values are given in radians, and in meters, wheres on teach pendant, the x, y, z of the pose are given in millimeters. )

thanks, Zahid

edit retag flag offensive close merge delete

Comments

Sorry, I can't get into the details right now. I suggest that you use a package like rviz_visual_tools to publish Coordinate System markers to visualize different orientations in Rviz and debug the different methods. Planning with the robot can fail for unrelated reasons. You should check if the results for your orientations are consistent, not if a motion plan can be found.

fvd ( 2022-02-21 07:18:56 -0500 )edit

I'd guess this is an issue with frames. Viewing the link you posted, it looks like the base frame has X pointing back, Y pointing right, and Z pointing up. The quat_inv you showed above is meant to adjust these frames so that X points down and Z pointing right, which may not be right for you. I'd play with those values.

pweids ( 2022-02-22 08:21:41 -0500 )edit

I need to pick up an object with the UR3 manipulator. But the orientation I get from the object view in the workspace has the Z-axis coordinate reversed. I guess this boils down to mirroring around the XY plane. To mirror a quaternion around the XY plane, I think you need to negate the Z value and the W value. Can you confirm? Except that when I negate the values the result is not correct. But if I add to Roll pi I already have the valid value. Only this is not a good solution. How can I invert the Z of the object to be coincident with the Z of the gripper?

RB_Code ( 2022-04-06 13:41:19 -0500 )edit

hi @RB_Code, I was having the same issue. the final orientation is reversed (upside down Z). I could not get it to work, The workaround that we did (and, this is not a proper solution), was to firstly, take the robotic arm to home position, where the gripper is perpendicular to the ground. This "home" position is a named target. And, then for reaching the goal target pose, I just copy the orientation from this "home" position to the orientation part of the target pose. Please update, if you get any final working solution. thanks,

zahid990170 ( 2022-04-07 08:20:36 -0500 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2022-02-19 07:07:56 -0500

fvd
2180 ●15 ●98 ●56

updated 2022-02-20 06:15:50 -0500

I wrote these functions in the O2AC package that used two UR5e robots and which I talked about at ROS World. They're in Python, but might be useful as a reference:

def ur_axis_angle_to_quat(axis_angle):
    # https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions
    angle = norm2(*axis_angle)
    axis_normed = [axis_angle[0]/angle, axis_angle[1]/angle, axis_angle[2]/angle]
    s = sin(angle/2)
    return [s*axis_normed[0], s*axis_normed[1], s*axis_normed[2], cos(angle/2)]  # xyzw


def quat_to_ur_axis_angle(quaternion):
    # https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions
    # quaternion must be [xyzw]
    angle = 2*atan2(norm2(quaternion[0], quaternion[1], quaternion[2]), quaternion[3])
    if abs(angle) > 1e-6:
        axis_normed = [quaternion[0]/sin(angle/2), quaternion[1]/sin(angle/2), quaternion[2]/sin(angle/2)]
    else:
        axis_normed = 0.0
    return [axis_normed[0]*angle, axis_normed[1]*angle, axis_normed[2]*angle]

Please post the C++ functions if you write them.

edit:

You say "the final orientation of the link is inverted". Why don't you just remove the line quat *= quat_inv;?

edit flag offensive delete link

Comments

thanks @fvd, please see the update to the question.

zahid990170 ( 2022-02-21 04:22:47 -0500 )edit

add a comment

answered 2022-04-07 08:49:10 -0500

RB_Code
11 ●4 ●11 ●12

Hi @zahid990170

I wrote some functions based on @fvd functions, it works well if the ar_tag is in a table with different orientations as long as it is not for example in the perpendicular, this limitation is because I do the rotation of the z axis adding to the Roll the value of pi radians. I'm still looking for a way to rotate the Z automatically.

def rpy_euler_angles_to_vector_angle_axis_ur(roll, pitch, yaw):
  yawMatrix = np.matrix([
    [cos(yaw), -sin(yaw), 0],
    [sin(yaw), cos(yaw), 0],
    [0, 0, 1]
    ])

  pitchMatrix = np.matrix([
    [cos(pitch), 0, sin(pitch)],
    [0, 1, 0],
    [-sin(pitch), 0, cos(pitch)]
    ])

  rollMatrix = np.matrix([
    [1, 0, 0],
    [0, cos(roll), -sin(roll)],
    [0, sin(roll), cos(roll)]
    ])

  R = yawMatrix * pitchMatrix * rollMatrix

  theta = acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1) / 2)
  multi = 1 / (2 * sin(theta))

  rx = multi * (R[2, 1] - R[1, 2]) * theta
  ry = multi * (R[0, 2] - R[2, 0]) * theta
  rz = multi * (R[1, 0] - R[0, 1]) * theta

  return (rx, ry, rz)

Here I get the pose from the ar_tags and correct the orientation.

def callback(data):

    PoseMarkers = Ar_Tag_Markers()
    for i in range(len(data.markers)):
      tag_ = data.markers[i]
      PoseMarker = Ar_Tag_Marker() # Nao sei porque mas se nao ficar aqui definido so aparece a ultima tag
      PoseMarker.id = tag_.id
      PoseMarker.pose.header.stamp = rospy.Time.now()
      PoseMarker.pose.header.frame_id = tag_.pose.header.frame_id
      PoseMarker.pose.pose.position.x = tag_.pose.pose.position.x
      PoseMarker.pose.pose.position.y = tag_.pose.pose.position.y
      PoseMarker.pose.pose.position.z = tag_.pose.pose.position.z
      quat = [tag_.pose.pose.orientation.x, tag_.pose.pose.orientation.y, tag_.pose.pose.orientation.z, tag_.pose.pose.orientation.w]

      euler_ang = quat_to_ur_axis_angle(quat)
      euler_ang[0] = euler_ang[0]+pi
      angle_ur3 = rpy_euler_angles_to_vector_angle_axis_ur(euler_ang[0], euler_ang[1], euler_ang[2])
      PoseMarker.pose.pose.orientation.x = angle_ur3[0]
      PoseMarker.pose.pose.orientation.y = angle_ur3[1]
      PoseMarker.pose.pose.orientation.z = angle_ur3[2]
      PoseMarker.pose.pose.orientation.w = 0
      PoseMarkers.header.stamp = rospy.Time.now()
      PoseMarkers.header.frame_id = tag_.pose.header.frame_id
      PoseMarkers.markers.append(PoseMarker) 

    MarkersPub_.publish(PoseMarkers)

edit flag offensive delete link

Comments

thanks a lot. I will have to understand as you have made use of roll-pitch-yaw angles, whereas I used axis-angles. As, currently, I am not actively working on the arm and the solution but involved in other things. I will look at it later.

zahid990170 ( 2022-04-08 04:40:39 -0500 )edit

add a comment

UR5 how to convert axis angles to quaternions

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

UR5 how to convert axis angles to quaternions edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

UR5 how to convert axis angles to quaternions