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

rename folders

Browse Source
This commit is contained in:
Zhengyu Peng
2022-01-06 11:25:06 -05:00
parent a31398347b
commit 143671fdb4
26 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
software/path tool/src/kinematics.py Normal file
View File

@ -0,0 +1,25 @@
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