ZhengLiu-cart/Gripper_UMI
2
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update the vel calculation.

Browse Source 
the integration should only executed in world frame, not in imu body frame
This commit is contained in:
LiuzhengSJ
2026-07-03 14:40:29 +01:00
parent 421684855c
commit 70fa3002a4
2 changed files with 243 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
get_vel.py Normal file
View File

@ -0,0 +1,75 @@
import math
G = 9.80665
def mat_vec_mul(R, v):
return [
R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2],
R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2],
R[2][0] * v[0] + R[2][1] * v[1] + R[2][2] * v[2],
]
def mat_transpose(R):
return [
[R[0][0], R[1][0], R[2][0]],
[R[0][1], R[1][1], R[2][1]],
[R[0][2], R[1][2], R[2][2]],
]
def rotation_matrix_body_to_world(rpy_deg):
"""
rpy_deg = [roll, pitch, yaw], degree
Convention:
roll around X
pitch around Y
yaw around Z
R_body_to_world = Rz(yaw) @ Ry(pitch) @ Rx(roll)
"""
roll = math.radians(rpy_deg[0])
pitch = math.radians(rpy_deg[1])
yaw = math.radians(rpy_deg[2])
cr, sr = math.cos(roll), math.sin(roll)
cp, sp = math.cos(pitch), math.sin(pitch)
cy, sy = math.cos(yaw), math.sin(yaw)
return [
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
[-sp, cp * sr, cp * cr],
]
#####################################################################
dt = 0.02
rpy_deg = [10, 20, 30]
linear_acc_ms2 = [3.2, 5.5 , 7.7] # linear acc in imu body frame
vel_world = [0.0, 0.0, 0.0] # linear vel in world frame
# get direction cosine matrix
R_bw = rotation_matrix_body_to_world(rpy_deg)
R_wb = mat_transpose(R_bw)
# Convert acceleration body -> world
acc_world_mps2 = mat_vec_mul(R_bw, linear_acc_ms2)
# Integrate velocity in world frame
vel_world[0] += acc_world_mps2[0] * dt
vel_world[1] += acc_world_mps2[1] * dt
vel_world[2] += acc_world_mps2[2] * dt
# Convert velocity world -> current body
vel_body = mat_vec_mul(R_wb, vel_world)

327
vel_esti.py
View File

@ -1,12 +1,31 @@
'''
calculate the velocity of the imu.
The measured acc is in imu body frame.
1. acc offset along each axis calculation.
- gravity mapped to imu body frame, according to rpy;
- subtract gravity items to get the pure acc along each axis;
- keep imu static, average the pure acc values, as the offset;
- calibrate the offset in following use.
2. vel calculation.
- get calibrated acc in imu body frame;
- convert this calibrate acc to world frame;
- integtate this acc, get the vel in world frame;
- convert the vel back to imu body frame.
'''
import math import math
from typing import List, Tuple
G = 9.80665 # m/s^2 G = 9.80665
def mat_vec_mul(R: List[List[float]], v: List[float]) -> List[float]: def mat_vec_mul(R, v):
"""3x3 matrix times 3x1 vector."""
return [ return [
R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2], R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2],
R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2], R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2],
@ -14,8 +33,7 @@ def mat_vec_mul(R: List[List[float]], v: List[float]) -> List[float]:
] ]
def transpose(R: List[List[float]]) -> List[List[float]]: def mat_transpose(R):
"""Transpose of 3x3 matrix."""
return [ return [
[R[0][0], R[1][0], R[2][0]], [R[0][0], R[1][0], R[2][0]],
[R[0][1], R[1][1], R[2][1]], [R[0][1], R[1][1], R[2][1]],
@ -23,18 +41,15 @@ def transpose(R: List[List[float]]) -> List[List[float]]:
] ]
def rotation_matrix_body_to_world(rpy_deg: List[float]) -> List[List[float]]: def rotation_matrix_body_to_world(rpy_deg):
""" """
Convert roll, pitch, yaw to rotation matrix. rpy_deg = [roll, pitch, yaw], degree
rpy_deg = [roll, pitch, yaw], unit degree
Convention: Convention:
roll : rotation around X axis roll around X
pitch : rotation around Y axis pitch around Y
yaw : rotation around Z axis yaw around Z
Rotation order:
R_body_to_world = Rz(yaw) @ Ry(pitch) @ Rx(roll) R_body_to_world = Rz(yaw) @ Ry(pitch) @ Rx(roll)
""" """
@ -42,195 +57,187 @@ def rotation_matrix_body_to_world(rpy_deg: List[float]) -> List[List[float]]:
pitch = math.radians(rpy_deg[1]) pitch = math.radians(rpy_deg[1])
yaw = math.radians(rpy_deg[2]) yaw = math.radians(rpy_deg[2])
cr = math.cos(roll) cr, sr = math.cos(roll), math.sin(roll)
sr = math.sin(roll) cp, sp = math.cos(pitch), math.sin(pitch)
cy, sy = math.cos(yaw), math.sin(yaw)
cp = math.cos(pitch)
sp = math.sin(pitch)
cy = math.cos(yaw)
sy = math.sin(yaw)
# R = Rz(yaw) @ Ry(pitch) @ Rx(roll)
R = [
[
cy * cp,
cy * sp * sr - sy * cr,
cy * sp * cr + sy * sr,
],
[
sy * cp,
sy * sp * sr + cy * cr,
sy * sp * cr - cy * sr,
],
[
-sp,
cp * sr,
cp * cr,
],
]
return R
def gravity_body_from_rpy(rpy_deg: List[float]) -> List[float]:
"""
Calculate gravity vector projected onto body frame.
Unit: g
This matches your original formula:
g_x = -sin(pitch)
g_y = cos(pitch) * sin(roll)
g_z = cos(pitch) * cos(roll)
Yaw does not affect gravity projection.
"""
roll = math.radians(rpy_deg[0])
pitch = math.radians(rpy_deg[1])
g_x = -math.sin(pitch)
g_y = math.cos(pitch) * math.sin(roll)
g_z = math.cos(pitch) * math.cos(roll)
return [g_x, g_y, g_z]
def remove_gravity_in_body(acc_body_g: List[float], rpy_deg: List[float]) -> List[float]:
"""
Remove gravity directly in body frame.
This is equivalent to your original acc_calib().
Unit input : g
Unit output: g
"""
g_body = gravity_body_from_rpy(rpy_deg)
return [ return [
acc_body_g[0] - g_body[0], [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
acc_body_g[1] - g_body[1], [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
acc_body_g[2] - g_body[2], [-sp, cp * sr, cp * cr],
] ]
class IMUVelocityEstimator: class IMUVelocityEstimator:
"""
Estimate velocity from IMU acceleration.
Internal velocity state is stored in world frame:
self.v_world = [vx, vy, vz], unit m/s
Each update returns:
a_world_linear_mps2 : gravity-compensated linear acceleration in world frame
v_world : velocity in world frame
v_body : velocity projected onto current IMU/body XYZ axes
"""
def __init__(self): def __init__(self):
self.v_world = [0.0, 0.0, 0.0] # Velocity is stored in world frame, unit m/s
self.vel_world = [0.0, 0.0, 0.0]
def reset_velocity(self, v_world: List[float] = None): # Acceleration offset in body frame, unit g
if v_world is None: self.acc_offset_body = [0.0, 0.0, 0.0]
self.v_world = [0.0, 0.0, 0.0]
else:
self.v_world = list(v_world)
def update( def reset_velocity(self):
self, self.vel_world = [0.0, 0.0, 0.0]
acc_body_g: List[float],
rpy_deg: List[float], def set_acc_offset_body(self, acc_offset_body):
dt: float, self.acc_offset_body = list(acc_offset_body)
) -> Tuple[List[float], List[float], List[float]]:
def update(self, acc_body_g, rpy_deg, dt):
""" """
One IMU update step.
Parameters Parameters
---------- ----------
acc_body_g: acc_body_g:
IMU measured acceleration in current body frame. measured acceleration in IMU/body frame, unit g
Unit: g
Example: [Ax, Ay, Az]
rpy_deg: rpy_deg:
IMU attitude angle. [roll, pitch, yaw], unit degree
Unit: degree
Format: [roll, pitch, yaw]
dt: dt:
Sampling interval. time step, unit second
Unit: second
Returns Returns
------- -------
a_world_linear_mps2: acc_world_linear:
Linear acceleration in world frame after gravity removal. gravity-compensated acceleration in world frame, unit m/s^2
Unit: m/s^2
v_world: vel_world:
Integrated velocity in world frame. velocity in world frame, unit m/s
Unit: m/s
v_body: vel_body:
Current velocity projected onto current IMU/body XYZ axes. velocity projected onto current IMU/body frame, unit m/s
Unit: m/s
""" """
# 1. Rotation matrix: body frame -> world frame
R_bw = rotation_matrix_body_to_world(rpy_deg) R_bw = rotation_matrix_body_to_world(rpy_deg)
R_wb = mat_transpose(R_bw)
# 2. Convert acceleration from g to m/s^2 # 1. Remove body-frame acceleration offset
acc_body_mps2 = [ acc_body_corrected_g = [
acc_body_g[0] * G, acc_body_g[0] - self.acc_offset_body[0],
acc_body_g[1] * G, acc_body_g[1] - self.acc_offset_body[1],
acc_body_g[2] * G, acc_body_g[2] - self.acc_offset_body[2],
] ]
# 3. Transform acceleration from body frame to world frame # 2. Convert body acceleration from g to m/s^2
acc_body_mps2 = [
acc_body_corrected_g[0] * G,
acc_body_corrected_g[1] * G,
acc_body_corrected_g[2] * G,
]
# 3. Convert acceleration body -> world
acc_world_mps2 = mat_vec_mul(R_bw, acc_body_mps2) acc_world_mps2 = mat_vec_mul(R_bw, acc_body_mps2)
# 4. Remove gravity in world frame # 4. Subtract gravity in world frame
# # World Z is upward, and static flat IMU gives +1g on Z.
# Since your IMU flat on table gives Az ≈ +1g, acc_world_linear = [
# and world Z is upward, gravity component in this convention is:
#
# g_world = [0, 0, +G]
#
# Therefore linear acceleration is:
#
# a_linear_world = acc_world - g_world
#
a_world_linear_mps2 = [
acc_world_mps2[0], acc_world_mps2[0],
acc_world_mps2[1], acc_world_mps2[1],
acc_world_mps2[2] - G, acc_world_mps2[2] - G,
] ]
# 5. Integrate velocity in world frame # 5. Integrate velocity in world frame
self.v_world[0] += a_world_linear_mps2[0] * dt self.vel_world[0] += acc_world_linear[0] * dt
self.v_world[1] += a_world_linear_mps2[1] * dt self.vel_world[1] += acc_world_linear[1] * dt
self.v_world[2] += a_world_linear_mps2[2] * dt self.vel_world[2] += acc_world_linear[2] * dt
# 6. Transform world velocity back to current body frame # 6. Convert velocity world -> current body
R_wb = transpose(R_bw) vel_body = mat_vec_mul(R_wb, self.vel_world)
v_body = mat_vec_mul(R_wb, self.v_world)
return a_world_linear_mps2, list(self.v_world), v_body return acc_world_linear, list(self.vel_world), vel_body
def calibrate_acc_offset(self, samples):
"""
Calibrate acceleration offset.
Parameters
----------
samples:
list of samples.
Each sample should be:
(acc_body_g, rpy_deg)
acc_body_g: [Ax, Ay, Az], unit g
rpy_deg: [roll, pitch, yaw], degree
Principle
---------
During static calibration:
measured_acc_body = gravity_body + offset_body
So:
offset_body = measured_acc_body - gravity_body
We average offset_body over all samples.
Returns
-------
acc_offset_body:
acceleration offset in body frame, unit g
"""
if len(samples) == 0:
raise ValueError("samples is empty.")
offset_sum = [0.0, 0.0, 0.0]
for acc_body_g, rpy_deg in samples:
R_bw = rotation_matrix_body_to_world(rpy_deg)
R_wb = mat_transpose(R_bw)
# Gravity in world frame.
# Unit g.
gravity_world_g = [0.0, 0.0, 1.0]
# Convert gravity world -> body
gravity_body_g = mat_vec_mul(R_wb, gravity_world_g)
# Offset = measured acceleration - expected gravity
offset_body_g = [
acc_body_g[0] - gravity_body_g[0],
acc_body_g[1] - gravity_body_g[1],
acc_body_g[2] - gravity_body_g[2],
]
offset_sum[0] += offset_body_g[0]
offset_sum[1] += offset_body_g[1]
offset_sum[2] += offset_body_g[2]
n = len(samples)
self.acc_offset_body = [
offset_sum[0] / n,
offset_sum[1] / n,
offset_sum[2] / n,
]
# Calibration means current velocity should be zero
self.reset_velocity()
return list(self.acc_offset_body)
if __name__ == "__main__": if __name__ == "__main__":
estimator = IMUVelocityEstimator() estimator = IMUVelocityEstimator()
acc = [0.2, 0.73, 0.66] # unit g # Example calibration samples.
rpy = [46, -10, -8] # [roll, pitch, yaw], unit degree # In real use, collect these while the IMU is static.
dt = 0.01 # 100 Hz sample rate calib_samples = [
([0.2, 0.73, 0.66], [46, -10, -8]),
([0.201, 0.731, 0.661], [46, -10, -8]),
([0.199, 0.729, 0.659], [46, -10, -8]),
]
a_world, v_world, v_body = estimator.update(acc, rpy, dt) offset = estimator.calibrate_acc_offset(calib_samples)
print("acc_offset_body =", offset)
print("a_world_linear_mps2 =", a_world) dt = 0.01
print("v_world_mps =", v_world)
print("v_body_mps =", v_body)
# For comparison with your original direct body-frame gravity compensation: acc = [0.22, 0.75, 0.67]
print("a_body_linear_g =", remove_gravity_in_body(acc, rpy)) rpy = [46, -10, -8]
acc_world_linear, vel_world, vel_body = estimator.update(acc, rpy, dt)
print("acc_world_linear =", acc_world_linear)
print("vel_world =", vel_world)
print("vel_body =", vel_body)