150 lines
No EOL
3.4 KiB
Python
150 lines
No EOL
3.4 KiB
Python
import rtde_control
|
|
import time
|
|
import math
|
|
import socket
|
|
import keyboard # pip install keyboard
|
|
|
|
# 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
|
|
|
|
|
|
|
|
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))
|
|
|
|
|
|
|
|
pan=0 #-=left, +=right, degrees
|
|
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
|
|
|
|
|
|
# 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/frequency
|
|
|
|
|
|
def constrain(v,_min,_max):
|
|
return min(_max,max(_min,v))
|
|
|
|
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")
|
|
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
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
#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() |