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