From ccb33d819a371df13d70aa5e12e07b2d10fb3587 Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Tue, 31 May 2022 16:45:36 +0200 Subject: [PATCH] add inverse kinematics code, not finished --- tests/pantilt.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/tests/pantilt.py b/tests/pantilt.py index 8090125..a50ea4e 100644 --- a/tests/pantilt.py +++ b/tests/pantilt.py @@ -26,14 +26,14 @@ def toRad(d): return d/360*2*math.pi -''' + actual_q = rtde_r.getActualQ() #current joint rotations in radians. print("Actual Q:"+str(actual_q)) fw_actual_q = rtde_c.getForwardKinematics(actual_q) print("forward Actual Q:"+str(fw_actual_q)) joint_q=rtde_c.getInverseKinematics(fw_actual_q) print("Final Q:"+str(joint_q)) -''' + 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 = 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):