512 lines
15 KiB
Python
512 lines
15 KiB
Python
import logging
|
|
|
|
import numpy as np
|
|
from math import *
|
|
from logs import *
|
|
from dynamixel_sdk import *
|
|
from xbox import *
|
|
import time
|
|
|
|
# DEBUG
|
|
LEGS_LOG_LEVEL = logging.WARNING
|
|
CONTROLLER_LOG_LEVEL = logging.CRITICAL
|
|
# Variables configurations
|
|
|
|
l1h = 0.049
|
|
l1v = 0.032
|
|
l1 = l1h # this is not the real distance as it's not the one needed to calculate position.
|
|
|
|
# length between motor 2 and motor 3.
|
|
l2h = 0.0605
|
|
l2v = 0.02215
|
|
l2 = sqrt(l2h ** 2 + l2v ** 2)
|
|
|
|
# length between motor 3 and end of the leg.
|
|
l3h = 0.012
|
|
l3v = 0.093
|
|
l3 = sqrt(l3h ** 2 + l3v ** 2)
|
|
|
|
# offset of the 'head', the legs isolated at each end.
|
|
tete_x = 0.095
|
|
|
|
# offset of the legs at the side.
|
|
patte_y = 0.032
|
|
patte_x = 0.079
|
|
num_patte = 6
|
|
|
|
# Logs functions
|
|
legsLogger = setup_logger("legs", LEGS_LOG_LEVEL)
|
|
controllerLogger = setup_logger("Controller", CONTROLLER_LOG_LEVEL)
|
|
|
|
# Initialize controller
|
|
xbox = Xbox(controllerLogger)
|
|
CONTROLLER_MODE = xbox.initialized
|
|
xbox.mode_count = 5
|
|
|
|
neutral_position = np.array([
|
|
[0.08, 0.16, -0.12],
|
|
[-0.08, 0.16, -0.12],
|
|
[-0.2, -0.00, -0.12],
|
|
[-0.08, -0.16, -0.12],
|
|
[0.08, -0.16, -0.12],
|
|
[0.2, 0, -0.12]
|
|
])
|
|
|
|
REAL = 0
|
|
|
|
# Robot setup
|
|
if REAL == 1:
|
|
try:
|
|
portHandler = PortHandler("/dev/ttyUSB0")
|
|
except:
|
|
portHandler = PortHandler("/dev/ttyUSB1")
|
|
|
|
packetHandler = PacketHandler(1.0)
|
|
|
|
portHandler.openPort()
|
|
portHandler.setBaudRate(1000000)
|
|
ADDR_GOAL_POSITION = 30
|
|
|
|
ids = []
|
|
for id in range(0, 100):
|
|
model_number, result, error = packetHandler.ping(portHandler, id)
|
|
if model_number == 12:
|
|
print(f"Found AX-12 with id: {id}")
|
|
ids.append(id)
|
|
time.sleep(0.01)
|
|
if len(ids) != 18:
|
|
legsLogger.error("Not all motor found. Press enter to continue")
|
|
input()
|
|
|
|
|
|
def interpol2(point2, point1, t):
|
|
"""
|
|
Linear interpolation between two points in space
|
|
"""
|
|
x1, y1, z1 = point1
|
|
x2, y2, z2 = point2
|
|
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):
|
|
"""
|
|
helper function used to calculate the current step of the movement i.e. what part of the movement.
|
|
"""
|
|
time_passed = 0
|
|
for i in range(len(step_duration)):
|
|
time_passed += step_duration[i]
|
|
if t % movement_duration < time_passed:
|
|
return i
|
|
|
|
|
|
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)
|
|
t = t % movement_duration
|
|
for i in range(0, current_step):
|
|
t -= step_duration[i]
|
|
return t / step_duration[current_step]
|
|
|
|
|
|
def inverse(x, y, z):
|
|
"""
|
|
Calculate the angle of each motor to have the end of the feet at a specified position
|
|
"""
|
|
# Dimensions (m)
|
|
z += l1v
|
|
|
|
theta0 = atan2(y, x)
|
|
|
|
l = sqrt((sqrt(x ** 2 + y ** 2) - l1) ** 2 + z ** 2)
|
|
|
|
param2 = -1 * (-(l ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3)
|
|
|
|
if param2 > 1 or param2 < -1:
|
|
legsLogger.warning("fTentative d'acces a une position impossible (param2) ({x}, {y}, {z})")
|
|
param2 = 1 if param2 > 1 else -1
|
|
|
|
theta2 = acos(param2)
|
|
|
|
param1 = (-l3 ** 2 + l2 ** 2 + l ** 2) / (2 * l2 * l)
|
|
if param1 > 1 or param1 < -1:
|
|
legsLogger.warning(f"Tentative d'acces a une position impossible (param1) ({x}, {y}, {z})")
|
|
param1 = 1 if param1 > 1 else -1
|
|
|
|
theta1 = acos(param1) + asin(z / l)
|
|
|
|
angle1 = atan(l2v / l2h)
|
|
return [-theta0, theta1 + angle1, theta2 + angle1 - pi / 2 + atan(l3h / l3v)]
|
|
|
|
|
|
def legs(targets_robot):
|
|
"""
|
|
takes a list of target and offsets them to be in the legs referential
|
|
"""
|
|
|
|
targets = [0] * 18
|
|
|
|
cos_val = [0, 0, -1, 0, 0, 1]
|
|
sin_val = [-1, -1, 0, 1, 1, 0]
|
|
|
|
offset_x = [-patte_x, -patte_x, -tete_x, -patte_x, -patte_x, -tete_x]
|
|
offset_y = [patte_y, -patte_y, 0, patte_y, -patte_y, 0]
|
|
|
|
for i in range(6):
|
|
target_x, target_y, target_z = targets_robot[i]
|
|
|
|
target_x_tmp = cos_val[i] * target_x - sin_val[i] * target_y
|
|
target_y = sin_val[i] * target_x + cos_val[i] * target_y
|
|
target_x = target_x_tmp
|
|
|
|
target_x += offset_x[i]
|
|
target_y += offset_y[i]
|
|
|
|
alpha, beta, gamma = inverse(target_x, target_y, target_z)
|
|
targets[3 * i] = alpha
|
|
targets[3 * i + 1] = beta
|
|
targets[3 * i + 2] = gamma
|
|
|
|
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):
|
|
"""
|
|
First version of walk, using triangle for each step
|
|
"""
|
|
slider_max = 0.200
|
|
|
|
real_position = np.copy(neutral_position)
|
|
|
|
movement_x = np.array([
|
|
[0.00, 0, 0],
|
|
[0.04, 0, 0],
|
|
[-0.04, 0, 0],
|
|
])
|
|
|
|
movement_y = np.array([
|
|
[0.0, 0, 0],
|
|
[0, 0.04, 0],
|
|
[0, -0.04, 0],
|
|
])
|
|
|
|
movement_z = np.array([
|
|
[0, 0, 0.08],
|
|
[0, 0, -0.02],
|
|
[0, 0, -0.02]
|
|
])
|
|
|
|
# duration of each step of the movement
|
|
step_duration = np.array([0.05, 0.3, 0.05])
|
|
|
|
step_count = len(movement_z)
|
|
movement_duration = np.sum(step_duration)
|
|
|
|
assert len(
|
|
step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"
|
|
|
|
def get_next_step(t):
|
|
return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)
|
|
|
|
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"
|
|
|
|
for patte in range(num_patte):
|
|
time = t + offsets[patte]
|
|
mov_index_start = get_current_step(time, step_duration, movement_duration)
|
|
mov_index_end = get_next_step(time)
|
|
|
|
mov_start_x = normalize(movement_x[mov_index_start], slider_max, speed_x)
|
|
mov_end_x = normalize(movement_x[mov_index_end], slider_max, speed_x)
|
|
|
|
mov_start_y = normalize(movement_y[mov_index_start], slider_max, speed_y)
|
|
mov_end_y = normalize(movement_y[mov_index_end], slider_max, speed_y)
|
|
|
|
mov_start_z = movement_z[mov_index_start]
|
|
mov_end_z = movement_z[mov_index_end]
|
|
|
|
mov_start = neutral_position[patte] + mov_start_z + mov_start_x + mov_start_y
|
|
mov_end = neutral_position[patte] + mov_end_z + mov_end_x + mov_end_y
|
|
|
|
(real_position[patte][0],
|
|
real_position[patte][1],
|
|
real_position[patte][2]) = interpol2(mov_start, mov_end,
|
|
get_current_step_advancement(time, movement_duration, step_duration,
|
|
mov_index_start))
|
|
legsLogger.debug(
|
|
f"[{patte}] [{mov_index_start}->{mov_index_end}], start: {mov_start}, end: {mov_end}, current ({real_position[patte][0]}, {real_position[patte][1]}, {real_position[patte][2]})")
|
|
|
|
return legs(real_position)
|
|
|
|
|
|
def static():
|
|
"""
|
|
Function to have the robot stand at the neutral position
|
|
"""
|
|
return legs(neutral_position)
|
|
|
|
|
|
# Walk V2
|
|
def walk_v2(t, speed_x, speed_y, speed_r, y2):
|
|
"""
|
|
Second version of the walking algorithm, using circle for each leg
|
|
This is the main algorithm.
|
|
"""
|
|
max_dist = 0.3
|
|
|
|
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)
|
|
|
|
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)
|
|
legsLogger.debug(f"rotation center: center_x: {center_x}, center_y: {center_y}")
|
|
real_position = np.copy(neutral_position)
|
|
|
|
movement_z = np.array([
|
|
[0, 0, 0.03],
|
|
[0, 0, -0.02],
|
|
[0, 0, -0.02]
|
|
])
|
|
|
|
step_duration = np.array([0.15, 0.3, 0.15])
|
|
|
|
step_count = len(movement_z)
|
|
movement_duration = np.sum(step_duration)
|
|
|
|
assert len(
|
|
step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"
|
|
|
|
def get_next_step(t):
|
|
return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)
|
|
|
|
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"
|
|
|
|
# used to cap the speed to a max
|
|
max_radius = 0
|
|
for patte in range(num_patte):
|
|
x1, y1 = real_position[patte][0], real_position[patte][1]
|
|
circle_radius = sqrt((center_x - x1) ** 2 + (center_y - y1) ** 2)
|
|
max_radius = max(max_radius, circle_radius)
|
|
|
|
for patte in range(num_patte):
|
|
time = t + offsets[patte]
|
|
mov_index_start = get_current_step(time, step_duration, movement_duration)
|
|
mov_index_end = get_next_step(time)
|
|
|
|
step_adv = get_current_step_advancement(time, movement_duration, step_duration, mov_index_start)
|
|
|
|
mov_start_z = movement_z[mov_index_start]
|
|
mov_end_z = movement_z[mov_index_end]
|
|
|
|
mov_start = neutral_position[patte] + mov_start_z
|
|
mov_end = neutral_position[patte] + mov_end_z
|
|
|
|
(real_position[patte][0],
|
|
real_position[patte][1],
|
|
real_position[patte][2]) = interpol2(mov_start, mov_end, step_adv)
|
|
|
|
x1, y1 = real_position[patte][0], real_position[patte][1]
|
|
|
|
circle_radius = sqrt((center_x - x1) ** 2 + (center_y - y1) ** 2)
|
|
cur_theta = acos((x1 - center_x) / circle_radius)
|
|
|
|
if y1 < center_y:
|
|
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)
|
|
|
|
if speed == 0:
|
|
speed = speed_r
|
|
dthteta = speed * (1 / max_radius * max_dist)
|
|
legsLogger.info(f"speed: {speed}, dthteta: {dthteta}")
|
|
if mov_index_start == 0:
|
|
# midair part 1
|
|
theta_offset = dthteta * (- step_adv)
|
|
elif mov_index_start == 1:
|
|
# moving part
|
|
theta_offset = dthteta * (step_adv - 0.5) * 2
|
|
else:
|
|
# midair part 1
|
|
theta_offset = dthteta * (1 - step_adv)
|
|
wanted_theta = cur_theta + theta_offset
|
|
|
|
wanted_x = center_x + circle_radius * cos(wanted_theta)
|
|
wanted_y = center_y + circle_radius * sin(wanted_theta)
|
|
|
|
real_position[patte][0] = wanted_x
|
|
real_position[patte][1] = wanted_y
|
|
|
|
if REAL:
|
|
return legs(real_position)
|
|
return legs(real_position), [center_x, center_y]
|
|
|
|
|
|
def jump(sy):
|
|
"""
|
|
Make the robot jump (at least we wished)
|
|
"""
|
|
offset = np.array([
|
|
[0, 0, -0.15],
|
|
[0, 0, -0.15],
|
|
[0, 0, -0.15],
|
|
[0, 0, -0.15],
|
|
[0, 0, -0.15],
|
|
[0, 0, -0.15]
|
|
])
|
|
offset *= sy
|
|
return legs(neutral_position + offset)
|
|
|
|
|
|
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
|
|
|
|
real_position = np.copy(neutral_position)
|
|
|
|
movement_x = np.array([
|
|
[0.00, 0, 0],
|
|
[0.04, 0, 0],
|
|
[-0.04, 0, 0],
|
|
])
|
|
|
|
movement_y = np.array([
|
|
[0.0, 0, 0],
|
|
[0, 0.04, 0],
|
|
[0, -0.04, 0],
|
|
])
|
|
|
|
movement_z = np.array([
|
|
[0, 0, 0.08],
|
|
[0, 0, -0.02],
|
|
[0, 0, -0.02]
|
|
])
|
|
|
|
# duration of each step of the movement
|
|
step_duration = np.array([0.05, 0.3, 0.05])
|
|
|
|
step_count = len(movement_z)
|
|
movement_duration = np.sum(step_duration)
|
|
|
|
assert len(
|
|
step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"
|
|
|
|
def get_next_step(t):
|
|
return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)
|
|
|
|
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"
|
|
|
|
for patte in range(num_patte):
|
|
if patte in [2, 5]:
|
|
continue
|
|
time = t + offsets[patte]
|
|
mov_index_start = get_current_step(time, step_duration, movement_duration)
|
|
mov_index_end = get_next_step(time)
|
|
|
|
mov_start_x = normalize(movement_x[mov_index_start], slider_max, speed_x)
|
|
mov_end_x = normalize(movement_x[mov_index_end], slider_max, speed_x)
|
|
|
|
mov_start_y = normalize(movement_y[mov_index_start], slider_max, speed_y)
|
|
mov_end_y = normalize(movement_y[mov_index_end], slider_max, speed_y)
|
|
|
|
mov_start_z = movement_z[mov_index_start]
|
|
mov_end_z = movement_z[mov_index_end]
|
|
|
|
mov_start = neutral_position[patte] + mov_start_z + mov_start_x + mov_start_y
|
|
mov_end = neutral_position[patte] + mov_end_z + mov_end_x + mov_end_y
|
|
|
|
(real_position[patte][0],
|
|
real_position[patte][1],
|
|
real_position[patte][2]) = interpol2(mov_start, mov_end,
|
|
get_current_step_advancement(time, movement_duration, step_duration,
|
|
mov_index_start))
|
|
|
|
real_position[2][2] = -0.1
|
|
real_position[2][0] = -0.28
|
|
|
|
real_position[5][2] = 0.05 + hz
|
|
real_position[5][0] = 0.25 + hx
|
|
real_position[5][1] = hy
|
|
|
|
return legs(real_position)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
print("N'exécutez pas ce fichier, mais simulator.py")
|