Send pose to Universal Robots in MoveIt

asked 2019-10-18 01:28:37 -0500

UR5e
5 ●4 ●6 ●9

updated 2019-10-20 05:51:29 -0500

I am working with a real UR5e robot and I can send a pose x,y,z,w to the robot. However, I need to send a pose x,y,z,rotx,roty,rotz as shown in the teach pendant. For example, for the robot shown pose values: 33.33,-704,219,-2.85,0.619,-2.917. xyz are in mm and rotations seems in radiants. However pose.position. seems to be working in meters. But, what about the rotations?

I tried to set the rotation values directly to pose.rotation.x/y/z in radiants, and also a quaternion as follows (this method throws a warning because is deprecated). But it doesn't seems to be the same pose.

What is the proper way to send this pose using the moveit group interface? Thank you.

  tf::Quaternion q;  
  q.setEulerZYX(rotz,roty,rotx);

  target_pose1.position.x = x;  
  target_pose1.position.y = y;  
  target_pose1.position.z = z;  
  target_pose1.orientation.x = q.x();  
  target_pose1.orientation.y = q.y();  
  target_pose1.orientation.z = q.z();
  target_pose1.orientation.w = q.w();

  move_group.setPoseTarget(target_pose1);

UPDATE:

I am using the RPY to Quaternion conversion as,

tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);

Compared with the current pose of the robot (getCurrentPose()), I have that position have the same values. However, for the current given RPY to convert, values are different for the orientation.

Quaternion pose orientation requested to ROS: -0.41x, 0.75y, -0.04z, 0.51w. Quaternion pose orientation obtained from current robot RPY from teach pendant (setRPY(2.57,-0.51,2.29)): 0.44x, 0.81y, -0.34z, 0.11w.

So, It doesn't fit at all. Any relationship with the real and the calculated quaternions that could help to find the error?

edit retag flag offensive close merge delete

Comments

Could you please add the relevant tags to your question next time? Your question is very specific to UR, but you had no universal_robot tag. I've added it.

gvdhoorn ( 2019-10-18 04:02:49 -0500 )edit

I intend to do the same, could you share the final solution for the conversion?

RB_Code ( 2022-04-06 05:07:04 -0500 )edit

add a comment

answered 2019-10-18 04:01:08 -0500

gvdhoorn
86574 ●283 ●1432 ●1054 http://cor.tudelft.nl/

updated 2019-10-20 05:55:31 -0500

UR uses angle-axis representation for the orientation part of poses.

From the UR script manual (version 3.11, August 26, 2019), section 1.3 Numbers, Variables and Types:

A pose is given as p[x,y,z,ax,ay,az], where x, y, z is the position of the TCP, and ax, ay, az is the orientation of the TCP, given in axis-angle notation.

That is not directly compatible with quaternions as used in geometry_msgs/Pose. As, as you already noticed, ROS uses metres for distances and radians for orientations (but those typically get encoded in Quaternions, for which the individual elements don't use radians). This is all standardised in REP 103: Standard Units of Measure and Coordinate Conventions.

So you'll have to either convert from axis-angle directly to the quaternion representation, or via Euler RPY as used by tf::Quaternion::setEulerZYX(..). At least for the orientation part.

As for the position: it would be straightforward to convert from metres to millimeters and back. It's a simple division or multiplication by 1e3.

What is the proper way to send this pose using the moveit group interface? Thank you.

If you're working "in ROS", then all you should use are metres and radians (converted to Quaternions when working with geometry_msgs/Pose or similar data structures).

If you're trying to use external data, I would recommend to always convert it to the ROS-standard metres and radians, and then start working with the data. This will make sure that everything is consistent.

And finally:

Send pose to Universal Robots in MoveIt

If/when you're using ROS + MoveIt, it would seem important to realise you're not actually sending anything "to Universal Robots". The whole of ROS is standardised on certain representations for poses and other geometric concepts.

Those get translated by drivers and other nodes which interface with the actual hardware (or other pieces of software). MoveIt does not know anything about Universal Robots, the fact that UR uses axis-angle and mms for positions. It doesn't have to.

So you're question ultimately would be answered by: you don't, as it's impossible. You can only specify poses in the representation used everywhere in ROS, then have MoveIt process that and finally tranform that representation into one that a UR controller can work with -- which is done by the driver node that you use.

edit flag offensive delete link

Comments

PS: I seem to remember there is a way to configure the TP such that it will use a different representation for the orientation of the EEF, but I can't check right now. You may want to see for yourself in the TP.

gvdhoorn ( 2019-10-18 04:01:58 -0500 )edit

Thank you for the answer. I made an update in the description. After converting the RPY to quaternion, it doesn't fit with the current retrieved quaternion pose. Any mistake in the conversion?

UR5e ( 2019-10-19 11:59:38 -0500 )edit

Your comment confuses me.

As I wrote: UR does not use RPY, but angle-axis. That is a different representation. You cannot plug in the values you see on the TP into setRPY(..). That will not work.

You'll have to either:

convert angle-axis to Euler angles
convert angle-axis to quaternion (perhaps like this)

Was that not clear from my answer?

gvdhoorn ( 2019-10-19 14:48:04 -0500 )edit

Thank you. UR TP shows the rotation vector as well as RPY. So, in the UPDATE, I am using the RPY values as an input of setRPY without success. I am going the to implement the conversion from axisAngle to quaternion as you mentioned.

UR5e ( 2019-10-20 05:26:00 -0500 )edit

Then it could be that UR uses a different axis/rotation ordering for their RPY display.

Seeing as there are many different valid orders of Eluer angles, that would not surprise me.

I also noticed this in the code you show:

target_pose1.orientation.w = 1.0;

you hard-code .w to 1. That may not be correct, depending on the values of the other fields. I'd just copy the .w from q.

gvdhoorn ( 2019-10-20 05:47:35 -0500 )edit

I am using q.w(), = 1 was when I tested only position. I alredy edit the question, thank you. Ok, I am testing the different orders of RPY.

UR5e ( 2019-10-20 05:57:06 -0500 )edit

add a comment

Send pose to Universal Robots in MoveIt

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Send pose to Universal Robots in MoveIt edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Send pose to Universal Robots in MoveIt