philipp.kramer/urcontrol
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

add inverse kinematics code, not finished

Browse source
This commit is contained in:
Philipp Kramer 2022-05-31 16:45:36 +02:00
parent 621a353140
commit ccb33d819a
1 changed files with 6 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
tests/pantilt.py
View file

@ -26,14 +26,14 @@ def toRad(d):
return d/360*2*math.pi return d/360*2*math.pi
'''
actual_q = rtde_r.getActualQ() #current joint rotations in radians. actual_q = rtde_r.getActualQ() #current joint rotations in radians.
print("Actual Q:"+str(actual_q)) print("Actual Q:"+str(actual_q))
fw_actual_q = rtde_c.getForwardKinematics(actual_q) fw_actual_q = rtde_c.getForwardKinematics(actual_q)
print("forward Actual Q:"+str(fw_actual_q)) print("forward Actual Q:"+str(fw_actual_q))
joint_q=rtde_c.getInverseKinematics(fw_actual_q) joint_q=rtde_c.getInverseKinematics(fw_actual_q)
print("Final Q:"+str(joint_q)) print("Final Q:"+str(joint_q))
'''
pan=0 #-=left, +=right, degrees pan=0 #-=left, +=right, degrees
@ -108,6 +108,10 @@ while not keyboard.is_pressed("q"):
joint_q_aim = [0,toRad(-90),toRad(90),toRad(-180-tilt),toRad(-90-pan),toRad(0)] joint_q_aim = [0,toRad(-90),toRad(90),toRad(-180-tilt),toRad(-90-pan),toRad(0)]
#joint_q_aim = rtde_c.getInverseKinematics([-0.895,-0.173,0.911,2.4,2.4,2.4])
# inverse kinematics: rx+ =look up
# ry+ = look right
# rz+ = rotate CW
for i in range(6): for i in range(6):