From 716b6699f3db54fb2858a7b4f7a3f4ea499a3f81 Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Thu, 9 Jun 2022 15:15:56 +0200 Subject: [PATCH] add inverse kinematics with joint constraints --- tests/pantilt.py | 20 +++- tests/pantilt_inv.py | 226 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 241 insertions(+), 5 deletions(-) create mode 100644 tests/pantilt_inv.py diff --git a/tests/pantilt.py b/tests/pantilt.py index a50ea4e..63eec7c 100644 --- a/tests/pantilt.py +++ b/tests/pantilt.py @@ -1,4 +1,3 @@ - import rtde_control import time import math @@ -29,7 +28,8 @@ def toRad(d): actual_q = rtde_r.getActualQ() #current joint rotations in radians. print("Actual Q:"+str(actual_q)) -fw_actual_q = rtde_c.getForwardKinematics(actual_q) +#fw_actual_q = rtde_c.getForwardKinematics(actual_q) +fw_actual_q = rtde_c.getForwardKinematics() print("forward Actual Q:"+str(fw_actual_q)) joint_q=rtde_c.getInverseKinematics(fw_actual_q) print("Final Q:"+str(joint_q)) @@ -41,6 +41,7 @@ tilt=0 #-=down, +=up, degrees joint_q=[0,toRad(-90),toRad(90),toRad(-180),toRad(-90),toRad(0)] #start position + #rtde_c.moveJ([0,toRad(-90),toRad(90-tilt/2),toRad(-180-tilt/2),toRad(-90-pan),toRad(0)],0.1,0.1) rtde_c.moveJ(joint_q,0.1,0.1) #move to initial position @@ -62,8 +63,11 @@ speed=1/frequency def constrain(v,_min,_max): return min(_max,max(_min,v)) -print("Press s to start!") +print("Press s to start") +print("Press q to stop") while not keyboard.is_pressed("s"): + if keyboard.is_pressed("q"): + exit() time.sleep(0.1) print("Starting Servo") @@ -107,15 +111,21 @@ while not keyboard.is_pressed("q"): # Robot Control stuff + 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 + reachable=rtde_c.isPoseWithinSafetyLimits(rtde_c.getForwardKinematics(joint_q_aim)) + if not reachable: + print("Reachable:"+str(reachable)) - for i in range(6): - joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed,speed) + + if reachable: + for i in range(6): + joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed,speed) #print(joint_q) diff --git a/tests/pantilt_inv.py b/tests/pantilt_inv.py new file mode 100644 index 0000000..91b7161 --- /dev/null +++ b/tests/pantilt_inv.py @@ -0,0 +1,226 @@ +import rtde_control +import time +import math +import socket +import keyboard # pip install keyboard +import numpy as np + +# Socket stuff +s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +host ="" +port =30002 +s.bind((host,port)) +s.listen(1) # Number of connections +s.setblocking(False) +client = None + + +# Robot Control Stuff + +rtde_c = rtde_control.RTDEControlInterface("192.168.1.101") + +import rtde_receive +rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.101") + +def toRad(d): + return d/360*2*math.pi + +def toDeg(r): + return r*360/2/math.pi + + +def RPYtoVec(roll,pitch,yaw): + yawMatrix = np.matrix([ + [math.cos(yaw), -math.sin(yaw), 0], + [math.sin(yaw), math.cos(yaw), 0], + [0, 0, 1] + ]) + + pitchMatrix = np.matrix([ + [math.cos(pitch), 0, math.sin(pitch)], + [0, 1, 0], + [-math.sin(pitch), 0, math.cos(pitch)] + ]) + + rollMatrix = np.matrix([ + [1, 0, 0], + [0, math.cos(roll), -math.sin(roll)], + [0, math.sin(roll), math.cos(roll)] + ]) + + R = yawMatrix * pitchMatrix * rollMatrix + + theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1) / 2) + multi = 1 / (2 * math.sin(theta)) + + rx = multi * (R[2, 1] - R[1, 2]) * theta + ry = multi * (R[0, 2] - R[2, 0]) * theta + rz = multi * (R[1, 0] - R[0, 1]) * theta + + return rx, ry, rz + +paused=False + +actual_q = rtde_r.getActualQ() #current joint rotations in radians. +print("Actual Q:"+str(actual_q)) +#fw_actual_q = rtde_c.getForwardKinematics(actual_q) +fw_actual_q = rtde_c.getForwardKinematics() +print("forward Actual Q:"+str(fw_actual_q)) +joint_q=rtde_c.getInverseKinematics(fw_actual_q) +print("Final Q:"+str(joint_q)) + + +centerpos=[-0.895,-0.177,0.91] +centerrotRPY=[-1.572,0,1.572] #roll, pitch, yaw + +pan=0 #-=left, +=right, degrees +tilt=0 #-=down, +=up, degrees + +joint_q_init=[0,toRad(-90),toRad(90),toRad(-180),toRad(-90),toRad(0)] #start position +joint_q=joint_q_init + +''' +_rot=RPYtoVec(centerrotRPY[0],centerrotRPY[1],centerrotRPY[2]) +pos_q=[centerpos[0],centerpos[1],centerpos[2],_rot[0],_rot[1],_rot[2]] + +joint_q2=rtde_c.getInverseKinematics(pos_q,joint_q_init) +print("Ninal Q:"+str(joint_q2)) +''' + +#rtde_c.moveJ([0,toRad(-90),toRad(90-tilt/2),toRad(-180-tilt/2),toRad(-90-pan),toRad(0)],0.1,0.1) + +rtde_c.moveJ(joint_q,0.2,0.1) #move to initial position + + +# Parameters + + +velocity = 0.2 #0.5 +acceleration = 0.1 #0.5 +frequency= 100 +dt = 1.0/frequency # 2ms +lookahead_time = 0.1 +gain = 300 + +speed=[1,1,1,3,3,3] + + + +def constrain(v,_min,_max): + return min(_max,max(_min,v)) + +print("Press s to start") +print("Press q to stop") +print("Press p to pause") +while not keyboard.is_pressed("s"): + if keyboard.is_pressed("q"): + exit() + time.sleep(0.1) + +print("Starting Servo") +while not keyboard.is_pressed("q"): + + start = time.time() + + # Receive Socket stuff + try: + client, address = s.accept() + print("Connected to", address) + + except socket.error: + pass + + if client is not None: + try: + data = client.recv( 1024 ).decode( 'utf-8' ) + if data: + lastdata=data.split('\n') + #print("Received :", repr(data)) + if (len(lastdata)>1): + splitdata=lastdata[-2].split(",") + + try: + _pan=float(splitdata[0]) + _tilt=float(splitdata[1]) + + winkellimit=45 + if _pan>=-winkellimit and _pan <=winkellimit and _tilt>=-winkellimit and _tilt <=winkellimit: + pan=_pan + tilt=_tilt + + print("Pan="+str(pan)+" Tilt="+str(tilt)) + except ValueError: + print("Not a float") + except socket.error: + pass + + + # Robot Control stuff + _rot=RPYtoVec(centerrotRPY[0]+toRad(tilt),centerrotRPY[1],centerrotRPY[2]+toRad(-pan)) + pos_q=[centerpos[0],centerpos[1],centerpos[2],_rot[0],_rot[1],_rot[2]] + joint_q_aim=rtde_c.getInverseKinematics(pos_q,joint_q_init) + #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 + + reachable=rtde_c.isPoseWithinSafetyLimits(pos_q) + if not reachable: + print("Reachable:"+str(reachable)) + + insideJointConstraints=True + if joint_q_aim[0]toRad(20): + print("Joint 0 Outside constraint with "+str(joint_q_aim[0])+" ("+str(toDeg(joint_q_aim[0]))+")") + insideJointConstraints=False + if joint_q_aim[1]toRad(-90+40): + print("Joint 1 Outside constraint with "+str(joint_q_aim[1])+" ("+str(toDeg(joint_q_aim[1]))+")") + insideJointConstraints=False + if joint_q_aim[2]toRad(90+40): + print("Joint 2 Outside constraint with "+str(joint_q_aim[2])+" ("+str(toDeg(joint_q_aim[2]))+")") + insideJointConstraints=False + if joint_q_aim[3]toRad(-180+110): + print("Joint 3 Outside constraint with "+str(joint_q_aim[3])+" ("+str(toDeg(joint_q_aim[3]))+")") + insideJointConstraints=False + if joint_q_aim[4]toRad(-90+70): + print("Joint 4 Outside constraint with "+str(joint_q_aim[4])+" ("+str(toDeg(joint_q_aim[4]))+")") + insideJointConstraints=False + if joint_q_aim[5]toRad(0+60): + print("Joint 5 Outside constraint with "+str(joint_q_aim[5])+" ("+str(toDeg(joint_q_aim[5]))+")") + insideJointConstraints=False + + + if reachable and insideJointConstraints and not paused: + for i in range(6): + joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed[i]/frequency,speed[i]/frequency) #constrain joint speeds + + + + if keyboard.is_pressed("p"): + print("paused") + paused=True + if keyboard.is_pressed("s"): + print("resumed") + paused=False + + #print(joint_q) + + rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain) + + end = time.time() + stepduration = end - start + + if stepduration < dt: + time.sleep(dt - stepduration) + +print("Finished") + +rtde_c.servoStop() +rtde_c.stopScript() + + + + + +s.close() + +