How to Rotate UR Robot Joints to Align TCP with a Known Point

Technical Questions URScript
1

Hi guys,
I’m a beginner with UR robots, and I’m trying ti figure out how to rotate the robot’s joints so that the TCP aligns with a known point A (assuming the coordinates of point A are already given).

2

@Heng_Li It took me some time to fully grasp this concept, but I’ll do my best to explain it clearly. When you command the robot’s TCP (Tool Center Point) to move to a specific position, you are implicitly defining that position relative to an absolute reference frame—specifically, the robot’s base frame at (0, 0, 0, 0, 0, 0) in terms of (x, y, z, rx, ry, rz) coordinates. This base frame is aligned with the robot’s own coordinate system.

However, the position displayed on the Teach Pendant is typically relative to a reference feature (either the robot base or the tool’s TCP), and this feature’s frame may not always be aligned with the robot’s base frame (x, y, z, rx, ry, rz).

To address this, you can create a new feature, such as a custom point or plane, and align its (x, y, z, rx, ry, rz) coordinates to match your desired target position. This can be done through in the tech pendant MOVE tab in the FEATURE option. This way, the movement commands will be correctly interpreted relative to the reference feature you’ve defined.

3

@thantharate Thank you for your response. I’m currently using Python with the RTDE to control a UR10e robot for scanning a sphere. I’ve written a test script that moves the robot to a known point on the sphere, which I’ve calculated based on the spherical coordinates relative to the sphere’s center.

However, I faced an issue during testing: the robot’s TCP doesn’t point toward the sphere’s center. Instead, it’s oriented in the exact opposite direction (180 degrees off). Could you help me understand why this might be happening? Here is my code:

import rtde_control
import rtde_receive
import numpy as np
import math
from scipy.spatial.transform import Rotation as R

def initialize_robot(robot_ip):
    rtde_c = rtde_control.RTDEControlInterface(robot_ip)
    rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip)
    return rtde_c, rtde_r

def calculate_point_on_sphere(o_x, o_y, o_z, r, theta, phi):
    a_x = (r * np.sin(theta) * np.cos(phi) + o_x)
    a_y = (r * np.sin(theta) * np.sin(phi) + o_y)
    a_z = (r * np.cos(theta) + o_z)
    return a_x, a_y, a_z

def direction_vector(o_x, o_y, o_z, a_x, a_y, a_z):
    richtung_vektor = np.array([o_x - a_x, o_y - a_y, o_z - a_z])
    richtung_vektor /= np.linalg.norm(richtung_vektor)
    return richtung_vektor

def calculate_rotation(tcp_orientation, richtung_vektor):
    rotation_matrix = R.from_rotvec(tcp_orientation).as_matrix()
    tcp_direction_vector = rotation_matrix[:, 2]
    tcp_direction_vector /= np.linalg.norm(tcp_direction_vector)

    rotation_axis = np.cross(tcp_direction_vector, richtung_vektor)
    if np.dot(rotation_axis, richtung_vektor) < 0:
        rotation_axis = -rotation_axis
    rotation_axis /= np.linalg.norm(rotation_axis)
    cos_angle = np.dot(richtung_vektor, tcp_direction_vector)
    winkel = np.arccos(cos_angle)

    rotation_vector = winkel * rotation_axis
    rotation_matrix = R.from_rotvec(rotation_vector).as_matrix()

    euler_angles = R.from_matrix(rotation_matrix).as_euler('xyz', degrees=True)
    roll, pitch, yaw = np.deg2rad(euler_angles)
    return roll, pitch, yaw

def rpy_to_rotvec(roll, pitch, yaw):
    alpha, beta, gamma = map(np.deg2rad, [yaw, pitch, roll])

    ca, cb, cg = np.cos([alpha, beta, gamma])
    sa, sb, sg = np.sin([alpha, beta, gamma])

    r11 = ca * cb
    r12 = ca * sb * sg - sa * cg
    r13 = ca * sb * cg + sa * sg
    r21 = sa * cb
    r22 = sa * sb * sg + ca * cg
    r23 = sa * sb * cg - ca * sg
    r31 = -sb
    r32 = cb * sg
    r33 = cb * cg

    theta = math.acos((r11 + r22 + r33 - 1) / 2)
    sth = math.sin(theta)
    kx = (r32 - r23) / (2 * sth)
    ky = (r13 - r31) / (2 * sth)
    kz = (r21 - r12) / (2 * sth)
    return [theta * kx, theta * ky, theta * kz]

def move_robot_to_point(rtde_c, ziel_position, rotation_angles):
    r_x, r_y, r_z = rpy_to_rotvec(*rotation_angles)
    ziel_pose = (ziel_position[0], ziel_position[1], ziel_position[2], r_x, r_y, r_z)
    rtde_c.moveL(ziel_pose, speed=0.01, acceleration=0.01)

# example
robot_ip = "192.168.1.200"
o_x, o_y, o_z = 0, -0.4, 0.3
r = 0.1
phi = 0
theta = 0

a_x, a_y, a_z = calculate_point_on_sphere(o_x, o_y, o_z, r, phi, theta)
rtde_c, rtde_r = initialize_robot(robot_ip)
tcp_pose = rtde_r.getActualTCPPose()
tcp_orientation = tcp_pose[3:]
ziel_vector = direction_vector(o_x, o_y, o_z, a_x, a_y, a_z)
rotation_angles = calculate_rotation(tcp_orientation, ziel_vector)

move_robot_to_point(rtde_c, (a_x, a_y, a_z), rotation_angles)

rtde_r.disconnect()
rtde_c.disconnect()
4

@Heng_Li Sorry for the late response, I was on medical leave.
To be honest, I am not an expert on the subject, but you can try giving end positions with quaternions. As it works for me as a ROS node.
Something like this…

geometry_msgs::msg::Pose msg;

msg.orientation.x = 0.0;
msg.orientation.y = 0.0;
msg.orientation.z = 0.0;
msg.orientation.w = 0.0;
msg.position.x = 0.400;
msg.position.y = 0.400;
msg.position.z = 0.400;