piair/vroom-vroom-vroom
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix bug in rotation

Browse Source
This commit is contained in:
Pierre Tellier 2025-04-14 18:02:30 +02:00
parent 25af55d963
commit 561b2f70a2
1 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
control.py
View File

@ -306,10 +306,16 @@ def dev(t, speed_x, speed_y, speed_r, y2):
x2: rotational speed along the z-axis x2: rotational speed along the z-axis
""" """
max_dist = 0.3 max_dist = 0.3
print(speed_r)
def get_rotation_center(speed_x, speed_y, theta_point): def get_rotation_center(speed_x, speed_y, theta_point):
direction = np.array([-speed_y, speed_x]) norm = sqrt(speed_x ** 2 + speed_y ** 2)
return direction / max(0.00001, theta_point)
r = 1000 if theta_point == 0 else norm / theta_point
if norm != 0:
return np.array([speed_y, -speed_x]) / norm * r
else:
return 0, 0
center_x, center_y = get_rotation_center(speed_x, speed_y, speed_r) center_x, center_y = get_rotation_center(speed_x, speed_y, speed_r)
@ -337,6 +343,7 @@ def dev(t, speed_x, speed_y, speed_r, y2):
offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration # offset between each leg offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration # offset between each leg
assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them" assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"
# used to cap the speed to a max
max_radius = 0 max_radius = 0
for patte in range(num_patte): for patte in range(num_patte):
x1, y1 = real_position[patte][0], real_position[patte][1] x1, y1 = real_position[patte][0], real_position[patte][1]
@ -367,8 +374,11 @@ def dev(t, speed_x, speed_y, speed_r, y2):
if y1 < center_y: if y1 < center_y:
cur_theta *= -1 cur_theta *= -1
if speed_r != 0:
speed = sqrt(speed_x ** 2 + speed_y ** 2) * (speed_r / abs(speed_r))
else:
speed = sqrt(speed_x ** 2 + speed_y ** 2)
speed = abs(speed_x) + abs(speed_y)
if speed == 0: if speed == 0:
speed = speed_r speed = speed_r
dthteta = speed * (1 / max_radius * max_dist) dthteta = speed * (1 / max_radius * max_dist)
@ -390,7 +400,9 @@ def dev(t, speed_x, speed_y, speed_r, y2):
real_position[patte][0] = wanted_x real_position[patte][0] = wanted_x
real_position[patte][1] = wanted_y real_position[patte][1] = wanted_y
if REAL:
return legs(real_position) return legs(real_position)
return legs(real_position), [center_x, center_y]
def jump(sy): def jump(sy):