I’m using Python with the rtde to control a UR10e robot to scan a sphere. I wrote a test script to move the robot to a specific point on a known sphere (the point is calculated using spherical coordinates relative to the sphere’s center). Below is my code.
However, I noticed an issue while running the program: the robot’s TCP doesn’t point towards the sphere’s center. Instead, it’s facing exactly the opposite direction—180 degrees away from the intended vector between the TCP and the sphere center. Does anyone know why this is happening? Could someone point out where I might have gone wrong?
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()