Update hexapod.py

master
Zhengyu Peng 2 years ago
parent 7033e4bdf3
commit b25d81e28d

@ -188,9 +188,11 @@ class Hexapod(Thread):
posture = np.zeros((6, 3))
posture[:, 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)
self.j2_j3*np.sin(j2_rad))+self.j3_tip*np.cos(j3_rad)) *\
np.cos(self.mount_angle)
posture[:, 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)
self.j2_j3*np.sin(j2_rad))+self.j3_tip*np.cos(j3_rad)) *\
np.sin(self.mount_angle)
posture[:, 2] = self.j2_j3 * \
np.cos(j2_rad) - self.j3_tip * \
np.sin(j3_rad)

Loading…
Cancel
Save