#!/usr/bin/env python # -*- coding: utf-8 -*- # Libraries # https://circuitpython.readthedocs.io/projects/servokit/en/latest/ from adafruit_servokit import ServoKit from leg import Leg # python3-numpy import numpy as np import time import json from collections import deque from lib import semicircle_generator SIN30 = 0.5 COS30 = 0.866 SIN45 = 0.7071 COS45 = 0.7071 SIN15 = 0.2588 COS15 = 0.9659 class Hexapod: def __init__(self): # 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=[-6, 4, -6]) # center right self.leg_1 = Leg(1, [self.pca_left.servo[7], self.pca_left.servo[8], self.pca_left.servo[6]], correction=[3, -5, -6]) # rear right self.leg_2 = Leg(2, [self.pca_left.servo[0], self.pca_left.servo[14], self.pca_left.servo[13]], correction=[3, -6, -5]) # rear left self.leg_3 = Leg(3, [self.pca_right.servo[15], self.pca_right.servo[1], self.pca_right.servo[2]], correction=[-3, -4, 6]) # 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, 0]) # front left self.leg_5 = Leg(5, [self.pca_right.servo[0], self.pca_right.servo[13], self.pca_right.servo[14]], correction=[-6, 4, 0]) # self.leg_0.reset() # self.leg_1.reset() # self.leg_2.reset() # self.leg_3.reset() # self.leg_4.reset() # self.leg_5.reset() self.standby_coordinate = np.zeros((6, 3)) self.standby() time.sleep(0.1) # self.ik(self.standby_coordinate) full_path = self.path_generator() for mm in range(0, 30): for idx in range(0, 20): move_to = np.array([full_path[0][0][idx], full_path[0][1][idx], full_path[0][2][idx], full_path[0][3][idx], full_path[0][4][idx], full_path[0][5][idx]])+self.standby_coordinate self.ik(move_to) self.leg_0.set_angle(0, self.angles[0,0]) self.leg_0.set_angle(1, self.angles[0,1]) self.leg_0.set_angle(2, self.angles[0,2]) self.leg_1.set_angle(0, self.angles[1,0]) self.leg_1.set_angle(1, self.angles[1,1]) self.leg_1.set_angle(2, self.angles[1,2]) self.leg_2.set_angle(0, self.angles[2,0]) self.leg_2.set_angle(1, self.angles[2,1]) self.leg_2.set_angle(2, self.angles[2,2]) self.leg_3.set_angle(0, self.angles[3,0]) self.leg_3.set_angle(1, self.angles[3,1]) self.leg_3.set_angle(2, self.angles[3,2]) self.leg_4.set_angle(0, self.angles[4,0]) self.leg_4.set_angle(1, self.angles[4,1]) self.leg_4.set_angle(2, self.angles[4,2]) self.leg_5.set_angle(0, self.angles[5,0]) self.leg_5.set_angle(1, self.angles[5,1]) self.leg_5.set_angle(2, self.angles[5,2]) time.sleep(0.005) print(np.shape(move_to)) # print(np.array([full_path[0][0][0], full_path[0][1][0], full_path[0][2][0], full_path[0][3][0], full_path[0][4][0], full_path[0][5][0]])) print(self.angles) def standby(self): self.standby_coordinate[:, 0] = np.array(self.mount_x)+(self.root_j1+self.j1_j2+( self.j2_j3*COS30)+self.j3_tip*SIN15)*np.cos(self.mount_angle) self.standby_coordinate[:, 1] = self.mount_y + (self.root_j1+self.j1_j2+( self.j2_j3*COS30)+self.j3_tip*SIN15)*np.sin(self.mount_angle) self.standby_coordinate[:, 2] = self.j2_j3 * \ SIN30 - self.j3_tip * COS15 self.leg_0.set_angle(0, 90) self.leg_0.set_angle(1, 60) self.leg_0.set_angle(2, 75) self.leg_1.set_angle(0, 90) self.leg_1.set_angle(1, 60) self.leg_1.set_angle(2, 75) self.leg_2.set_angle(0, 90) self.leg_2.set_angle(1, 60) self.leg_2.set_angle(2, 75) self.leg_3.set_angle(0, 90) self.leg_3.set_angle(1, 60) self.leg_3.set_angle(2, 75) self.leg_4.set_angle(0, 90) self.leg_4.set_angle(1, 60) self.leg_4.set_angle(2, 75) self.leg_5.set_angle(0, 90) self.leg_5.set_angle(1, 60) self.leg_5.set_angle(2, 75) def ik(self, to): temp_to = to-self.mount_position to[:, 0] = temp_to[:, 0] * \ np.cos(self.mount_angle) + temp_to[:, 1] * np.sin(self.mount_angle) to[:, 1] = temp_to[:, 0] * \ np.sin(self.mount_angle) - temp_to[:, 1] * np.cos(self.mount_angle) to[:, 2] = temp_to[:, 2] self.angles = np.zeros((6, 3)) x = to[:, 0] - self.root_j1 y = to[:, 1] self.angles[:, 0] = -(np.arctan2(y, x) * 180 / np.pi)+90 x = np.sqrt(x*x + y*y) - self.j1_j2 y = to[:, 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)) self.angles[:, 1] = 90-((ar + a1) * 180 / np.pi) self.angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))+90 def path_generator(self): # assert (g_steps % 4) == 0 g_steps = 20 g_radius = 25 halfsteps = int(g_steps/2) path = semicircle_generator(g_radius, g_steps) mir_path = deque(path) mir_path.rotate(halfsteps) return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps) def main(): hexapod = Hexapod() if __name__ == '__main__': main()