From b4c12e76c9d8253cf4de15d38dd4312e77a5346f Mon Sep 17 00:00:00 2001 From: Shervin Javdani Date: Mon, 18 Jan 2016 16:28:35 -0500 Subject: [PATCH 1/6] added publishing for each joint --- src/prpy/base/mico.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/prpy/base/mico.py b/src/prpy/base/mico.py index 8b40024..15dcaea 100644 --- a/src/prpy/base/mico.py +++ b/src/prpy/base/mico.py @@ -30,6 +30,8 @@ import openravepy from manipulator import Manipulator +from std_msgs.msg import Float64 +import rospy class Mico(Manipulator): def __init__(self, sim, @@ -67,6 +69,11 @@ def __init__(self, sim, self.GetName(), '', self.GetArmIndices(), 0, True) self.servo_simulator = ServoSimulator(self, rate=20, watchdog_timeout=0.1) + + else: + #if not simulation, create publishers for each joint + self.velocity_topic_names = ['vel_j'+str(i)+'_controller/command' for i in range(1,7)] + self.velocity_publishers = [rospy.Publisher(topic_name, Float64) for topic_name in self.velocity_topic_names] def CloneBindings(self, parent): super(Mico, self).CloneBindings(parent) @@ -99,4 +106,9 @@ def Servo(self, velocities): self.GetRobot().GetController().Reset(0) self.servo_simulator.SetVelocity(velocities) else: - raise NotImplementedError('Servo is not implemented.') + for velocity_publisher,velocity in zip(self.velocity_publishers, velocities): + velocity_publisher.publish(velocity) + #raise NotImplementedError('Servo is not implemented.') + + + From 8b91b686cd203b530976b317c99f47952e51939c Mon Sep 17 00:00:00 2001 From: Shervin Javdani Date: Tue, 19 Jan 2016 14:57:04 -0500 Subject: [PATCH 2/6] added watchdog for servoing --- src/prpy/base/mico.py | 20 +++++++++++++------- src/prpy/util.py | 15 +++++++++++++++ 2 files changed, 28 insertions(+), 7 deletions(-) diff --git a/src/prpy/base/mico.py b/src/prpy/base/mico.py index 15dcaea..e72478a 100644 --- a/src/prpy/base/mico.py +++ b/src/prpy/base/mico.py @@ -32,6 +32,8 @@ from manipulator import Manipulator from std_msgs.msg import Float64 import rospy +from ..util import Watchdog +from functools import partial class Mico(Manipulator): def __init__(self, sim, @@ -69,11 +71,13 @@ def __init__(self, sim, self.GetName(), '', self.GetArmIndices(), 0, True) self.servo_simulator = ServoSimulator(self, rate=20, watchdog_timeout=0.1) - else: - #if not simulation, create publishers for each joint + #if not simulation, create publishers for each joint self.velocity_topic_names = ['vel_j'+str(i)+'_controller/command' for i in range(1,7)] - self.velocity_publishers = [rospy.Publisher(topic_name, Float64) for topic_name in self.velocity_topic_names] + self.velocity_publishers = [rospy.Publisher(topic_name, Float64, queue_size=1) for topic_name in self.velocity_topic_names] + + #create watchdog to send zero velocity + self.servo_watchdog = Watchdog(timeout_duration=0.3, handler=lambda: self.SendVelocitiesToMico([0.,0.,0.,0.,0.,0.,])) def CloneBindings(self, parent): super(Mico, self).CloneBindings(parent) @@ -106,9 +110,11 @@ def Servo(self, velocities): self.GetRobot().GetController().Reset(0) self.servo_simulator.SetVelocity(velocities) else: - for velocity_publisher,velocity in zip(self.velocity_publishers, velocities): - velocity_publisher.publish(velocity) - #raise NotImplementedError('Servo is not implemented.') - + self.SendVelocitiesToMico(velocities) + #reset watchdog timer + self.servo_watchdog.reset() + def SendVelocitiesToMico(self, velocities): + for velocity_publisher,velocity in zip(self.velocity_publishers, velocities): + velocity_publisher.publish(velocity) diff --git a/src/prpy/util.py b/src/prpy/util.py index 018a31f..c9e10ca 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -497,6 +497,21 @@ def stop(self): def get_duration(self): return self.end - self.start +class Watchdog(object): + def __init__(self, timeout_duration, handler): + self.timeout_duration = timeout_duration + self.handler = handler + self.timer = threading.Timer(self.timeout_duration, self.handler) + self.timer.start() + + def reset(self): + self.timer.cancel() + self.timer = threading.Timer(self.timeout_duration, self.handler) + self.timer.start() + + def stop(self): + self.timer.cancel() + def quadraticPlusJointLimitObjective(dq, J, dx, q, q_min, q_max, delta_joint_penalty=5e-1, lambda_dqdist=0.01, *args): ''' From 1e7778fe824a9a020645574b9201e4a9993672af Mon Sep 17 00:00:00 2001 From: Shervin Javdani Date: Tue, 19 Jan 2016 16:16:56 -0500 Subject: [PATCH 3/6] removed extraneous include --- src/prpy/base/mico.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/prpy/base/mico.py b/src/prpy/base/mico.py index e72478a..7c84d64 100644 --- a/src/prpy/base/mico.py +++ b/src/prpy/base/mico.py @@ -33,7 +33,6 @@ from std_msgs.msg import Float64 import rospy from ..util import Watchdog -from functools import partial class Mico(Manipulator): def __init__(self, sim, @@ -77,7 +76,7 @@ def __init__(self, sim, self.velocity_publishers = [rospy.Publisher(topic_name, Float64, queue_size=1) for topic_name in self.velocity_topic_names] #create watchdog to send zero velocity - self.servo_watchdog = Watchdog(timeout_duration=0.3, handler=lambda: self.SendVelocitiesToMico([0.,0.,0.,0.,0.,0.,])) + self.servo_watchdog = Watchdog(timeout_duration=0.25, handler=lambda: self.SendVelocitiesToMico([0.,0.,0.,0.,0.,0.,])) def CloneBindings(self, parent): super(Mico, self).CloneBindings(parent) From 48dd913b1d95f2de307b2dda0c2d4e35017e86e4 Mon Sep 17 00:00:00 2001 From: Shervin Javdani Date: Fri, 22 Jan 2016 16:00:01 -0500 Subject: [PATCH 4/6] modified watchdog to spawn fewer threads. added locks for thread safety --- src/prpy/base/mico.py | 9 ++++-- src/prpy/util.py | 71 ++++++++++++++++++++++++++++++++++++++----- 2 files changed, 70 insertions(+), 10 deletions(-) diff --git a/src/prpy/base/mico.py b/src/prpy/base/mico.py index 7c84d64..ec8a892 100644 --- a/src/prpy/base/mico.py +++ b/src/prpy/base/mico.py @@ -32,6 +32,7 @@ from manipulator import Manipulator from std_msgs.msg import Float64 import rospy +import threading from ..util import Watchdog class Mico(Manipulator): @@ -74,9 +75,10 @@ def __init__(self, sim, #if not simulation, create publishers for each joint self.velocity_topic_names = ['vel_j'+str(i)+'_controller/command' for i in range(1,7)] self.velocity_publishers = [rospy.Publisher(topic_name, Float64, queue_size=1) for topic_name in self.velocity_topic_names] + self.velocity_publisher_lock = threading.Lock() #create watchdog to send zero velocity - self.servo_watchdog = Watchdog(timeout_duration=0.25, handler=lambda: self.SendVelocitiesToMico([0.,0.,0.,0.,0.,0.,])) + self.servo_watchdog = Watchdog(timeout_duration=0.25, handler=self.SendVelocitiesToMico, args=[[0.,0.,0.,0.,0.,0.,]]) def CloneBindings(self, parent): super(Mico, self).CloneBindings(parent) @@ -115,5 +117,6 @@ def Servo(self, velocities): def SendVelocitiesToMico(self, velocities): - for velocity_publisher,velocity in zip(self.velocity_publishers, velocities): - velocity_publisher.publish(velocity) + with self.velocity_publisher_lock: + for velocity_publisher,velocity in zip(self.velocity_publishers, velocities): + velocity_publisher.publish(velocity) diff --git a/src/prpy/util.py b/src/prpy/util.py index c9e10ca..f08e7a3 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -498,19 +498,76 @@ def get_duration(self): return self.end - self.start class Watchdog(object): - def __init__(self, timeout_duration, handler): + ''' + Calls specified function after duration, unless reset/stopped beforehand + + @param timeout_duration how long to wait before calling handler + @param handler function to call after timeout_duration + @param args for handler + @param kwargs for handler + ''' + def __init__(self, timeout_duration, handler, args=(), kwargs={}): self.timeout_duration = timeout_duration self.handler = handler - self.timer = threading.Timer(self.timeout_duration, self.handler) - self.timer.start() + self.handler_args = args + self.handler_kwargs = kwargs + + self.thread_checking_time = threading.Thread(target=self._check_timer_loop) + self.timer_thread_lock = threading.Lock() + self.start_time = time.time() + self.canceled = False + + self.thread_checking_time.start() + #self.timer = threading.Timer(self.timeout_duration, self.handler) + #self.timer.start() def reset(self): - self.timer.cancel() - self.timer = threading.Timer(self.timeout_duration, self.handler) - self.timer.start() + ''' + Resets the timer + + Causes the handler function to be called after the next + timeout duration is reached + + Also restarts the timer thread if it has existed + ''' + with self.timer_thread_lock: + self.start_time = time.time() + self.canceled = False + if not self.thread_checking_time.is_alive(): + self.thread_checking_time = threading.Thread(target=self._check_timer_loop) + self.thread_checking_time.start() + + #self.timer.cancel() + #self.timer = threading.Timer(self.timeout_duration, self.handler) + #self.timer.start() def stop(self): - self.timer.cancel() + ''' + Stop the watchdog, so it will not call handler + ''' + with self.timer_thread_lock: + self.canceled = True + + def _check_timer_loop(self): + ''' + Internal function for timer thread to loop + + If elapsed time has passed, calls the handler function + Exists if watchdog was canceled, or handler was called + ''' + while True: + with self.timer_thread_lock: + if self.canceled: + break + elapsed_time = time.time() - self.start_time + if elapsed_time > self.timeout_duration: + self.handler(*self.handler_args, **self.handler_kwargs) + self.canceled = True + break + else: + time.sleep(self.timeout_duration - elapsed_time) + + def quadraticPlusJointLimitObjective(dq, J, dx, q, q_min, q_max, delta_joint_penalty=5e-1, lambda_dqdist=0.01, *args): From 29ee9e89ff2c59a88e3aecf2c48d1deeecc96518 Mon Sep 17 00:00:00 2001 From: Shervin Javdani Date: Fri, 22 Jan 2016 16:03:43 -0500 Subject: [PATCH 5/6] removed old commented out code --- src/prpy/util.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/prpy/util.py b/src/prpy/util.py index f08e7a3..00d13d2 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -518,8 +518,6 @@ def __init__(self, timeout_duration, handler, args=(), kwargs={}): self.canceled = False self.thread_checking_time.start() - #self.timer = threading.Timer(self.timeout_duration, self.handler) - #self.timer.start() def reset(self): ''' @@ -537,10 +535,6 @@ def reset(self): self.thread_checking_time = threading.Thread(target=self._check_timer_loop) self.thread_checking_time.start() - #self.timer.cancel() - #self.timer = threading.Timer(self.timeout_duration, self.handler) - #self.timer.start() - def stop(self): ''' Stop the watchdog, so it will not call handler From 2afadf728a55ea598f0d06435ca0a6f304e6a15f Mon Sep 17 00:00:00 2001 From: Shervin Javdani Date: Fri, 22 Jan 2016 16:46:53 -0500 Subject: [PATCH 6/6] small change in case the target thread has exited, but is_alive is not set to false yet --- src/prpy/util.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/prpy/util.py b/src/prpy/util.py index 00d13d2..e4fd310 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -529,11 +529,12 @@ def reset(self): Also restarts the timer thread if it has existed ''' with self.timer_thread_lock: + if self.canceled or not self.thread_checking_time.is_alive(): + self.thread_checking_time = threading.Thread(target=self._check_timer_loop) + self.thread_checking_time.start() + self.start_time = time.time() self.canceled = False - if not self.thread_checking_time.is_alive(): - self.thread_checking_time = threading.Thread(target=self._check_timer_loop) - self.thread_checking_time.start() def stop(self): ''' @@ -556,7 +557,8 @@ def _check_timer_loop(self): elapsed_time = time.time() - self.start_time if elapsed_time > self.timeout_duration: self.handler(*self.handler_args, **self.handler_kwargs) - self.canceled = True + with self.timer_thread_lock: + self.canceled = True break else: time.sleep(self.timeout_duration - elapsed_time)