Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5

CONVERTING IJK to EULER ANGLES

#1
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 numpy-stl 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.
#2
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:
https://robodk.com/doc/en/PythonAPI/robo...ose_2_KUKA
Please read the Forum Guidelines before posting!
Find useful information about RoboDK by visiting our Online Documentation.
#3
This worked perfect, thanks Sam!
#4
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.)
#5
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.
Please read the Forum Guidelines before posting!
Find useful information about RoboDK by visiting our Online Documentation.
#6
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[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
   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)

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




Users browsing this thread:
1 Guest(s)