shark/hexapod
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

rename folders

Browse Source
This commit is contained in:
Zhengyu Peng
2022-01-06 11:25:06 -05:00
parent a31398347b
commit 143671fdb4
26 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
software/raspberry pi/__init__.py Normal file
View File

@ -0,0 +1,25 @@
#!python
#
# 2021 Zhengyu Peng
# Website: https://zpeng.me
#
# ` `
# -:. -#:
# -//:. -###:
# -////:. -#####:
# -/:.://:. -###++##:
# .. `://:- -###+. :##:
# `:/+####+. :##:
# .::::::::/+###. :##:
# .////-----+##: `:###:
# `-//:. :##: `:###/.
# `-//:. :##:`:###/.
# `-//:+######/.
# `-/+####/.
# `+##+.
# :##:
# :##:
# :##:
# :##:
# :##:
# .+:

12
software/raspberry pi/config.json Normal file
View File

@ -0,0 +1,12 @@
{
"legNames":["front_right", "center_right", "rear_right", "rear_left", "center_left", "front_left"],
"legMountX":[29.41, 36.87, 29.41, -29.41, -36.87, -29.41],
"legMountY":[55.41, 0, -55.41, -55.41, 0, 55.41],
"legMountAngle":[45, 0, -45, -135, -180, -225],
"legRootToJoint1":20.75,
"legJoint1ToJoint2":28.0,
"legJoint2ToJoint3":42.6,
"legJoint3ToTip":89.07,
"movementInterval":5,
"movementSwitchDuration":150
}

371
software/raspberry pi/hexapod.py Normal file
View File

@ -0,0 +1,371 @@
#!python
#
# 2021 Zhengyu Peng
# Website: https://zpeng.me
#
# ` `
# -:. -#:
# -//:. -###:
# -////:. -#####:
# -/:.://:. -###++##:
# .. `://:- -###+. :##:
# `:/+####+. :##:
# .::::::::/+###. :##:
# .////-----+##: `:###:
# `-//:. :##: `:###/.
# `-//:. :##:`:###/.
# `-//:+######/.
# `-/+####/.
# `+##+.
# :##:
# :##:
# :##:
# :##:
# :##:
# .+:
# Libraries
# https://circuitpython.readthedocs.io/projects/servokit/en/latest/
from adafruit_servokit import ServoKit
from leg import Leg
from queue import Queue, Empty
# python3-numpy
import numpy as np
import time
import json
from path_generator import gen_forward_path, gen_backward_path
from path_generator import gen_fastforward_path, gen_fastbackward_path
from path_generator import gen_leftturn_path, gen_rightturn_path
from path_generator import gen_shiftleft_path, gen_shiftright_path
from path_generator import gen_climb_path
from path_generator import gen_rotatex_path, gen_rotatey_path, gen_rotatez_path
from path_generator import gen_twist_path
import socket
import errno
from threading import Thread
from tcpserver import TCPServer
class Hexapod(Thread):
def __init__(self, in_cmd_queue):
Thread.__init__(self)
self.cmd_queue = in_cmd_queue
self.interval = 0.005
# x -> right
# y -> front
# z -> up
# origin is the center of the body
# roots are the positions of the bottom screws
# length units are in mm
# time units are in ms
with open('./config.json', 'r') as read_file:
self.config = json.load(read_file)
self.mount_x = np.array(self.config['legMountX'])
self.mount_y = np.array(self.config['legMountY'])
self.root_j1 = self.config['legRootToJoint1']
self.j1_j2 = self.config['legJoint1ToJoint2']
self.j2_j3 = self.config['legJoint2ToJoint3']
self.j3_tip = self.config['legJoint3ToTip']
self.mount_angle = np.array(self.config['legMountAngle'])/180*np.pi
self.mount_position = np.zeros((6, 3))
self.mount_position[:, 0] = self.mount_x
self.mount_position[:, 1] = self.mount_y
# Objects
self.pca_right = ServoKit(channels=16, address=0x40, frequency=120)
self.pca_left = ServoKit(channels=16, address=0x41, frequency=120)
# front right
self.leg_0 = Leg(0,
[self.pca_left.servo[15], self.pca_left.servo[2],
self.pca_left.servo[1]],
correction=[4, 6, 2])
# center right
self.leg_1 = Leg(1,
[self.pca_left.servo[7], self.pca_left.servo[8],
self.pca_left.servo[6]],
correction=[0, 8, -6])
# rear right
self.leg_2 = Leg(2,
[self.pca_left.servo[0], self.pca_left.servo[14],
self.pca_left.servo[13]],
correction=[2, 8, -1])
# rear left
self.leg_3 = Leg(3,
[self.pca_right.servo[15], self.pca_right.servo[1],
self.pca_right.servo[2]],
correction=[-3, 10, -8])
# center left
self.leg_4 = Leg(4,
[self.pca_right.servo[7], self.pca_right.servo[6],
self.pca_right.servo[8]],
correction=[-6, 2, -4])
# front left
self.leg_5 = Leg(5,
[self.pca_right.servo[0], self.pca_right.servo[13],
self.pca_right.servo[14]],
correction=[0, 0, -10])
# self.leg_0.reset(True)
# self.leg_1.reset(True)
# self.leg_2.reset(True)
# self.leg_3.reset(True)
# self.leg_4.reset(True)
# self.leg_5.reset(True)
self.standby_coordinate = self.calculate_standby_coordinate(60, 75)
self.forward_path = gen_forward_path(self.standby_coordinate)
self.backward_path = gen_backward_path(self.standby_coordinate)
self.fastforward_path = gen_fastforward_path(self.standby_coordinate)
self.fastbackward_path = gen_fastbackward_path(self.standby_coordinate)
self.leftturn_path = gen_leftturn_path(self.standby_coordinate)
self.rightturn_path = gen_rightturn_path(self.standby_coordinate)
self.shiftleft_path = gen_shiftleft_path(self.standby_coordinate)
self.shiftright_path = gen_shiftright_path(self.standby_coordinate)
self.climb_path = gen_climb_path(self.standby_coordinate)
self.rotatex_path = gen_rotatex_path(self.standby_coordinate)
self.rotatey_path = gen_rotatey_path(self.standby_coordinate)
self.rotatez_path = gen_rotatez_path(self.standby_coordinate)
self.twist_path = gen_twist_path(self.standby_coordinate)
self.current_motion = None
self.standby()
time.sleep(1)
# self.leg_0.set_angle(1, 30)
# for mm in range(0, 10):
# self.move(self.forward_path)
# for mm in range(0, 10):
# self.move(self.backward_path, 0.005)
# for mm in range(0, 10):
# self.move(self.fastforward_path, 0.005)
# for mm in range(0, 10):
# self.move(self.fastbackward_path, 0.005)
# for mm in range(0, 10):
# self.move(self.leftturn_path, 0.005)
# for mm in range(0, 10):
# self.move(self.rightturn_path, 0.005)
# for mm in range(0, 10):
# self.move(self.shiftleft_path, 0.005)
# for mm in range(0, 10):
# self.move(self.shiftright_path, 0.005)
# for mm in range(0, 10):
# self.move(self.climb_path, 0.005)
# for mm in range(0, 10):
# self.move(self.rotatex_path, 0.005)
# for mm in range(0, 10):
# self.move(self.rotatey_path, 0.005)
# for mm in range(0, 10):
# self.move(self.rotatez_path, 0.005)
# for mm in range(0, 10):
# self.move(self.twist_path, 0.005)
# time.sleep(1)
# self.standby()
def calculate_standby_coordinate(self, j2_angle, j3_angle):
j2_rad = j2_angle/180*np.pi
j3_rad = j3_angle/180*np.pi
standby_coordinate = np.zeros((6, 3))
standby_coordinate[:, 0] = self.mount_x+(self.root_j1+self.j1_j2+(
self.j2_j3*np.sin(j2_rad))+self.j3_tip*np.cos(j3_rad))*np.cos(self.mount_angle)
standby_coordinate[:, 1] = self.mount_y + (self.root_j1+self.j1_j2+(
self.j2_j3*np.sin(j2_rad))+self.j3_tip*np.cos(j3_rad))*np.sin(self.mount_angle)
standby_coordinate[:, 2] = self.j2_j3 * \
np.cos(j2_rad) - self.j3_tip * \
np.sin(j3_rad)
return standby_coordinate
def move(self, path):
for p_idx in range(0, np.shape(path)[0]):
dest = path[p_idx, :, :]
angles = self.inverse_kinematics(dest)
self.leg_0.move_junctions(angles[0, :])
self.leg_5.move_junctions(angles[5, :])
self.leg_1.move_junctions(angles[1, :])
self.leg_4.move_junctions(angles[4, :])
self.leg_2.move_junctions(angles[2, :])
self.leg_3.move_junctions(angles[3, :])
time.sleep(self.interval)
def move_routine(self, path):
for p_idx in range(0, np.shape(path)[0]):
dest = path[p_idx, :, :]
angles = self.inverse_kinematics(dest)
self.leg_0.move_junctions(angles[0, :])
self.leg_5.move_junctions(angles[5, :])
self.leg_1.move_junctions(angles[1, :])
self.leg_4.move_junctions(angles[4, :])
self.leg_2.move_junctions(angles[2, :])
self.leg_3.move_junctions(angles[3, :])
try:
data = self.cmd_queue.get(block=False)
print('interrput')
print(data)
except Empty:
time.sleep(self.interval)
pass
else:
if data == 'standby':
self.current_motion = None
self.standby()
elif data == 'forward':
self.current_motion = self.forward_path
elif data == 'backward':
self.current_motion = self.backward_path
elif data == 'fastforward':
self.current_motion = self.fastforward_path
elif data == 'fastbackward':
self.current_motion = self.fastbackward_path
elif data == 'leftturn':
self.current_motion = self.leftturn_path
elif data == 'rightturn':
self.current_motion = self.rightturn_path
elif data == 'shiftleft':
self.current_motion = self.shiftleft_path
elif data == 'shiftright':
self.current_motion = self.shiftright_path
elif data == 'climb':
self.current_motion = self.climb_path
elif data == 'rotatex':
self.current_motion = self.rotatex_path
elif data == 'rotatey':
self.current_motion = self.rotatey_path
elif data == 'rotatez':
self.current_motion = self.rotatez_path
elif data == 'twist':
self.current_motion = self.twist_path
else:
self.current_motion = None
self.cmd_queue.task_done()
break
def standby(self):
dest = self.standby_coordinate
angles = self.inverse_kinematics(dest)
self.leg_0.move_junctions(angles[0, :])
self.leg_5.move_junctions(angles[5, :])
self.leg_1.move_junctions(angles[1, :])
self.leg_4.move_junctions(angles[4, :])
self.leg_2.move_junctions(angles[2, :])
self.leg_3.move_junctions(angles[3, :])
def inverse_kinematics(self, dest):
temp_dest = dest-self.mount_position
local_dest = np.zeros_like(dest)
local_dest[:, 0] = temp_dest[:, 0] * \
np.cos(self.mount_angle) + \
temp_dest[:, 1] * np.sin(self.mount_angle)
local_dest[:, 1] = temp_dest[:, 0] * \
np.sin(self.mount_angle) - \
temp_dest[:, 1] * np.cos(self.mount_angle)
local_dest[:, 2] = temp_dest[:, 2]
angles = np.zeros((6, 3))
x = local_dest[:, 0] - self.root_j1
y = local_dest[:, 1]
angles[:, 0] = -(np.arctan2(y, x) * 180 / np.pi)+90
x = np.sqrt(x*x + y*y) - self.j1_j2
y = local_dest[:, 2]
ar = np.arctan2(y, x)
lr2 = x*x + y*y
lr = np.sqrt(lr2)
a1 = np.arccos((lr2 + self.j2_j3*self.j2_j3 -
self.j3_tip*self.j3_tip)/(2*self.j2_j3*lr))
a2 = np.arccos((lr2 - self.j2_j3*self.j2_j3 +
self.j3_tip*self.j3_tip)/(2*self.j3_tip*lr))
angles[:, 1] = 90-((ar + a1) * 180 / np.pi)
angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))+90
return angles
def run(self):
while True:
if self.current_motion is None:
try:
data = self.cmd_queue.get(block=False)
print(data)
except Empty:
time.sleep(self.interval)
pass
else:
if data == 'standby':
self.current_motion = None
self.standby()
elif data == 'forward':
self.current_motion = self.forward_path
elif data == 'backward':
self.current_motion = self.backward_path
elif data == 'fastforward':
self.current_motion = self.fastforward_path
elif data == 'fastbackward':
self.current_motion = self.fastbackward_path
elif data == 'leftturn':
self.current_motion = self.leftturn_path
elif data == 'rightturn':
self.current_motion = self.rightturn_path
elif data == 'shiftleft':
self.current_motion = self.shiftleft_path
elif data == 'shiftright':
self.current_motion = self.shiftright_path
elif data == 'climb':
self.current_motion = self.climb_path
elif data == 'rotatex':
self.current_motion = self.rotatex_path
elif data == 'rotatey':
self.current_motion = self.rotatey_path
elif data == 'rotatez':
self.current_motion = self.rotatez_path
elif data == 'twist':
self.current_motion = self.twist_path
else:
self.current_motion = None
self.cmd_queue.task_done()
if self.current_motion is not None:
self.move_routine(self.current_motion)
def main():
q = Queue()
tcp_server = TCPServer(q)
tcp_server.start()
hexapod = Hexapod(q)
hexapod.start()
if __name__ == '__main__':
main()

64
software/raspberry pi/leg.py Normal file
View File

@ -0,0 +1,64 @@
#!python
#
# 2021 Zhengyu Peng
# Website: https://zpeng.me
#
# ` `
# -:. -#:
# -//:. -###:
# -////:. -#####:
# -/:.://:. -###++##:
# .. `://:- -###+. :##:
# `:/+####+. :##:
# .::::::::/+###. :##:
# .////-----+##: `:###:
# `-//:. :##: `:###/.
# `-//:. :##:`:###/.
# `-//:+######/.
# `-/+####/.
# `+##+.
# :##:
# :##:
# :##:
# :##:
# :##:
# .+:
import numpy as np
class Leg:
def __init__(self,
id,
junction_servos,
correction=[0, 0, 0],
constraint=[[35, 145], [10, 165], [30, 150]]):
self.id = id
self.junction_servos = junction_servos
self.correction = correction
self.constraint = constraint
def set_angle(self, junction, angle):
set_angle = np.min(
[angle+self.correction[junction], self.constraint[junction][1]+self.correction[junction], 180])
set_angle = np.max(
[set_angle, self.constraint[junction][0]+self.correction[junction], 0])
self.junction_servos[junction].angle = set_angle
def set_raw_angle(self, junction, angle):
self.junction_servos[junction].angle = angle
def move_junctions(self, angles):
self.set_angle(0, angles[0])
self.set_angle(1, angles[1])
self.set_angle(2, angles[2])
def reset(self, calibrated=False):
if calibrated:
self.set_angle(0, 90)
self.set_angle(1, 90)
self.set_angle(2, 90)
else:
self.set_raw_angle(0, 90)
self.set_raw_angle(1, 90)
self.set_raw_angle(2, 90)

134
software/raspberry pi/lib.py Normal file
View File

@ -0,0 +1,134 @@
#!python
#
# 2021 Zhengyu Peng
# Website: https://zpeng.me
#
# ` `
# -:. -#:
# -//:. -###:
# -////:. -#####:
# -/:.://:. -###++##:
# .. `://:- -###+. :##:
# `:/+####+. :##:
# .::::::::/+###. :##:
# .////-----+##: `:###:
# `-//:. :##: `:###/.
# `-//:. :##:`:###/.
# `-//:+######/.
# `-/+####/.
# `+##+.
# :##:
# :##:
# :##:
# :##:
# :##:
# .+:
import numpy as np
def semicircle_generator(radius, steps, reverse=False):
assert (steps % 4) == 0
halfsteps = int(steps/2)
step_angle = np.pi / halfsteps
result = np.zeros((steps, 3))
halfsteps_array = np.arange(halfsteps)
# first half, move backward (only y change)
result[:halfsteps, 1] = radius - halfsteps_array*radius*2/(halfsteps)
# second half, move forward in semicircle shape (y, z change)
angle = np.pi - step_angle*halfsteps_array
result[halfsteps:, 1] = radius * np.cos(angle)
result[halfsteps:, 2] = radius * np.sin(angle)
result = np.roll(result, int(steps/4), axis=0)
if reverse:
result = np.flip(result, axis=0)
result = np.roll(result, 1, axis=0)
return result
def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
assert (steps % 4) == 0
halfsteps = int(steps/2)
step_angle = np.pi / halfsteps
result = np.zeros((steps, 3))
halfsteps_array = np.arange(halfsteps)
# first half, move backward (only y change)
result[:halfsteps, 1] = y_radius - halfsteps_array*y_radius*2/(halfsteps)
# second half, move forward in semicircle shape (x, y, z change)
angle = np.pi - step_angle*halfsteps_array
result[halfsteps:, 0] = x_radius * np.sin(angle)
result[halfsteps:, 1] = y_radius * np.cos(angle)
result[halfsteps:, 2] = z_radius * np.sin(angle)
result = np.roll(result, int(steps/4), axis=0)
if reverse:
result = np.flip(result, axis=0)
result = np.roll(result, 1, axis=0)
return result
def get_rotate_x_matrix(angle):
angle = angle * np.pi / 180
return np.matrix([
[1, 0, 0, 0],
[0, np.cos(angle), -np.sin(angle), 0],
[0, np.sin(angle), np.cos(angle), 0],
[0, 0, 0, 1],
])
def get_rotate_y_matrix(angle):
angle = angle * np.pi / 180
return np.matrix([
[np.cos(angle), 0, np.sin(angle), 0],
[0, 1, 0, 0],
[-np.sin(angle), 0, np.cos(angle), 0],
[0, 0, 0, 1],
])
def get_rotate_z_matrix(angle):
angle = angle * np.pi / 180
return np.matrix([
[np.cos(angle), -np.sin(angle), 0, 0],
[np.sin(angle), np.cos(angle), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
])
def matrix_mul(m, pt):
ptx = list(pt) + [1]
return list((m * np.matrix(ptx).T).T.flat)[:-1]
def path_rotate_x(path, angle):
ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1)
return ((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T)[:, :-1]
def path_rotate_y(path, angle):
ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1)
return ((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T)[:, :-1]
def path_rotate_z(path, angle):
ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1)
return ((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T)[:, :-1]
if __name__ == '__main__':
pt = [0, 1, 0]

368
software/raspberry pi/path_generator.py Normal file
View File

@ -0,0 +1,368 @@
#!python
#
# 2021 Zhengyu Peng
# Website: https://zpeng.me
#
# ` `
# -:. -#:
# -//:. -###:
# -////:. -#####:
# -/:.://:. -###++##:
# .. `://:- -###+. :##:
# `:/+####+. :##:
# .::::::::/+###. :##:
# .////-----+##: `:###:
# `-//:. :##: `:###/.
# `-//:. :##:`:###/.
# `-//:+######/.
# `-/+####/.
# `+##+.
# :##:
# :##:
# :##:
# :##:
# :##:
# .+:
from lib import semicircle_generator, semicircle2_generator
from lib import path_rotate_z
from lib import get_rotate_x_matrix, get_rotate_y_matrix, get_rotate_z_matrix
import numpy as np
def gen_forward_path(standby_coordinate,
g_steps=20,
g_radius=25):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
semi_circle = semicircle_generator(g_radius, g_steps)
path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
path[:, [1, 3, 5], :] = np.tile(
np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_backward_path(standby_coordinate,
g_steps=20,
g_radius=25):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
semi_circle = semicircle_generator(g_radius, g_steps, reverse=True)
path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
path[:, [1, 3, 5], :] = np.tile(
np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_fastforward_path(standby_coordinate,
g_steps=20,
y_radius=50,
z_radius=30,
x_radius=10):
assert (g_steps % 2) == 0
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
semi_circle_r = semicircle2_generator(
g_steps, y_radius, z_radius, x_radius)
semi_circle_l = semicircle2_generator(
g_steps, y_radius, z_radius, -x_radius)
path[:, [0, 2], :] = np.tile(semi_circle_r[:, np.newaxis, :], (1, 2, 1))
path[:, 1, :] = np.roll(semi_circle_r, halfsteps, axis=0)
path[:, 4, :] = semi_circle_l
path[:, [3, 5], :] = np.tile(
np.roll(semi_circle_l[:, np.newaxis, :], halfsteps, axis=0), (1, 2, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_fastbackward_path(standby_coordinate,
g_steps=20,
y_radius=50,
z_radius=30,
x_radius=10):
assert (g_steps % 2) == 0
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
semi_circle_r = semicircle2_generator(
g_steps, y_radius, z_radius, x_radius, reverse=True)
semi_circle_l = semicircle2_generator(
g_steps, y_radius, z_radius, -x_radius, reverse=True)
path[:, [0, 2], :] = np.tile(semi_circle_r[:, np.newaxis, :], (1, 2, 1))
path[:, 1, :] = np.roll(semi_circle_r, halfsteps, axis=0)
path[:, 4, :] = semi_circle_l
path[:, [3, 5], :] = np.tile(
np.roll(semi_circle_l[:, np.newaxis, :], halfsteps, axis=0), (1, 2, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_leftturn_path(standby_coordinate,
g_steps=20,
g_radius=25):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
semi_circle = semicircle_generator(g_radius, g_steps)
mir_path = np.roll(semi_circle, halfsteps, axis=0)
path[:, 0, :] = path_rotate_z(semi_circle, 45)
path[:, 1, :] = path_rotate_z(mir_path, 0)
path[:, 2, :] = path_rotate_z(semi_circle, 315)
path[:, 3, :] = path_rotate_z(mir_path, 225)
path[:, 4, :] = path_rotate_z(semi_circle, 180)
path[:, 5, :] = path_rotate_z(mir_path, 135)
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_rightturn_path(standby_coordinate,
g_steps=20,
g_radius=25):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
semi_circle = semicircle_generator(g_radius, g_steps)
mir_path = np.roll(semi_circle, halfsteps, axis=0)
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = path_rotate_z(semi_circle, 45+180)
path[:, 1, :] = path_rotate_z(mir_path, 0+180)
path[:, 2, :] = path_rotate_z(semi_circle, 315+180)
path[:, 3, :] = path_rotate_z(mir_path, 225+180)
path[:, 4, :] = path_rotate_z(semi_circle, 180+180)
path[:, 5, :] = path_rotate_z(mir_path, 135+180)
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_shiftleft_path(standby_coordinate,
g_steps=20,
g_radius=25):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
semi_circle = semicircle_generator(g_radius, g_steps)
# shift 90 degree to make the path "left" shift
semi_circle = np.array(path_rotate_z(semi_circle, 90))
mir_path = np.roll(semi_circle, halfsteps, axis=0)
path = np.zeros((g_steps, 6, 3))
path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
path[:, [1, 3, 5], :] = np.tile(mir_path[:, np.newaxis, :], (1, 3, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_shiftright_path(standby_coordinate,
g_steps=20,
g_radius=25):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
semi_circle = semicircle_generator(g_radius, g_steps)
# shift 90 degree to make the path "left" shift
semi_circle = np.array(path_rotate_z(semi_circle, 270))
mir_path = np.roll(semi_circle, halfsteps, axis=0)
path = np.zeros((g_steps, 6, 3))
path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
path[:, [1, 3, 5], :] = np.tile(mir_path[:, np.newaxis, :], (1, 3, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_climb_path(standby_coordinate,
g_steps=20,
y_radius=20,
z_radius=80,
x_radius=30,
z_shift=-30):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
rpath = semicircle2_generator(g_steps, y_radius, z_radius, x_radius)
rpath[:, 2] = rpath[:, 2]+z_shift
lpath = semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)
lpath[:, 2] = lpath[:, 2]+z_shift
mir_rpath = np.roll(rpath, halfsteps, axis=0)
mir_lpath = np.roll(lpath, halfsteps, axis=0)
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = rpath
path[:, 1, :] = mir_rpath
path[:, 2, :] = rpath
path[:, 3, :] = mir_lpath
path[:, 4, :] = lpath
path[:, 5, :] = mir_lpath
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_rotatex_path(standby_coordinate,
g_steps=20,
swing_angle=15,
y_radius=15):
assert (g_steps % 4) == 0
quarter = int(g_steps/4)
path = np.zeros((g_steps, 6, 3))
step_angle = swing_angle / quarter
step_offset = y_radius / quarter
scx = np.append(standby_coordinate, np.ones((6, 1)), axis=1)
for i in range(quarter):
m = get_rotate_x_matrix(swing_angle - i*step_angle)
m[1, 3] = -i * step_offset
path[i, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
for i in range(quarter):
m = get_rotate_x_matrix(-i*step_angle)
m[1, 3] = -y_radius + i * step_offset
path[i+quarter, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
for i in range(quarter):
m = get_rotate_x_matrix(i*step_angle-swing_angle)
m[1, 3] = i * step_offset
path[i+quarter*2, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
for i in range(quarter):
m = get_rotate_x_matrix(i*step_angle)
m[1, 3] = y_radius-i * step_offset
path[i+quarter*3, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
return path
def gen_rotatey_path(standby_coordinate,
g_steps=20,
swing_angle=15,
x_radius=15):
assert (g_steps % 4) == 0
quarter = int(g_steps/4)
path = np.zeros((g_steps, 6, 3))
step_angle = swing_angle / quarter
step_offset = x_radius / quarter
scx = np.append(standby_coordinate, np.ones((6, 1)), axis=1)
for i in range(quarter):
m = get_rotate_y_matrix(swing_angle - i*step_angle)
m[1, 3] = -i * step_offset
path[i, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
for i in range(quarter):
m = get_rotate_y_matrix(-i*step_angle)
m[1, 3] = -x_radius + i * step_offset
path[i+quarter, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
for i in range(quarter):
m = get_rotate_y_matrix(i*step_angle-swing_angle)
m[1, 3] = i * step_offset
path[i+quarter*2, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
for i in range(quarter):
m = get_rotate_y_matrix(i*step_angle)
m[1, 3] = x_radius-i * step_offset
path[i+quarter*3, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
return path
def gen_rotatez_path(standby_coordinate,
g_steps=20,
z_lift=4.5,
xy_radius=1):
assert (g_steps % 4) == 0
path = np.zeros((g_steps, 6, 3))
step_angle = 2*np.pi / g_steps
scx = np.append(standby_coordinate, np.ones((6, 1)), axis=1)
for i in range(g_steps):
x = xy_radius * np.cos(i*step_angle)
y = xy_radius * np.sin(i*step_angle)
m = get_rotate_y_matrix(np.arctan2(x, z_lift)*180/np.pi) * \
get_rotate_x_matrix(np.arctan2(y, z_lift)*180/np.pi)
path[i, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]
return path
def gen_twist_path(standby_coordinate,
g_steps=20,
raise_angle=3,
twist_x_angle=20,
twise_y_angle=12):
assert (g_steps % 4) == 0
quarter = int(g_steps / 4)
step_x_angle = twist_x_angle / quarter
step_y_angle = twise_y_angle / quarter
scx = np.append(standby_coordinate, np.ones((6, 1)), axis=1)
m = get_rotate_x_matrix(raise_angle)
path = np.zeros((g_steps, 6, 3))
for i in range(quarter):
temp = m * get_rotate_z_matrix(i*step_x_angle) * \
get_rotate_x_matrix(i*step_y_angle)
path[i, :, :] = ((np.matmul(temp, scx.T)).T)[:, :-1]
for i in range(quarter):
temp = m * get_rotate_z_matrix((quarter-i)*step_x_angle) * \
get_rotate_x_matrix((quarter-i)*step_y_angle)
path[i+quarter*1, :, :] = ((np.matmul(temp, scx.T)).T)[:, :-1]
for i in range(quarter):
temp = m * get_rotate_z_matrix(-i*step_x_angle) * \
get_rotate_x_matrix(i*step_y_angle)
path[i+quarter*2, :, :] = ((np.matmul(temp, scx.T)).T)[:, :-1]
for i in range(quarter):
temp = m * get_rotate_z_matrix((-quarter+i)*step_x_angle) * \
get_rotate_x_matrix((quarter-i)*step_y_angle)
path[i+quarter*3, :, :] = ((np.matmul(temp, scx.T)).T)[:, :-1]
return path

96
software/raspberry pi/tcpserver.py Normal file
View File

@ -0,0 +1,96 @@
#!python
#
# 2021 Zhengyu Peng
# Website: https://zpeng.me
#
# ` `
# -:. -#:
# -//:. -###:
# -////:. -#####:
# -/:.://:. -###++##:
# .. `://:- -###+. :##:
# `:/+####+. :##:
# .::::::::/+###. :##:
# .////-----+##: `:###:
# `-//:. :##: `:###/.
# `-//:. :##:`:###/.
# `-//:+######/.
# `-/+####/.
# `+##+.
# :##:
# :##:
# :##:
# :##:
# :##:
# .+:
import socket
from threading import Thread
import json
class TCPServer(Thread):
ERROR = -1
LISTEN = 1
CONNECTED = 2
STOP = 3
SIG_NORMAL = 0
SIG_STOP = 1
SIG_DISCONNECT = 2
def __init__(self, out_cmd_queue):
Thread.__init__(self)
self.cmd_queue = out_cmd_queue
with open('./config.json', 'r') as read_file:
self.config = json.load(read_file)
self.ip = '192.168.1.127'
self.port = 1234
self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.signal = self.SIG_NORMAL
def run(self):
try:
self.tcp_socket.bind((self.ip, self.port))
self.tcp_socket.listen(1)
print('TCP listening')
except OSError as err:
# print('emit tcp server error')
# self.status.emit(self.STOP, '')
pass
else:
while True:
# Wait for a connection
# print('wait for a connection')
# self.status.emit(self.LISTEN, '')
try:
self.connection, addr = self.tcp_socket.accept()
# self.connection.setblocking(False)
# self.connection.settimeout(1)
print('New connection')
except socket.timeout as t_out:
pass
else:
while True:
# print('waiting for data')
# if self.signal == self.SIG_NORMAL:
try:
data = self.connection.recv(4096)
except socket.error as e:
print(e)
break
else:
if data:
self.cmd_queue.put(data.decode())
else:
self.cmd_queue.put('standby')
break
finally:
self.tcp_socket.close()
self.cmd_queue.put('standby')
print('exit')