[sig.johnnson]

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_RECALCworks 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 usenumpy-stlto 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.

[Sam]

Take a look at the source code of the PointUtilities and CurveUtilities Add-ins:
https://github.com/RoboDK/Plug-In-Interf...oTarget.py
https://github.com/RoboDK/Plug-In-Interf...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:
//www.sinclairbody.com/doc/en/PythonAPI/robo...ose_2_KUKA

[sig.johnnson]

This worked perfect, thanks Sam!

[sig.johnnson]

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.)

[Sam]

IJK does not provide rotation along the axis.

The script I linked to use point_Zaxis_2_pose with a preferred Y direction.
//www.sinclairbody.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.

[sig.johnnson]

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我< (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[i-1])
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[i-1]), target_handles[i], prev_target=target_handles[i-1])
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 = |a||b|cos(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
如果(prev_target = =没有和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)

# Built-in 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

[sig.johnnson]

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.