Relative turn movement around TCP coordinate system

[generalmidi]

My task is to align a object with a surface using a force torque sensor.

For that task it would be very helpful if I could write “my own movement” method in URScript.
I could achieve this with the movement in x-, respectively y- and z- axis.

But with the rotation I have difficulties due to the coordinate system, I couldn´t find a reference to base or tcp coordinate system in the function parameters.
Maybe I have to use a different function, I don´t know yet?

Here is my method to move the robot relative to the actual position by 100mm:

def move_rel_z(velocity):

xVal = 0.4
yVal = 0.4
zVal = 0.4
rx = 3.141
ry = 0
rz = 0
fromVal = 0.95
toVal = 1.95
a = 0.5
r = 0.0

movel(pose_add(get_forward_kin(), pose_sub(p[xVal, yVal, fromVal, rx, ry, rz], p[xVal, yVal, toVal, rx, ry, rz])), a=a, v=velocity, r=r)
end

I don´t exactly understand why there must be a “pose_add(…(get_forward_kin), pose_sub(…)))” but the important thing is that the difference between the two poses is only in the z-axis and has the value of 100mm.

When I try the same idea with the rotations the robots move “very strange”, a difference in the “rz”- value doesn´t create a rotation in the z- axis of the tcp.
When tcp and base are aligned in the z-axis the rotation should be the same, but somehow it isn´t.

Looking forward to your answer. Thanks so far.

The method is called in a URScript call by: move_rel_z(0.01)

Here´s a screenshot from the Polyscope MoveRelative method I “redesigned”:

[mbush]

Have you tried using the pose_tras() function? In that you give it the base coordinate that you want to transpose the new motion into. If you assign the current pose into a variable you would then transpose the new pose by transposing between the current pose and the relative new position that you want.

global var_1 = -100   #This is how far I want to move in the Z direction in mm
global var_2 = get_forward_kin()
global var_3 = p[var_2[0],var_2[1],var_2[2]+var_1/1000,var_2[3],var_2[4],var_2[5]])
movel(pase_trans(p[0,0,0,0,0,0],var_3), a=1.2, v=0.25) # The first pose in the pose_trans here is the frame of reference from which you are making the movel, in this case its the base coordinate system of the robot so 0,0,0,0,0,0

Or if you just want to have the robot assume the base frame of reference a simpler version is:

movel(pose_trans(var_3, a=1.2, v=0.25) or
movel(p[var_2[0],var_2[1],var_2[2]+var_1/1000,var_2[3],var_2[4],var_2[5]], a=1.2, v=0.25)

This results in the tool moving down 100mm from the current position. Is this what you are looking for?

Here is the program that I wrote and the results of the outputs:

And the motion associated with the program:

[generalmidi]

I tried your code example.
The robot moves downwards from the actual position by 100mm.

But instead of providing the base frame I wish the robot to move in the current tcp frame, like this:

movel(pose_trans( getMyTCPFrameSomehow(), var_3), a=1.2, v=0.25)

I tried to move the robot to Waypoint_2 and the use get_forward_kin(), but this doesnt seem to get it right:
movel(pose_trans( get_forward_kin(), var_3), a=1.2, v=0.25)
I also tried get_actual_tcp_pose():
movel(pose_trans( get_actual_tcp_pose(), var_3), a=1.2, v=0.25)

How did you create the *.gif animation ?

[jbm]

Using get_actual_tcp_pose() returns the current frame of the TCP, hence the actual position of the TCP.
Additionally, pose_trans() is probably the right function to use.
There is a small pose_trans()-example here.

[jbm]

When you have the F/T sensor, why not use Force Mode in Base frame, while applying force in -Z (downwards) and allowing freedrive in RX and RY.
I.e. force_mode(p[0,0,0,0,0,0], [0,0,1,1,1,0], [0, 0, -10, 0, 0, 0], ...
Thus, with 0 Nm in RX and RY, it is almost the same as freedriving in these two directions.

[generalmidi]

I tried using force_mode too.
But I like to “recognize a obstacle” or an surface with an object.

So force_mode doesnt know when it hits the surface it adjust the force and tries to keep the force value.
I was “playing” around with a time limit but this is not possible because I don´t know how long the detection process is.

I followed some of these examples:
http://www.zacobria.com/universal-robots-zacobria-forum-hints-tips-how-to/force-feedback-function/

[mbush]

I use licecap on Mac to capture gif files.

I guess I’m not sure I totally understand what you are trying to do. What point are you trying to start your move from? With what I did I only moved in the Z-axis 100mm per the following quote from your initial post:

“…but the important thing is that the difference between the two poses is only in the z-axis and has the value of 100mm.”

If you ware looking for the robot to move 100 mm in the Z-axis of the TCP then you would use the following commands

global var_2 = get_forward_kin()
global var_3 = p[0,0,var_2[2]-100/1000,0,0,0]
movel(pose_trans(get_forward_kin(), var_3), a=1.2, v=0.25)

If you are going to translate the pose you only need to fill in the value(s) of the new pose that you want to be different, so in this case you would only use the z value. This causes the robot to move like this

If you do what I originally posted the robot will still move in the Z-axis but relative to the base frame, not the tool frame and the motion looks like this, but I thought this was more along what you were looking for originally.

Let me know if you need some more help on this, its taken me quite a lot of playing around to really understand what is happening with this and how to get the robot to respond the way that you really want it to at times

Matt

[generalmidi]

Alright, now I got it.
I extended your example a bit and made some comments so other people can hopefully benefit from it too.
Thanks very much for spending time about thinking my task, I really learned something from you.

If you ware looking for the robot to move 100 mm in the Z-axis of the TCP then you would use the following commands...

Yes I was looking for that, so here is the example working:

Here´s the program:

Program
BeforeStart
‘Pose definition’
‘Translation 3 axis, Rotation 3 axis’
‘Pose variable consists of x,y,z, Rx, Ry, Rz’
myPoseVar≔get_actual_tcp_pose()
‘we need the value “pi” later’
pi≔3.1415

Robot Program
MoveL
goToStart
‘To better see the movement a wait is inserted here’
Wait: 0.5
‘Wrist moves and so is NOT aligned with base anymore’
wrist2_nickMove
Wait: 0.5
‘Now save tcp pose to variable’
myCurrentPose≔get_forward_kin()
‘“create” distance var- only Z of pose array is changed’
distance_Z_only≔p[0, 0, myCurrentPose[2] -.1, 0, 0, 0]
‘move command with pose_trans- does the trick’
movel(pose_trans(myCurrentPose, distance_Z_only), a=1.2, v=0.25)
Wait: 0.5
‘get tcp pose again- for better reading’
myCurrentPose≔get_forward_kin()
‘“create” rotation var- only Rz of pose array is changed’
‘rotation in radians- 3.14 is about 180degrees’
rotate_Rz_only≔p[0, 0, 0, 0, 0, myCurrentPose[5] - pi]
‘move command with pose_trans- does the trick again:-)’
movel(pose_trans(myCurrentPose, rotate_Rz_only), a=1.2, v=0.25)
Wait: 0.5
‘example move finished robot returns to start pose’

[mbush]

Glad you were able to get it working! BTW, you don’t need Pi, use the function d2r(), converts degrees to radians, r2d() converts radians to degrees.

Matt

[generalmidi]

Now I even found an easier way to do it, the speedl(…) command.
You can define a list of 6 speeds for x,y,z,Rx,Ry,Rz
To move in z- axis [0,0,0.25,0,0,0] and assign it to a variable like mySpeedList.
The parameter acceleration is also needed, e.g. acceleration = 0.1

Call the command like speedl(mySpeedList, acceleration) and the robot will move with constant speed in the z-axis.

[jbm]

You might also want to use the time parameter in the speedl()or speedj(), as the command end after the speed given is reached using the specified acceleration.

[mbush]

That is pretty interesting, I will need to try that myself. I may have an application for that currently.

Matt