From 0dcc55d09f3f20518f15af48190ea3a60e4742c1 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Wed, 8 Dec 2021 20:20:13 -0500 Subject: [PATCH] update --- software/rpi/config.json | 3 --- software/rpi/hexapod.py | 51 ++++++++++++++++++++++++++-------------- 2 files changed, 33 insertions(+), 21 deletions(-) diff --git a/software/rpi/config.json b/software/rpi/config.json index 985d780..4bb725c 100644 --- a/software/rpi/config.json +++ b/software/rpi/config.json @@ -2,9 +2,6 @@ "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], - "legMountLeftRightX":36.87, - "legMountOtherX":29.41, - "legMountOtherY":55.41, "legRootToJoint1":20.75, "legJoint1ToJoint2":28.0, "legJoint2ToJoint3":42.6, diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index 86c034e..021a1a7 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -23,10 +23,28 @@ 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_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) @@ -69,13 +87,17 @@ class Hexapod: # self.leg_4.reset() # self.leg_5.reset() - self.mount_position = np.zeros((6, 3)) - self.mount_position[:, 0] = np.array(self.config['legMountX']) - self.mount_position[:, 1] = np.array(self.config['legMountY']) - self.standby() def standby(self): + self.standby_coordinate = np.zeros((6, 3)) + self.standby_coordinate[:, 2] = self.j2_j3 * \ + SIN30 - self.j3_tip * COS15 + self.standby_coordinate[:, 0] = np.array(self.mount_x)+(self.root_j1+self.j1_j2+( + self.j2_j3*COS30)+self.j3_tip*SIN15)*1 + self.standby_coordinate[:, 1] = self.mount_y + (self.root_j1+self.j1_j2+( + self.j2_j3*COS30)+self.j3_tip*SIN15)*0 + self.leg_0.set_angle(0, 90) self.leg_0.set_angle(1, 60) self.leg_0.set_angle(2, 75) @@ -99,33 +121,26 @@ class Hexapod: self.leg_5.set_angle(0, 90) self.leg_5.set_angle(1, 60) self.leg_5.set_angle(2, 75) - # self.standby_coordinate = np.zeros((6, 3)) - # self.standby_coordinate[:, 2] = self.config['legJoint2ToJoint3'] * \ - # SIN30 - self.config['legJoint3ToTip'] * COS15 - # self.standby_coordinate[:, 0] = np.array(self.config['legMountX'])+(self.config['legRootToJoint1']+self.config['legJoint1ToJoint2']+( - # self.config['legJoint2ToJoint3']*COS30)+self.config['legJoint3ToTip']*SIN15)*1 - # self.standby_coordinate[:, 1] = np.array(self.config['legMountY']) + (self.config['legRootToJoint1']+self.config['legJoint1ToJoint2']+( - # self.config['legJoint2ToJoint3']*COS30)+self.config['legJoint3ToTip']*SIN15)*0 - # self.ik(self.standby_coordinate) def ik(self, to): to = to-self.mount_position + self.angles = np.zeros((6, 3)) - x = to[:, 0] - self.config['legRootToJoint1'] + x = to[:, 0] - self.root_j1 y = to[:, 1] self.angles[:, 0] = (np.arctan2(y, x) * 180 / np.pi) - x = np.sqrt(x*x + y*y) - self.config['legJoint1ToJoint2'] + 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.config['legJoint2ToJoint3']*self.config['legJoint2ToJoint3'] - - self.config['legJoint3ToTip']*self.config['legJoint3ToTip'])/(2*self.config['legJoint2ToJoint3']*lr)) - a2 = np.arccos((lr2 - self.config['legJoint2ToJoint3']*self.config['legJoint2ToJoint3'] + - self.config['legJoint3ToTip']*self.config['legJoint3ToTip'])/(2*self.config['legJoint3ToTip']*lr)) + 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] = ((ar + a1) * 180 / np.pi) self.angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))