82 lines
2.6 KiB
Python
82 lines
2.6 KiB
Python
|
from helpfunctions import *
|
||
|
|
import time
|
||
|
|
|
||
|
|
class UR:
|
||
|
|
|
||
|
|
import rtde_receive
|
||
|
|
import rtde_control
|
||
|
|
|
||
|
|
def __init__(self,_joint_min,_joint_max,_frequency_rtde_send=500,_frequency_rtde_receive=500):
|
||
|
|
self.frequency_rtde_send=_frequency_rtde_send
|
||
|
|
self.frequency_rtde_receive=_frequency_rtde_receive
|
||
|
|
|
||
|
|
|
||
|
|
self.last_receivertde=0
|
||
|
|
self.last_sendrtde=0
|
||
|
|
|
||
|
|
self.joint_min=_joint_min
|
||
|
|
self.joint_max=_joint_max
|
||
|
|
|
||
|
|
self.rtde_r=None
|
||
|
|
self.rtde_c=None
|
||
|
|
|
||
|
|
|
||
|
|
# Parameters
|
||
|
|
self.velocity = 0.1
|
||
|
|
self.acceleration = 0.1
|
||
|
|
self.lookahead_time = 0.1
|
||
|
|
self.gain = 300
|
||
|
|
self.joint_q_aim=[0,0,0,0,0,0] #joint rotations aiming for
|
||
|
|
self.joint_q = [0,0,0,0,0,0] #joint rotation to be send
|
||
|
|
self.velocitylimit=[1,1,1,3,3,3] #joint velocity limits
|
||
|
|
|
||
|
|
|
||
|
|
self.joint_q_received=[0,0,0,0,0,0]
|
||
|
|
|
||
|
|
def connect(self,robotip):
|
||
|
|
|
||
|
|
rtde_r = self.rtde_receive.RTDEReceiveInterface(robotip, self.frequency_rtde_receive)
|
||
|
|
|
||
|
|
rtde_c = self.rtde_control.RTDEControlInterface(robotip)
|
||
|
|
|
||
|
|
def setJointQAim(self,q):
|
||
|
|
self.joint_q_aim=q
|
||
|
|
|
||
|
|
|
||
|
|
def send(self,looptime):
|
||
|
|
|
||
|
|
if looptime-self.last_sendrtde<1.0/self.frequency_rtde_send:
|
||
|
|
return
|
||
|
|
self.last_sendrtde=looptime
|
||
|
|
|
||
|
|
for i in range(6): #update joint_q with max speeds
|
||
|
|
self.joint_q[i] += constrain(self.joint_q_aim[i]-self.joint_q[i], -self.velocitylimit[i]/self.frequency_rtde_send,self.velocitylimit[i]/self.frequency_rtde_send) #constrain joint speeds
|
||
|
|
|
||
|
|
if self.rtde_c is not None:
|
||
|
|
self.rtde_c.servoJ(self.joint_q, self.velocity, self.acceleration, 1.0/self.frequency_rtde_send, self.lookahead_time, self.gain)
|
||
|
|
|
||
|
|
def receive(self,looptime):
|
||
|
|
|
||
|
|
#Get robot joint angles
|
||
|
|
if looptime-self.last_receivertde>=1.0/self.frequency_rtde_receive:
|
||
|
|
self.last_receivertde=looptime
|
||
|
|
|
||
|
|
if self.rtde_r is not None:
|
||
|
|
self.joint_q_received=self.rtde_r.getActualQ()
|
||
|
|
else:
|
||
|
|
_offsets=[12.2,5.3,0,6,12.4,612.91]
|
||
|
|
_factors=[1.2,1.14,1.214,0.9921,1,1.092]
|
||
|
|
|
||
|
|
for _i,_offset in enumerate(_offsets): #generate mockup values
|
||
|
|
self.joint_q_received[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(self.joint_max[_i]-self.joint_min[_i])+self.joint_min[_i]
|
||
|
|
|
||
|
|
def getReceivedQ(self):
|
||
|
|
return self.joint_q_received
|
||
|
|
|
||
|
|
def disconnect(self):
|
||
|
|
if self.rtde_c is not None:
|
||
|
|
self.rtde_c.servoStop()
|
||
|
|
self.rtde_c.stopScript()
|
||
|
|
|
||
|
|
|