Posts: 152
Threads: 55
Joined: Jun 2022
Reputation:
0
04102023, 08:58 PM
(This post was last modified: 04132023, 07:34 AM by Albert.)
I have a workstation with an imported object, which consists of a surface, a curve, and points. Item.GetPoints() works great to get the position of each point. However, the rotations associated with the points are in IJK coordinates. How can I interpret IJK coordinates in a way that is valid outside of the curve object? Specifically, I would like to convert IJK coordinates to Euler angles.
The full process I eventually want to do is:
1. Import surface/curve/points to RoboDK workstation
2. Get point coordinates (6 DoF)
3. Adjust 6 DoF point coordinates to be normal to the surface ( PROJECTION_RECALC works great for this one)
4. Adjust 6 DoF point coordinates to be tangent to the curve
5. Export the adjusted 6 DoF coordinates for other use
For getting surface normals, my only lead right now is to use numpystl to parse my mesh and pull the nearest surface normal from there, but that seems pretty hard (and slow), so there's probably a better way.
For getting curve tangents, I have in the past calculated curve derivatives in each dimension to allow conversion from ijk to Euler angles, but it was painful and complicated and I don't want to do it again. Similarly to above, there must be an easier way that I am just not aware of.
Any pointers would be much appreciated.
Posts: 323
Threads: 2
Joined: Jun 2021
Reputation:
25
Take a look at the source code of the PointUtilities and CurveUtilities Addins:
https://github.com/RoboDK/PlugInInterf...oTarget.py
https://github.com/RoboDK/PlugInInterf...Targets.py
It shows how to convert XYZ+IJK to a pose. You can then convert the pose to another format, such as Euler angles:
https://robodk.com/doc/en/PythonAPI/robo...ose_2_KUKA
Posts: 152
Threads: 55
Joined: Jun 2022
Reputation:
0
This worked perfect, thanks Sam!
Posts: 152
Threads: 55
Joined: Jun 2022
Reputation:
0
Hi Sam,
I am now realizing that this does not address tangency. How can I make the X axis of the target tangent to the curve at that point?
(I could always do the math on this one, it just seems like an unnecessary hurdle.)
Posts: 323
Threads: 2
Joined: Jun 2021
Reputation:
25
IJK does not provide rotation along the axis.
The script I linked to use point_Zaxis_2_pose with a preferred Y direction.
https://robodk.com/doc/en/PythonAPI/robo...xis_2_pose
You can provide a fixed value for Y or calculate it dynamically by calculating a vector pointing to the next point (look ahead).
I know, it's not perfect. But if your curve step size is small enough, it should be fine.
I do not have a script ready for this, unfortunately.
Feel free to provide your implementation here, maybe we could add it as a feature for the Apps.
Posts: 152
Threads: 55
Joined: Jun 2022
Reputation:
0
This works on the paths I tested. I expect it may be unpredictable if surface normals vary considerably between adjacent waypoints. Please let me know if you put a version of this code into an API release so I can adopt the standard version.
Code: # Given: target_handles is a list of targets of type robolink.Item
# Given: Z axis of each target is normal to nearest surface facet
# Given: function definitions in separate code block, below
assert len(target_handles) >= 2, "MustHaveAtLeastTwoTargetsToCalculateTangency"
for i in range(len(target_handles)):
# the first target only: forward difference
if i == 0:
angle_about_z = checkRotationSign(forwardDifference(target_handles[i], target_handles[i+1]), target_handles[i], next_target=target_handles[i+1])
target_handles[i].setPose(target_handles[i].Pose() * robomath.rotz(angle_about_z))
# all middle targets: central difference is average of forwards and backwards difference
elif i < (len(target_handles)  1):
forward_rotation = checkRotationSign(forwardDifference(target_handles[i], target_handles[i+1]), target_handles[i], next_target=target_handles[i+1])
backward_rotation = checkRotationSign(backwardDifference(target_handles[i], target_handles[i+1]), target_handles[i], prev_target=target_handles[i1])
target_handles[i].setPose(target_handles[i].Pose() * robomath.rotz((forward_rotation + backward_rotation) /2))
# the final target only: backwards difference
else:
angle_about_z = checkRotationSign(backwardDifference(target_handles[i], target_handles[i1]), target_handles[i], prev_target=target_handles[i1])
target_handles[i].setPose(target_handles[i].Pose() * robomath.rotz(angle_about_z))
Code: # Forward and backwards difference take naming inspiration from discrete differentiation methods, but do not use the same underlying math.
# Method:
# 1. Get vector(s) from target to its neighbor(s)
# 2. Project vector(s) to surface plane
# 3. Calculate the rotational difference between the vector(s) and the current X axis (rotation is about Z)
# 4. Check result and flip sign of rotation or offset by 180deg, if necessary
# 5. Apply calculated rotation to target, averaging the forwards and backwards offsets, if both present
def forwardDifference(base_target: robolink.Item, next_target: robolink.Item) > float:
# calcs done in frame of base target
tangent_vector = np.asarray(pose_wrt(next_target, base_target)[0:3,3].tolist())
plane_normal = [0, 0, 1]
# The projection of vector u onto a plane is obtained by subtracting from u the component of u that is orthogonal to the plane
tangent_orthog = np.multiply(plane_normal, np.dot(tangent_vector, plane_normal))
tangent_vector = tangent_vector  tangent_orthog
angle_about_z = np.arccos(np.dot(tangent_vector, [1,0,0]) / np.linalg.norm(tangent_vector))
return float(angle_about_z)
def backwardDifference(base_target: robolink.Item, prev_target: robolink.Item) > float:
# calcs done in frame of base target
tangent_vector = np.asarray(pose_wrt(prev_target, base_target)[0:3,3].tolist())
plane_normal = [0, 0, 1]
# the projection of vector u onto a plane is obtained by subtracting from u the component of u that is orthogonal to the plane
tangent_orthog = np.multiply(plane_normal, np.dot(tangent_vector, plane_normal))
tangent_vector = tangent_vector  tangent_orthog
# a dot b = abcos(theta)
angle_about_z = np.arccos(np.dot(tangent_vector, [1,0,0]) / np.linalg.norm(tangent_vector))
return float(2*3.14  angle_about_z)
# Since cos() is an even function, it is possible to rotate in the wrong direction.
# Check that Y error reduces when rotating target. Otherwise, rotate the opposite direction.
# Additionally, since 180deg off looks good as far as error in Y, must separately check sign of X error
# to ensure that result is not 180deg off.
def checkRotationSign(angle_about_z, base_target, prev_target=None, next_target=None):
assert (prev_target == None) ^ (next_target == None), "MustCheckExactlyOneTargetRelativeToBaseTarget"
if prev_target != None:
target = prev_target
else:
target = next_target
original_error = pose_wrt(target, base_target)[1,3]
# flip target, check remaining error, then put back
base_target.setPose(base_target.Pose() * robomath.rotz(angle_about_z))
residual_error = pose_wrt(target, base_target)[0:3,3].tolist()
base_target.setPose(base_target.Pose() * robomath.rotz(angle_about_z))
# if we were in the right quadrant, but rotated the wrong direction, the the residual error in Y will not be near 0
if abs(residual_error[1]) > 0.9 * abs(original_error):
angle_about_z =  angle_about_z
base_target.setPose(base_target.Pose() * robomath.rotz(angle_about_z))
residual_error = pose_wrt(target, base_target)[0:3,3].tolist()
base_target.setPose(base_target.Pose() * robomath.rotz(angle_about_z))
# if X is aimed the wrong way, then residual error in X will be negative for forwards difference or positive for backwards difference
if (prev_target == None and residual_error[0] < 0) or (next_target == None and residual_error[0] > 0):
angle_about_z = angle_about_z + 3.14
return angle_about_z % (2*3.14)
# Builtin PoseWrt is unreliable when robot is present in workstation. Use hack until fixed.
def pose_wrt(child: robolink.Item, parent: robolink.Item) > robomath.Mat:
childPose = child.PoseAbs()
parentPose = parent.PoseAbs()
return parentPose.inv() * childPose
Posts: 152
Threads: 55
Joined: Jun 2022
Reputation:
0
For others who come here with the same issue: The code above has more than a few bugs that I found since I posted it. I may post an updated version when I am more confident in it.
