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

update

Browse Source
This commit is contained in:
Zhengyu Peng 2021-12-08 20:20:13 -05:00
parent d7b347c922
commit 0dcc55d09f
2 changed files with 33 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
software/rpi/config.json
View File

@ -2,9 +2,6 @@
"legNames":["front_right", "center_right", "rear_right", "rear_left", "center_left", "front_left"], "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], "legMountX":[29.41, 36.87, 29.41, -29.41, -36.87, -29.41],
"legMountY":[55.41, 0, -55.41, -55.41, 0, 55.41], "legMountY":[55.41, 0, -55.41, -55.41, 0, 55.41],
"legMountLeftRightX":36.87,
"legMountOtherX":29.41,
"legMountOtherY":55.41,
"legRootToJoint1":20.75, "legRootToJoint1":20.75,
"legJoint1ToJoint2":28.0, "legJoint1ToJoint2":28.0,
"legJoint2ToJoint3":42.6, "legJoint2ToJoint3":42.6,

51
software/rpi/hexapod.py
View File

@ -23,10 +23,28 @@ COS15 = 0.9659
class Hexapod: class Hexapod:
def __init__(self): 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: with open('./config.json', 'r') as read_file:
self.config = json.load(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 # Objects
self.pca_right = ServoKit(channels=16, address=0x40, frequency=120) self.pca_right = ServoKit(channels=16, address=0x40, frequency=120)
self.pca_left = ServoKit(channels=16, address=0x41, frequency=120) self.pca_left = ServoKit(channels=16, address=0x41, frequency=120)
@ -69,13 +87,17 @@ class Hexapod:
# self.leg_4.reset() # self.leg_4.reset()
# self.leg_5.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() self.standby()
def standby(self): 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(0, 90)
self.leg_0.set_angle(1, 60) self.leg_0.set_angle(1, 60)
self.leg_0.set_angle(2, 75) 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(0, 90)
self.leg_5.set_angle(1, 60) self.leg_5.set_angle(1, 60)
self.leg_5.set_angle(2, 75) 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): def ik(self, to):
to = to-self.mount_position to = to-self.mount_position
self.angles = np.zeros((6, 3)) self.angles = np.zeros((6, 3))
x = to[:, 0] - self.config['legRootToJoint1'] x = to[:, 0] - self.root_j1
y = to[:, 1] y = to[:, 1]
self.angles[:, 0] = (np.arctan2(y, x) * 180 / np.pi) 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] y = to[:, 2]
ar = np.arctan2(y, x) ar = np.arctan2(y, x)
lr2 = x*x + y*y lr2 = x*x + y*y
lr = np.sqrt(lr2) lr = np.sqrt(lr2)
a1 = np.arccos((lr2 + self.config['legJoint2ToJoint3']*self.config['legJoint2ToJoint3'] - a1 = np.arccos((lr2 + self.j2_j3*self.j2_j3 -
self.config['legJoint3ToTip']*self.config['legJoint3ToTip'])/(2*self.config['legJoint2ToJoint3']*lr)) self.j3_tip*self.j3_tip)/(2*self.j2_j3*lr))
a2 = np.arccos((lr2 - self.config['legJoint2ToJoint3']*self.config['legJoint2ToJoint3'] + a2 = np.arccos((lr2 - self.j2_j3*self.j2_j3 +
self.config['legJoint3ToTip']*self.config['legJoint3ToTip'])/(2*self.config['legJoint3ToTip']*lr)) self.j3_tip*self.j3_tip)/(2*self.j3_tip*lr))
self.angles[:, 1] = ((ar + a1) * 180 / np.pi) self.angles[:, 1] = ((ar + a1) * 180 / np.pi)
self.angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi)) self.angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))