How to rotate UR5e with respect to base frame

ukindler

Hi,

in our PC based UR robot application we would like to provide a control panel for manual robot control of an URe robot like you know it from the UR robot teach pendant. That means the user should be able to manual move the robot in tool space (with respect to tool) and in base space (wrt base). We are almost there. We use the servoj command to implement this kind of control. To do this, we manipulate the pose that is given in cartesian space and then convert this with the get_inverse_kin function into joint position that are passed to the servoj function (see the thread jog_thread(): function here: https://gitlab.com/gitlabuser0xFFFF/ur_rtde/-/blob/uk_dev/scripts/rtde_control.script).

A UR robot 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. The x, y and z coordinates are seen from the Base position whereas the Rotation (ax, ay, az) of the Tool head is seen from the tool head position itself. To move with respect to the base frame, we can simply add a value to the x, y, z coordinate of a pose. To move with respect to the tool, we use the pose_trans function like it is written here (https://www.universal-robots.com/articles/ur/urscript-move-with-respect-to-a-custom-featureframe/). To rotate with respect to the tool, we can simply add an angle to the ax, ay and az part of the pose. This all works perfectly fine. But in the teach pendant it is also possible, to rotate with respect to the base frame. And this is something that we do not know how to implements this. That means, we do not know, how to translate a pose rotation wrt to the tool into a rotation wrt to the base frame.

Is there something like the pose_trans function to translate a rotation wrt to tool into a rotation wrt to base? Or does someone know the math how to do this?  Or does someone know, how this functionality is implemented in the teach pendant.

Thanky in advance for any help.

Uwe

jelms

Well I repeated the same mistake and misunderstood.

Third time is the charm?

p1=p[0,0.5,0.5,2.22,0,0]
movel(p1)
textmsg("p1 ", p1)
pr=p[0,0,0,0,0,-1.57/2]
p2=pose_add(p[p1[0],p1[1],p1[2],0,0,0], pose_trans(pr, p[0,0,0,p1[3],p1[4],p1[5]]))
textmsg("p2 ", p2)
movel(p2)

p1=p2
p2=pose_add(p[p1[0],p1[1],p1[2],0,0,0], pose_trans(pr, p[0,0,0,p1[3],p1[4],p1[5]]))
textmsg("p2 ", p2)
movel(p2)

p1=p2
p2=pose_add(p[p1[0],p1[1],p1[2],0,0,0], pose_trans(pr, p[0,0,0,p1[3],p1[4],p1[5]]))
textmsg("p2 ", p2)
movel(p2)


p1=p2
p2=pose_add(p[p1[0],p1[1],p1[2],0,0,0], pose_trans(pr, p[0,0,0,p1[3],p1[4],p1[5]]))
textmsg("p2 ", p2)
movel(p2)

ukindler

@jelms Wow, awesome - I think this is the solution. I just tested it and it works like the manual jogging. That is really great and you helped me a lot - to solve the problem and to learn more about pose_trans, pose_add and the math. Thank you very much for your kind help  

@matthewd92 Thank you for testing this and for your time to help me.

You guys are great !!!

jelms

Hi Uwe,

You should be able to use pose_trans to accomplish this. A pose p[x,y,z,ax,ay,az] describes the TCP completely in base coordinates. Your statement makes me believe this is a misunderstanding.

whereas the Rotation (ax, ay, az) of the Tool head is seen from the tool head position itself

If you have a pose and wish to rotate it about the base Z axis you can compose the poses. Composing axis-angles is not easy for me to reason about but have a lot of useful features.

I picked a somewhat arbitrary start position and then, this will rotate by 90 degrees around the base z-axis. I commented out the moves, but poses will be logged on the log tab.

p1=p[0,0.5,0.5,2.22,-2.22,0]
#movej(p1)
textmsg("p1 ", p1)
pr=p[0,0,0,0,0,1.57]
p2=pose_trans(pr, p1)
textmsg("p2 ", p2)
#movej(p2)

Hope that helps.

ukindler

Hi,

thank you very much for your response, but I unfortunately don't think that this is the right solution. The output of your code above is:

p1  [0, 0.5, 0.5, 2.22, -2.22, 0]
p2  [-0.49999985098838806, 0.0003981371410191059, 0.5, 3.1401495933532715, -0.001250210334546864, 0.0022622691467404366]

In p2 the X, Y and Z positions changed and this should not happen. If you open the teach pendant and then manually rotate in base coordinate system, then you can see, that X, Y and Z remain constant. If you manually rotate in the teach pendant, then it rotates around the TCP but the X, Y and Z positions do not change.

Kind regards,

Uwe

matthewd92

We have not figured it out yet ourselves. But based on the idea above have one idea. 

If you created a new point that had the x,y,z of the tool pose but 0 for RX,Ry,Rz and then did a pose trans around that point with your desired rotation that should work correctly. That point would represent a point that was taught with the tool axes aligned with the base axes and so even though rotation is occurring around the point it should be the same as if it were rotating around the base. 

I’d be curious to know if this works, it does in my head if I am understanding what you want to do correctly. 

jelms

@ukindler Ahh I misunderstood. It's actually just switching the parameters of `pose_trans`

p1=p[0,0.5,0.5,2.22,-2.22,0]
#movej(p1)textmsg("p1 ", p1)
pr=p[0,0,0,0,0,1.57]
p2=pose_trans(p1, pr) # params switched from previous example
textmsg("p2 ", p2)
#movej(p2)

p1  [0,0.5,0.5,2.22,-2.22,0]

p2  [0,0.5,0.5,0.00125029,-3.14015,0.00226231 ]

matthewd92

I believe that still just rotates around the z axis of p1, since it’s aligned with the base it works but if you rotated the tool to be not aligned in the z axes I believe that will not work, at least as I understand the use case. The frame of reference for the pose_trans is still P1

matthewd92

Here's a quick GIF that I put together showing the tool not aligned in the Z axis of the base frame, you can see using a pose_trans with p1 that only wrist 3 rotates since the pose_trans (or pose_add) is rotating in the tool frame of reference.  If you put the arm into the p1 position on the move screen and then in base frame use the +RZ button you will see that more than just wrist 3 rotates.  That is the use case that I would like to solve for or understand how UR does that on the move screen.  @ukindler assume that what I captured in the GIF is not the behavior you are looking for either correct?

matthewd92

@jelms that is awesome, thanks for that.  I had tried something similar last night but forgot the pose_trans as an argument.  I have tested in the simulator and that appears to do exactly what we have been looking for.  I wrote a small script function that would make this easier to use in an actual program, it works like a pose_add but adds the rotational element in the base frame.