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

clean

Browse Source
This commit is contained in:
Zhengyu Peng
2021-12-11 10:09:53 -05:00
parent c53d0ee4df
commit c3dc17bca0
54 changed files with 1 additions and 340 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
software/pathTool/src/kinematics.py
View File

@@ -1,25 +0,0 @@
import math
import config
pi = math.acos(-1)
def ik(to):
angles = []
x = to[0] - config.kLegRootToJoint1
y = to[1]
angles.append(math.atan2(y, x) * 180 / pi)
x = math.sqrt(x*x + y*y) - config.kLegJoint1ToJoint2
y = to[2]
ar = math.atan2(y, x)
lr2 = x*x + y*y
lr = math.sqrt(lr2)
a1 = math.acos((lr2 + config.kLegJoint2ToJoint3*config.kLegJoint2ToJoint3 - config.kLegJoint3ToTip*config.kLegJoint3ToTip)/(2*config.kLegJoint2ToJoint3*lr))
a2 = math.acos((lr2 - config.kLegJoint2ToJoint3*config.kLegJoint2ToJoint3 + config.kLegJoint3ToTip*config.kLegJoint3ToTip)/(2*config.kLegJoint3ToTip*lr))
angles.append((ar + a1) * 180 / pi)
angles.append(90 - ((a1 + a2) * 180 / pi))
return angles