add inverse kinematics with joint constraints

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,13 +111,19 @@ 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))
 
+
+    if reachable:
         for i in range(6):
             joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed,speed)
         

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) or 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) or 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-90) or 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-80) or 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) or 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) or 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()
+
+