def __init__(self, dxl_io, controller_namespace, port_namespace):
JointControllerMX.__init__(self, dxl_io, controller_namespace, port_namespace)
self.name = controller_namespace
self.velocity_standoff_service = rospy.Service(self.controller_namespace + '/velocity_standoff', VelocityStandoff, self.velocity_standoff)
self.torque_standoff_service = rospy.Service(self.controller_namespace + '/torque_standoff', TorqueStandoff, self.torque_standoff)
self.torque_hold_service = rospy.Service(self.controller_namespace + '/torque_hold', TorqueHold, self.torque_hold)
self.go_to_position_service = rospy.Service(self.controller_namespace + '/go_to_position', GoToPosition, self.service_go_to_position)
self.pause_service = rospy.Service(self.controller_namespace + '/pause', Enable, self.service_pause)
#condition variable thingy to control waiting
self.waitCV = threading.Condition()
#minimum moving velocity for an "is moving" condition check
#XXX TODO: GET THIS FROM ROS PARAMETERS
self.min_moving_velocity = None
self.velocity_start_delay = 0.5
self.move_timeout = 2.0
self.max_stopped_velocity = .01
self.position_tol = .05
#create an array to smooth important state variables
#to ensure no false state detections
self.velocity_array = array.array('f', (0,)*3)
self.current_array = array.array('f', (0,)*3)
self.avg_current = 0.0
self.avg_velocity = 0
self.goal_position = 0.0
self.check_for_move = False
self.check_for_stop = False
self.check_for_position = False
self.check_for_timeout = False
self.check_current = None
self.timeout = None
self.timed_out = False
self.paused = True #start up these dynamixels paused
#record initial torque limit, to be reset after pauses
self.initial_torque_limit = self.torque_limit
def process_motor_states(self, state_list):
JointControllerMX.process_motor_states(self, state_list)
# this block of crap creates the running average of velocity
self.velocity_array.pop(0)
self.velocity_array.append(self.joint_state.velocity)
self.avg_velocity = sum(self.velocity_array) / len(self.velocity_array)
# this block of crap creates the running average of current
self.current_array.pop(0)
self.current_array.append(self.joint_state.current)
self.avg_current = sum(self.current_array)/len(self.current_array)
# if we have any waiting flags set, see if the wait should be cleared
if self.check_for_move:
if abs(self.avg_velocity) > self.min_moving_velocity:
self.unblock()
self.check_for_move = False
if self.check_for_stop:
if abs(self.avg_velocity) < self.max_stopped_velocity:
self.unblock()
self.check_for_stop = False
if self.check_for_position:
if abs(self.joint_state.error) < self.position_tol:
self.unblock()
self.check_for_position = False
if self.check_for_timeout:
if (rospy.Time.now() - self.timeout_start_time) > self.timeout:
self.timed_out = True
self.unblock()
self.check_for_timeout = False
if self.check_current is not None:
if abs(self.avg_current) > self.check_current:
self.unblock()
self.check_current = None
