feat: added comments
This commit is contained in:
Pierre Tellier
parent
aebc157507
commit
0b23964d25
177
control.py
177
control.py
@ -56,12 +56,15 @@ REAL = 0
|
|||||||
|
|
||||||
# Robot setup
|
# Robot setup
|
||||||
if REAL == 1:
|
if REAL == 1:
|
||||||
|
try:
|
||||||
|
portHandler = PortHandler("/dev/ttyUSB0")
|
||||||
|
except:
|
||||||
portHandler = PortHandler("/dev/ttyUSB1")
|
portHandler = PortHandler("/dev/ttyUSB1")
|
||||||
|
|
||||||
packetHandler = PacketHandler(1.0)
|
packetHandler = PacketHandler(1.0)
|
||||||
|
|
||||||
portHandler.openPort()
|
portHandler.openPort()
|
||||||
x = portHandler.setBaudRate(1000000)
|
portHandler.setBaudRate(1000000)
|
||||||
print(x)
|
|
||||||
ADDR_GOAL_POSITION = 30
|
ADDR_GOAL_POSITION = 30
|
||||||
|
|
||||||
ids = []
|
ids = []
|
||||||
@ -76,15 +79,19 @@ if REAL == 1:
|
|||||||
input()
|
input()
|
||||||
|
|
||||||
|
|
||||||
# Done
|
|
||||||
|
|
||||||
def interpol2(point2, point1, t):
|
def interpol2(point2, point1, t):
|
||||||
|
"""
|
||||||
|
Linear interpolation between two points in space
|
||||||
|
"""
|
||||||
x1, y1, z1 = point1
|
x1, y1, z1 = point1
|
||||||
x2, y2, z2 = point2
|
x2, y2, z2 = point2
|
||||||
return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
|
return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
|
||||||
|
|
||||||
|
|
||||||
def get_current_step(t, step_duration, movement_duration):
|
def get_current_step(t, step_duration, movement_duration):
|
||||||
|
"""
|
||||||
|
helper function used to calculate the current step of the movement i.e. what part of the movement.
|
||||||
|
"""
|
||||||
time_passed = 0
|
time_passed = 0
|
||||||
for i in range(len(step_duration)):
|
for i in range(len(step_duration)):
|
||||||
time_passed += step_duration[i]
|
time_passed += step_duration[i]
|
||||||
@ -93,6 +100,9 @@ def get_current_step(t, step_duration, movement_duration):
|
|||||||
|
|
||||||
|
|
||||||
def get_current_step_advancement(t, movement_duration, step_duration, current_step):
|
def get_current_step_advancement(t, movement_duration, step_duration, current_step):
|
||||||
|
"""
|
||||||
|
helper function used to calculate the current advancement of the movement step i.e. the percentage of progression of said movement.
|
||||||
|
"""
|
||||||
current_step = get_current_step(t, step_duration, movement_duration)
|
current_step = get_current_step(t, step_duration, movement_duration)
|
||||||
t = t % movement_duration
|
t = t % movement_duration
|
||||||
for i in range(0, current_step):
|
for i in range(0, current_step):
|
||||||
@ -102,6 +112,7 @@ def get_current_step_advancement(t, movement_duration, step_duration, current_st
|
|||||||
|
|
||||||
def inverse(x, y, z):
|
def inverse(x, y, z):
|
||||||
"""
|
"""
|
||||||
|
Calculate the angle of each motor to have the end of the feet at a specified position
|
||||||
"""
|
"""
|
||||||
# Dimensions (m)
|
# Dimensions (m)
|
||||||
z += l1v
|
z += l1v
|
||||||
@ -109,7 +120,6 @@ def inverse(x, y, z):
|
|||||||
theta0 = atan2(y, x)
|
theta0 = atan2(y, x)
|
||||||
|
|
||||||
l = sqrt((sqrt(x ** 2 + y ** 2) - l1) ** 2 + z ** 2)
|
l = sqrt((sqrt(x ** 2 + y ** 2) - l1) ** 2 + z ** 2)
|
||||||
# l = sqrt((x - l1h*cos(theta0)) ** 2 + (y - l1h*sin(theta0)) ** 2 + (z + l1v) ** 2)
|
|
||||||
|
|
||||||
param2 = -1 * (-(l ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3)
|
param2 = -1 * (-(l ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3)
|
||||||
|
|
||||||
@ -126,15 +136,13 @@ def inverse(x, y, z):
|
|||||||
|
|
||||||
theta1 = acos(param1) + asin(z / l)
|
theta1 = acos(param1) + asin(z / l)
|
||||||
|
|
||||||
# return [-theta0, theta1, theta2]
|
|
||||||
angle1 = atan(l2v / l2h)
|
angle1 = atan(l2v / l2h)
|
||||||
return [-theta0, theta1 + angle1, theta2 + angle1 - pi / 2 + atan(l3h / l3v)]
|
return [-theta0, theta1 + angle1, theta2 + angle1 - pi / 2 + atan(l3h / l3v)]
|
||||||
# return [0, angle1 , angle1 -pi/2 + atan(l3h/l3v)]
|
|
||||||
|
|
||||||
|
|
||||||
def legs(targets_robot):
|
def legs(targets_robot):
|
||||||
"""
|
"""
|
||||||
takes a list of target and offsets it to be in the legs referential
|
takes a list of target and offsets them to be in the legs referential
|
||||||
"""
|
"""
|
||||||
|
|
||||||
targets = [0] * 18
|
targets = [0] * 18
|
||||||
@ -163,7 +171,72 @@ def legs(targets_robot):
|
|||||||
return targets
|
return targets
|
||||||
|
|
||||||
|
|
||||||
|
def normalize(matrix, slider_max, speed):
|
||||||
|
return (matrix / slider_max) * speed
|
||||||
|
|
||||||
|
|
||||||
|
counter = 0
|
||||||
|
|
||||||
|
|
||||||
|
def convert_to_robot(positions):
|
||||||
|
"""
|
||||||
|
Send a robot position to the real robot.
|
||||||
|
"""
|
||||||
|
global counter
|
||||||
|
counter += 1
|
||||||
|
if counter % 2 != 0:
|
||||||
|
return
|
||||||
|
index = [
|
||||||
|
11, 12, 13,
|
||||||
|
41, 42, 43,
|
||||||
|
21, 22, 23,
|
||||||
|
51, 52, 53,
|
||||||
|
61, 62, 63,
|
||||||
|
31, 32, 33]
|
||||||
|
dir = [-1] * 18
|
||||||
|
|
||||||
|
for i in range(len(positions)):
|
||||||
|
value = int(512 + positions[i] * 150 * dir[i])
|
||||||
|
packetHandler.write2ByteTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, value)
|
||||||
|
|
||||||
|
|
||||||
|
def walk(t, sx, sy, sr):
|
||||||
|
"""
|
||||||
|
Choose the algorithm depending on input of xbox controller
|
||||||
|
"""
|
||||||
|
xboxdata = xbox.get_data()
|
||||||
|
if xbox.initialized:
|
||||||
|
max_slider = 0.200
|
||||||
|
controllerLogger.debug(xboxdata)
|
||||||
|
if xbox.mode == 0:
|
||||||
|
positions = static()
|
||||||
|
elif xbox.mode == 1:
|
||||||
|
positions = jump(xboxdata["y2"])
|
||||||
|
elif xbox.mode == 2:
|
||||||
|
positions = dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"],
|
||||||
|
max_slider * xboxdata["y2"],
|
||||||
|
max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
|
||||||
|
elif xbox.mode == 3:
|
||||||
|
positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
|
||||||
|
elif xbox.mode == 4:
|
||||||
|
positions = walk_v2(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"],
|
||||||
|
xboxdata["x2"] * max_slider,
|
||||||
|
max_slider * xboxdata["y2"])
|
||||||
|
# print(positions)
|
||||||
|
if REAL == 1:
|
||||||
|
convert_to_robot(positions)
|
||||||
|
|
||||||
|
return positions
|
||||||
|
else:
|
||||||
|
return walk_v2(t, sx, sy, sr, None)
|
||||||
|
|
||||||
|
|
||||||
|
# different move algorithm
|
||||||
|
|
||||||
def naive_walk(t, speed_x, speed_y):
|
def naive_walk(t, speed_x, speed_y):
|
||||||
|
"""
|
||||||
|
First version of walk, using triangle for each step
|
||||||
|
"""
|
||||||
slider_max = 0.200
|
slider_max = 0.200
|
||||||
|
|
||||||
real_position = np.copy(neutral_position)
|
real_position = np.copy(neutral_position)
|
||||||
@ -229,88 +302,25 @@ def naive_walk(t, speed_x, speed_y):
|
|||||||
return legs(real_position)
|
return legs(real_position)
|
||||||
|
|
||||||
|
|
||||||
def normalize(matrix, slider_max, speed):
|
|
||||||
return (matrix / slider_max) * speed
|
|
||||||
|
|
||||||
|
|
||||||
counter = 0
|
|
||||||
|
|
||||||
|
|
||||||
def convert_to_robot(positions):
|
|
||||||
global counter
|
|
||||||
counter += 1
|
|
||||||
if counter % 2 != 0:
|
|
||||||
return
|
|
||||||
index = [
|
|
||||||
11, 12, 13,
|
|
||||||
41, 42, 43,
|
|
||||||
21, 22, 23,
|
|
||||||
51, 52, 53,
|
|
||||||
61, 62, 63,
|
|
||||||
31, 32, 33]
|
|
||||||
dir = [-1] * 18
|
|
||||||
|
|
||||||
for i in range(len(positions)):
|
|
||||||
value = int(512 + positions[i] * 150 * dir[i])
|
|
||||||
packetHandler.write2ByteTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, value)
|
|
||||||
|
|
||||||
|
|
||||||
def stand(x1, y1, x2, y2):
|
|
||||||
return [
|
|
||||||
0, 0, 0,
|
|
||||||
0, 0, 0,
|
|
||||||
x1, 0, 0,
|
|
||||||
0, -0.5, 0,
|
|
||||||
0, -0.5, 0,
|
|
||||||
y1, 0, 0
|
|
||||||
]
|
|
||||||
|
|
||||||
|
|
||||||
def walk(t, sx, sy, sr):
|
|
||||||
xboxdata = xbox.get_data()
|
|
||||||
if xbox.initialized:
|
|
||||||
max_slider = 0.200
|
|
||||||
controllerLogger.debug(xboxdata)
|
|
||||||
if xbox.mode == 0:
|
|
||||||
positions = static()
|
|
||||||
elif xbox.mode == 1:
|
|
||||||
positions = jump(xboxdata["y2"])
|
|
||||||
elif xbox.mode == 2:
|
|
||||||
positions = dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"],
|
|
||||||
max_slider * xboxdata["y2"],
|
|
||||||
max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
|
|
||||||
elif xbox.mode == 3:
|
|
||||||
positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
|
|
||||||
elif xbox.mode == 4:
|
|
||||||
positions = walk_v2(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"],
|
|
||||||
xboxdata["x2"] * max_slider,
|
|
||||||
max_slider * xboxdata["y2"])
|
|
||||||
elif xbox.mode == 5:
|
|
||||||
positions = stand(xboxdata["x1"], xboxdata["y1"], xboxdata["x2"], xboxdata["y2"])
|
|
||||||
|
|
||||||
# print(positions)
|
|
||||||
if REAL == 1:
|
|
||||||
convert_to_robot(positions)
|
|
||||||
|
|
||||||
return positions
|
|
||||||
else:
|
|
||||||
return walk_v2(t, sx, sy, sr, None)
|
|
||||||
|
|
||||||
|
|
||||||
def static():
|
def static():
|
||||||
|
"""
|
||||||
|
Function to have the robot stand at the neutral position
|
||||||
|
"""
|
||||||
return legs(neutral_position)
|
return legs(neutral_position)
|
||||||
|
|
||||||
|
|
||||||
# Walk V2
|
# Walk V2
|
||||||
def walk_v2(t, speed_x, speed_y, speed_r, y2):
|
def walk_v2(t, speed_x, speed_y, speed_r, y2):
|
||||||
"""
|
"""
|
||||||
x1: speed along the x-axis
|
Second version of the walking algorithm, using circle for each leg
|
||||||
y1: speed along the y-axis
|
This is the main algorithm.
|
||||||
x2: rotational speed along the z-axis
|
|
||||||
"""
|
"""
|
||||||
max_dist = 0.3
|
max_dist = 0.3
|
||||||
|
|
||||||
def get_rotation_center(speed_x, speed_y, theta_point):
|
def get_rotation_center(speed_x, speed_y, theta_point):
|
||||||
|
"""
|
||||||
|
Helper function that calculate the center of rotation of the robot based on the current parameters
|
||||||
|
"""
|
||||||
norm = sqrt(speed_x ** 2 + speed_y ** 2)
|
norm = sqrt(speed_x ** 2 + speed_y ** 2)
|
||||||
|
|
||||||
r = 1000 if theta_point == 0 else norm / theta_point
|
r = 1000 if theta_point == 0 else norm / theta_point
|
||||||
@ -320,9 +330,7 @@ def walk_v2(t, speed_x, speed_y, speed_r, y2):
|
|||||||
return 0, 0
|
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)
|
||||||
|
|
||||||
legsLogger.debug(f"rotation center: center_x: {center_x}, center_y: {center_y}")
|
legsLogger.debug(f"rotation center: center_x: {center_x}, center_y: {center_y}")
|
||||||
|
|
||||||
real_position = np.copy(neutral_position)
|
real_position = np.copy(neutral_position)
|
||||||
|
|
||||||
movement_z = np.array([
|
movement_z = np.array([
|
||||||
@ -408,6 +416,9 @@ def walk_v2(t, speed_x, speed_y, speed_r, y2):
|
|||||||
|
|
||||||
|
|
||||||
def jump(sy):
|
def jump(sy):
|
||||||
|
"""
|
||||||
|
Make the robot jump (at least we wished)
|
||||||
|
"""
|
||||||
offset = np.array([
|
offset = np.array([
|
||||||
[0, 0, -0.15],
|
[0, 0, -0.15],
|
||||||
[0, 0, -0.15],
|
[0, 0, -0.15],
|
||||||
@ -420,8 +431,10 @@ def jump(sy):
|
|||||||
return legs(neutral_position + offset)
|
return legs(neutral_position + offset)
|
||||||
|
|
||||||
|
|
||||||
# based on walk V1
|
|
||||||
def dino_naive(t, speed_x, speed_y, hz, hy, hx):
|
def dino_naive(t, speed_x, speed_y, hz, hy, hx):
|
||||||
|
"""
|
||||||
|
based on walk naive, but with a head and a tail
|
||||||
|
"""
|
||||||
slider_max = 0.200
|
slider_max = 0.200
|
||||||
|
|
||||||
real_position = np.copy(neutral_position)
|
real_position = np.copy(neutral_position)
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user