From ef6b6ed6f756277759f5c726e2dff4dd1cd2b6f2 Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Wed, 28 Mar 2012 15:31:57 -0700
Subject: [PATCH] Servoing in a circle with on-robot code
---
ur5_driver/driver.py | 68 ++++++++++++++++++++++---------
ur5_driver/prog | 96 +++++++++++++++++++++++++++++++++++++++++---
2 files changed, 140 insertions(+), 24 deletions(-)
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index fcfb88c..1afed55 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -1,6 +1,6 @@
#!/usr/bin/env python
import roslib; roslib.load_manifest('ur5_driver')
-import time, sys, threading
+import time, sys, threading, math
import datetime
import socket
import struct
@@ -22,6 +22,7 @@ MSG_JOINT_STATES = 3
MSG_MOVEJ = 4
MSG_WAYPOINT_FINISHED = 5
MSG_STOPJ = 6
+MSG_SERVOJ = 7
MULT_jointstate = 1000.0
MULT_time = 1000000.0
MULT_blend = 1000.0
@@ -31,6 +32,7 @@ JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
Q1 = [2.2,0,-1.57,0,0,0]
Q2 = [1.5,0,-1.57,0,0,0]
+Q3 = [1.5,-0.2,-1.57,0,0,0]
connected_robot = None
@@ -69,6 +71,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def handle(self):
self.socket_lock = threading.Lock()
self.waypoint_finished_cb = None
+ self.last_joint_states = None
setConnectedRobot(self)
#print "Handling a request"
try:
@@ -97,7 +100,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
elif mtype == MSG_JOINT_STATES:
while len(buf) < 3*(6*4):
buf = buf + self.recv_more()
- state_mult = struct.unpack_from("!" + ("i"*3*6), buf, 0)
+ state_mult = struct.unpack_from("!%ii" % (3*6), buf, 0)
buf = buf[3*6*4:]
state = [s / MULT_jointstate for s in state_mult]
@@ -108,6 +111,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
msg.velocity = state[6:12]
msg.effort = state[12:18]
pub_joint_states.publish(msg)
+ self.last_joint_states = msg
elif mtype == MSG_QUIT:
print "Quitting"
raise EOF()
@@ -132,13 +136,23 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def send_movej(self, waypoint_id, q, a=3, v=0.75, t=0, r=0):
assert(len(q) == 6)
- buf = ''.join([struct.pack("!ii", MSG_MOVEJ, waypoint_id),
- struct.pack("!iiiiii", *[MULT_jointstate * qq for qq in q]),
- struct.pack("!ii", MULT_jointstate * a, MULT_jointstate * v),
- struct.pack("!ii", MULT_time * t, MULT_blend * r)])
+ params = [MSG_MOVEJ, waypoint_id] + \
+ [MULT_jointstate * qq for qq in q] + \
+ [MULT_jointstate * a, MULT_jointstate * v, MULT_time * t, MULT_blend * r]
+ buf = struct.pack("!%ii" % len(params), *params)
with self.socket_lock:
self.request.send(buf)
+ def send_servoj(self, waypoint_id, q, t):
+ assert(len(q) == 6)
+ params = [MSG_SERVOJ, waypoint_id] + \
+ [MULT_jointstate * qq for qq in q] + \
+ [MULT_time * t]
+ buf = struct.pack("!%ii" % len(params), *params)
+ with self.socket_lock:
+ self.request.send(buf)
+
+
def send_stopj(self):
with self.socket_lock:
self.request.send(struct.pack("!i", MSG_STOPJ))
@@ -313,6 +327,15 @@ def main():
with open('prog') as fin:
sock.sendall(fin.read())
+ print "Dump"
+ print "="*70
+ o = ""
+ while len(o) < 4096:
+ r = sock.recv(1024)
+ if not r: break
+ o = o + r
+ print o
+
server = TCPServer(("", 50001), CommanderTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler", target=server.handle_request)
@@ -327,6 +350,27 @@ def main():
try:
+ #r.send_servoj(1, Q1, 1.0)
+ #time.sleep(0.5)
+ #r.send_servoj(2, Q2, 1.0)
+ #time.sleep(0.2)
+ #r.send_servoj(3, Q3, 1.0)
+ r.send_servoj(4, Q2, 1.0)
+ time.sleep(0.5)
+
+ t0 = time.time()
+ waypoint_id = 100
+ while True:
+ t = time.time() - t0
+ q = Q2[:]
+ q[0] = Q2[0] + 0.2 * math.sin(0.25 * t*(2*math.pi))
+ q[1] = Q2[1] - 0.2 + 0.2 * math.cos(0.25 * t*(2*math.pi))
+ r.send_servoj(waypoint_id, q, 0.008)
+ waypoint_id += 1
+ #print "Servo:", t, q[0], q[1]
+ time.sleep(0.008)
+
+
#log("movej Q1")
#r.send_movej(1, Q1, t=2.0)
#log("movej Q2")
@@ -342,16 +386,4 @@ def main():
rospy.signal_shutdown("KeyboardInterrupt")
raise
- '''
- print "Dump"
- print "="*70
- o = ""
- while len(o) < 4096:
- r = sock.recv(1024)
- if not r: break
- o = o + r
- print o
- '''
-
-
if __name__ == '__main__': main()
diff --git a/ur5_driver/prog b/ur5_driver/prog
index 0164d4a..5c80188 100644
--- a/ur5_driver/prog
+++ b/ur5_driver/prog
@@ -6,9 +6,11 @@ def driverProg():
MSG_MOVEJ = 4
MSG_WAYPOINT_FINISHED = 5
MSG_STOPJ = 6
+ MSG_SERVOJ = 7
MULT_jointstate = 1000.0
MULT_time = 1000000.0
MULT_blend = 1000.0
+ pi = 3.14159265359
def send_out(msg):
enter_critical
@@ -62,15 +64,75 @@ def driverProg():
sync()
end
+ SERVO_IDLE = 0
+ SERVO_RUNNING = 1
+ cmd_servo_state = SERVO_IDLE
+ cmd_servo_id = 0 # 0 = idle, -1 = stop
+ cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ cmd_servo_dt = 0.0
+ def set_servo_setpoint(id, q, dt):
+ enter_critical
+ cmd_servo_state = SERVO_RUNNING
+ cmd_servo_id = id
+ cmd_servo_q = q
+ cmd_servo_dt = dt
+ exit_critical
+ end
+ thread servoThread():
+ state = SERVO_IDLE
+ while True:
+ # Latches the new command
+ enter_critical
+ q = cmd_servo_q
+ dt = cmd_servo_dt
+ id = cmd_servo_id
+ do_brake = False
+ if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
+ # No command pending
+ do_brake = True
+ end
+ state = cmd_servo_state
+ cmd_servo_state = SERVO_IDLE
+ exit_critical
+
+ #do_brake = False
+
+ # Executes the command
+ if do_brake:
+ #stopj(1.0) # TODO
+ send_out("Braking")
+ sync()
+ elif state == SERVO_RUNNING:
+ servoj(q, 0, 0, dt)
+ send_out("Servoed")
+ else:
+ send_out("Idle")
+ sync()
+ end
+ end
+
+ end
+
socket_open(HOSTNAME, 50001)
- socket_send_int(1)
- socket_send_string("hello~")
+ send_out("hello")
thread_state = run statePublisherThread()
-
- #movej([2.2,0,-1.57,0,0,0],2)
- #movej([1.5,0,-1.57,0,0,0],2)
-
+ thread_servo = run servoThread()
+
+ # Servoes in a circle
+ movej([1.5,-0.4,-1.57,0,0,0], 3, 0.75, 1.0)
+ t = 0
+ while True:
+ q = [1.5,0,-1.57,0,0,0]
+ q[0] = q[0] + 0.2 * sin(0.25 * t*(2*pi))
+ q[1] = q[1] - 0.2 + 0.2 * cos(0.25 * t*(2*pi))
+ #servoj(q, 3, 1, 0.08)
+ #send_out("servoed")
+ set_servo_setpoint(t, q, 0.08)
+ t = t + 0.08
+ sleep(0.08)
+ end
+
while True:
#send_out("Listening")
ll = socket_read_binary_integer(1)
@@ -109,6 +171,28 @@ def driverProg():
movej(q, a, v, t, r)
send_waypoint_finished(waypoint_id)
send_out("movej finished")
+ elif mtype == MSG_SERVOJ:
+ send_out("Received servoj")
+
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for movej message")
+ end
+
+ # Unpacks the parameters
+ waypoint_id = params_mult[1]
+ q = [params_mult[2] / MULT_jointstate,
+ params_mult[3] / MULT_jointstate,
+ params_mult[4] / MULT_jointstate,
+ params_mult[5] / MULT_jointstate,
+ params_mult[6] / MULT_jointstate,
+ params_mult[7] / MULT_jointstate]
+ t = params_mult[8] / MULT_time
+
+ # Servos
+ servoj(q, 3, 0.1, t)
+ send_waypoint_finished(waypoint_id)
elif mtype == MSG_STOPJ:
send_out("Received stopj")
stopj(1.0)
--
GitLab