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
