I have a robot on a 2.5d surface. i assume that robot doesn't flip, so gimbal lock is not a problem. For a robot, I know: linear velocity (local frame) angular velocity (local frame) the two above come from teleop command. I also know roll, pitch and yaw in a global frame.
Question: how to get global angular velocity (wx, wy, wz)?
Please help.
P.S. Just in case: this question is a part of an attempt to calculate robot's next position from the current one and teleop command, it will be used by UKF for localization. Here is the code, and the line i am struggling with is
wGlobal = transform_angular_velocity(wLocal, [roll, pitch, yaw])
If by any chance you can suggest a better way of doing it, it would be even better. Here is the code itself:
def getNextState(self, x, dt, u):
# Unpack the state
xPos, yPos, zPos, roll, pitch, yaw, vx, vy, vz, wxLocal, wyLocal, wzLocal, ax, ay, az, slip_linear, slip_angular = x
xPosPrev, yPosPrev, zPosPrev = xPos, yPos, zPos
# Control inputs, local frame
vel_linear = u[0]
vel_angular = u[1]
# No slippage. Only used to draw "perfect" trajectory.
if(self.nSlip == 0):
pass
# Slippage calculated by Kalman filter
elif(self.nSlip == 1):
vel_linear = vel_linear * slip_linear
vel_angular = vel_angular * slip_angular
# Simulate slippage in entire area
elif(self.nSlip == 2):
vel_linear *= 0.5
vel_angular *= 0.2
# Simulate slippage in selected rectangles only
elif(self.nSlip == 3):
for rect in self.arrSlippers:
if((rect[0] <= x[0] <= rect[0] + rect[2]) and (rect[1] <= x[1] <= rect[1] + rect[3])):
vel_linear *= 0.5
vel_angular *= 0.2
break
# ---
# wLocal = [wxLocal, wyLocal, vel_angular]
wLocal = [0, 0, vel_angular]
#wGlobal = transform(wLocal, [roll, pitch, yaw], LOCAL_TO_GLOBAL)
wGlobal = transform_angular_velocity(wLocal, [roll, pitch, yaw])
# Update yaw (local yaw)
rpy_new = normalize_angle([
roll + wGlobal[0] * dt,
pitch + wGlobal[1] * dt,
yaw + wGlobal[2] * dt
])
# ---
vLocal = np.array([vel_linear, 0, 0])
vGlobal = transform(vLocal, rpy_new, LOCAL_TO_GLOBAL)
#vGlobal = transform(vLocal, [roll, pitch, yaw], False)
vx_new, vy_new, vz_new = vGlobal
# Calculate accelerations
ax = (vx_new - vx) / dt
ay = (vy_new - vy) / dt
az = (vz_new - vz) / dt
xPos += vx_new * dt
yPos += vy_new * dt
#zPos += vz_new * dt
zPos = self.surface_func(xPos, yPos)
# if(pos_new != prev_pos):
if(self.bUseGetRPY):
pos_new = [xPos, yPos]
prev_pos = [xPosPrev, yPosPrev]
rpy_new = self.getRPY(prev_pos, pos_new, yaw, vel_angular * dt)
roll_new, pitch_new, yaw_new = rpy_new
# Calculate angular velocities (approximation: finite difference)
wxGlobal = (roll_new - roll) / dt
wyGlobal = (pitch_new - pitch) / dt
wzGlobal = (yaw_new - yaw) / dt
wxLocal, wyLocal, wzLocal = transform([wxGlobal, wyGlobal, wzGlobal], rpy_new, GLOBAL_TO_LOCAL)
# Prepare and return the new state vector
return np.array([xPos, yPos, zPos, roll_new, pitch_new, yaw_new, vx_new, vy_new, vz_new,
wxLocal, wyLocal, wzLocal, ax, ay, az, slip_linear, slip_angular])
def transform_angular_velocity(w, rpy, nGlobalToLocal):
roll, pitch, yaw = rpy
# Create the transformation matrix
T = np.array([
[1, 0, -np.sin(pitch)],
[0, np.cos(roll), np.cos(pitch) * np.sin(roll)],
[0, -np.sin(roll), np.cos(pitch) * np.cos(roll)]
])
if(nGlobalToLocal == LOCAL_TO_GLOBAL):
# Local to global: apply inverse of T
w_transformed = np.linalg.inv(T) @ w
elif(nGlobalToLocal == GLOBAL_TO_LOCAL):
# Global to local: apply T directly
w_transformed = T @ w
else:
raise ValueError("Direction must be 'local_to_global' or 'global_to_local'")
return w_transformed
def euler_to_rotation_matrix(roll, pitch, yaw):
"""
Create the rotation matrix for transforming angular velocities between the local and global frames
based on the robot's roll, pitch, and yaw angles.
"""
# Rotation matrix for yaw (around z-axis)
R_yaw = np.array([
[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]
])
# Rotation matrix for pitch (around y-axis)
R_pitch = np.array([
[np.cos(pitch), 0, np.sin(pitch)],
[0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]
])
# Rotation matrix for roll (around x-axis)
R_roll = np.array([
[1, 0, 0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]
])
# Combined rotation matrix: R = R_yaw * R_pitch * R_roll
R = R_yaw @ R_pitch @ R_roll
return R
def get_rotation_matrix(rpy):
roll, pitch, yaw = rpy
# Create a rotation matrix from roll, pitch, yaw using tf_transformations
return tf_transformations.euler_matrix(roll, pitch, yaw, 'rxyz')[:3, :3]
#return tf_transformations.euler_matrix(roll, pitch, yaw)[:3, :3]
# ---
def global_to_local(global_vec, rpy):
# Get the rotation matrix for the robot's orientation (RPY)
rotation_matrix = get_rotation_matrix(rpy)
#local_vec = np.matmul(rotation_matrix.T, global_vec)
local_vec = np.matmul(np.linalg.inv(rotation_matrix), global_vec)
return local_vec
# ---
def local_to_global(local_vec, rpy):
# Get the rotation matrix for the robot's orientation (RPY)
rotation_matrix = get_rotation_matrix(rpy)
# Apply the rotation matrix to the angular velocity
global_vec = np.dot(rotation_matrix, local_vec)
return global_vec
# ---
def transform(arrVector, arrRobotOrientation, nGlobalToLocal):
if nGlobalToLocal:
return global_to_local(arrVector, arrRobotOrientation)
else:
return local_to_global(arrVector, arrRobotOrientation)
def getRPY(self, prev_pos, pos, yaw_prev, deltaYawLocal):
x, y = pos
x_prev, y_prev = prev_pos
step = 0.01
yaw = yaw_prev + deltaYawLocal
# If there is movement, proceed with the regular calculation
if x == x_prev and y == y_prev:
delta_x = step * np.cos(yaw)
delta_y = step * np.sin(yaw)
# Update x, y with the small movement
x = x_prev + delta_x
y = y_prev + delta_y
# Calculate surface gradients (partial derivatives of surface function)
dz_dx = (self.surface_func(x + step, y) - self.surface_func(x - step, y)) / (2 * step)
dz_dy = (self.surface_func(x, y + step) - self.surface_func(x, y - step)) / (2 * step)
# Calculate movement vector
move_vector = np.array([x - x_prev, y - y_prev])
# # Calculate yaw from movement vector
# #yaw = np.arctan2(move_vector[1], move_vector[0])
# yaw = yaw_prev + deltaYawLocal
# Calculate the movement direction in the x-y plane
move_dir = move_vector / np.linalg.norm(move_vector)
# Calculate pitch (angle between the surface normal and the horizontal plane)
pitch = -np.arctan2(dz_dx * move_dir[0] + dz_dy * move_dir[1], 1)
# Calculate roll (tilt perpendicular to the movement direction)
move_perp = np.array([-move_dir[1], move_dir[0]])
roll = -np.arctan2(dz_dx * move_perp[0] + dz_dy * move_perp[1], 1)
return np.array([roll, pitch, yaw])
What I do: I have a map and robot's previous and current positions. So I can calculate RPY from that.
