How do I calculate position and rotation?

General Application Help: How do I?
1

I have a UR5e. I’m trying to figure out how to calculate the robot orientation using CAD. In SolidWorks, I have an assembly of my part in a gripper, sitting in a pallet for pick up and drop off. The part is omitted from the enclosed screenshot. I tried making a Python script to calculate the angles (Rotation Vector, and RPY) using two vectors from the TCP coordinate system. Tried doing it in both radians and degrees. The angle is always way off whether I’m inputting say the TCP’s X vector , Y-vector, or Z-vector as primary, or trying different permutations of using minus vectors of those vectors. There must be an easy way to do this.

image
image1648×1027 128 KB

2

Here’s my Python Script for reference.

import numpy as np

def vectors_to_euler_angles(vector1, vector2):
    # Normalize the input vectors
    vector1 = vector1 / np.linalg.norm(vector1)
    vector2 = vector2 / np.linalg.norm(vector2)

    # Calculate the dot and cross products.
    dot_product = np.dot(vector1, vector2)
    cross_product = np.cross(vector1, vector2)

    # Calculate the angle between the two vectors.
    angle_rad = np.arccos(dot_product)

    # Calculate the axis of rotation (unit vector).
    axis = cross_product / np.linalg.norm(cross_product)

    # Convert the axis and angle to Euler angles in radians.
    euler_angles_rad = axis * angle_rad

    return euler_angles_rad

if __name__ == "__main__":

    # Tool Vector in Robot/CMM Vice coordinates.
    # ------------------------------------------

    toolXVector = np.array([-0.7625, -0.4454, -0.4694])
    toolXVector = toolXVector / np.linalg.norm(toolXVector)

    toolYVector = np.array([-0.4339, 0.8901, -0.1397])
    toolYVector = toolYVector / np.linalg.norm(toolYVector)

    toolZVector = np.cross(toolXVector, toolYVector)
    #toolZVector = np.array([0.48, 0.0971, -0.8719])
    toolZVector = toolZVector / np.linalg.norm(toolZVector)



    # Robot Orientation Vectors. Intended to make orienting the robot more intuitive.
    # -------------------------------------------------------------------------------

    # Pointing Vector for Gripper. Should control direction of last joint of Robot. In practice, it should be pointing towards the object to picked up/dropped off.
    pointingVector = toolZVector
    pointingVector = pointingVector / np.linalg.norm(pointingVector)

    # Thumb Vector for Gripper. Used to control rotation about last joint of robot.
    thumbVector = toolXVector
    thumbVector = thumbVector / np.linalg.norm(thumbVector)

    tertiaryVector = np.cross(pointingVector, thumbVector)
    tertiaryVector = tertiaryVector / np.linalg.norm(tertiaryVector)

    # Vectors for use with converter function.
    # ----------------------------------------

    # FIXME: Not sure what vectors to use to control robot orientation. Each combination seems to be way off (about 90 degrees).
    vector1 = -pointingVector
    vector2 = thumbVector

    euler_angles_rad = vectors_to_euler_angles(vector1, vector2)
    print("Euler angles (radians): ", euler_angles_rad)
3

Think I might have figured out a better, simpler solution. Will provide an update after testing.

The previous Python script is junk.