# How do I calculate position and rotation?

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.

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.

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

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