DoF - a Robotiq Community
Warning sign
The Dof Community was shut down in June 2023. This is a read-only archive.
If you have questions about Robotiq products please reach our support team.
JacobBom

What @matthewd92 suggest in terms of using "get_actual_joint_positions()" and definitely the "stopj(a)" is a very good approach. 

The following indexing applies to respectively joint vectors and tool vectors: 

Joint vector: 
q = [j0, j1, j2, j3, j4, j5]
base = q[0]
shoulder = q[1]
elbow = q[2]
wrist1 = q[3]
wrist2 = q[4]
wrist3 = q[5]
(All units radians)

Tool vector:
tcp = p[x, y, z, rx, ry, rz]
x = tcp[0]
y = tcp[1]
z = tcp[2]
rx = tcp[3]
ry = tcp[4]
rz = tcp[5]
(x, y, z = meters, rx, ry, rz = radians)

A program structure like this, can make the robot rotate Wrist 3 until a certain condition is met, e.g. here DI[0] = True.
Remember to check the IF-statement to check continuously. 
Immediately, when DI[0] is no longer False, the program will jump from the IF-clause to the ELSE-clause, and hence stop nicely with "stopj".
zoom

The "speedj" command is explained in the URScript manual. 
In this case, it gets the arguments: 
Joint speeds: qd = [0,0,0,0,0,d2r(-15)]    (d2r(deg) converts a degree value to radians, hence 15 degrees per second, counter clockwise)
Acceleration: qdd = 3.14 (180 deg/sec^2)
Timeout: 30 (seconds, hence it will automatically stop after a 30 second rotation)

Sampling the position can either be done using "get_actual_joint_positions()" for a joint vector, or "get_actual_tcp_pose()" for a Cartesian vector relative to the Base. 
Consider this like a vector from "Base to TCP". We are looking for a vector from "Feature to TCP". 
The feature is saved as "Base to Feature". 
The pose trans command is explained here: 
https://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/simple-vision-system-23871/

Hence, the "PosRelToPlane1" vector is calculated like: 
"Feature to Base" = pose_inv("Base to Feature")
"Feature to TCP" = pose_trans("Feature to Base", "Base to TCP")
Or as written: 
PosRelToPlane1 = pose_trans( pose_inv( Plane_1 ), actualTCP )