class AutostepNode(object): def __init__(self, port='/dev/ttyACM0'): # Parameters self.port = rospy.get_param('port', '/dev/ttyACM0') self.step_mode = rospy.get_param('step_mode', 'STEP_FS_128') self.fullstep_per_rev = rospy.get_param('fullstep_per_rev', 200) self.gear_ratio = rospy.get_param('gear_ratio', 2.0) self.tracking_mode_gain = rospy.get_param('tracking_mode_gain', 5.0) self.tracking_mode_absolute = rospy.get_param('tracking_mode_absolute', True) self.max_mode_params = rospy.get_param('max_mode_params', { 'speed': 1000, 'accel': 10000, 'decel': 10000 }) self.jog_mode_params = rospy.get_param('max_mode_params', { 'speed': 400, 'accel': 800, 'decel': 800 }) # Tracking mode state data self.tracking_mode_is_first = False self.tracking_mode_first_update_t = 0.0 self.tracking_mode_last_update_t = 0.0 self.tracking_mode_position = 0.0 self.tracking_mode_velocity = 0.0 self.tracking_mode_position_start = 0.0 self.initialize() self.enable() rospy.init_node('autostep') self.motion_pub = rospy.Publisher('motion_data', MotionData, queue_size=10) self.tracking_sub = rospy.Subscriber('tracking_data', TrackingData, self.on_tracking_data_callback) self.command_srv_table = { 'run': self.on_run_command, 'enable': self.on_enable_command, 'release': self.on_release_command, 'is_busy': self.on_is_busy_command, 'was_stopped': self.on_was_stopped_command, 'move_to': self.on_move_to_command, 'move_by': self.on_move_by_command, 'soft_stop': self.on_soft_stop_command, 'set_position': self.on_set_position_command, 'get_position': self.on_get_position_command, 'set_move_mode': self.on_set_move_mode_command, 'get_jog_mode_params': self.on_get_jog_mode_params, 'set_jog_mode_params': self.on_set_jog_mode_params, 'get_max_mode_params': self.on_get_max_mode_params, 'set_max_mode_params': self.on_set_max_mode_params, 'get_params': self.on_get_params_command, 'print_params': self.on_print_params_command, 'sinusoid': self.on_sinusoid_command, 'move_to_sinusoid_start': self.on_move_to_sinusoid_start_command, 'run_trajectory': self.on_run_trajectory_command, 'enable_tracking_mode': self.on_enable_tracking_mode_command, 'disable_tracking_mode': self.on_disable_tracking_mode_command, } self.command_srv = rospy.Service('command', Command, self.command_srv_callback) self.lock = threading.Lock() self.tracking_mode_enabled = False self.running_motion_cmd = False self.stopped_flag = False def initialize(self): self.autostep = Autostep(self.port) self.autostep.set_step_mode(self.step_mode) self.autostep.set_fullstep_per_rev(self.fullstep_per_rev) self.autostep.set_gear_ratio(self.gear_ratio) self.autostep.set_move_mode_to_jog() self.autostep.set_jog_mode_params(self.jog_mode_params) self.autostep.set_max_mode_params(self.max_mode_params) self.have_sensor = False def enable(self): self.autostep.enable() self.enabled_flag = True def release(self): self.autostep.release() self.enabled_flag = False def command_srv_callback(self, req): args_dict = {} if req.args_json != '': args_json = req.args_json.replace('\{', '{') args_json = args_json.replace('\}', '}') args_dict = json.loads(args_json) ok = False try: command_method = self.command_srv_table[req.command] ok = True except KeyError: rsp_dict = {'success': False, 'message': 'unknown command'} if ok: rsp_dict = command_method(args_dict) return CommandResponse(json.dumps(rsp_dict)) def on_run_command(self, args_dict): self.stopped_flag = False ok = False velocity = 0.0 rsp_dict = {} try: velocity = args_dict['velocity'] rsp_dict['success'] = True rsp_dict['message'] = '' ok = True except KeyError: rsp_dict['success'] = False rsp_dict['message'] = 'velocity argument missing' if ok: self.autostep.run(velocity) return rsp_dict def on_enable_command(self, args_dict): self.enable() return {'success': True, 'message': ''} def on_release_command(self, args_dict): self.release() return {'success': True, 'message': ''} def on_move_to_command(self, args_dict): self.stopped_flag = False ok = False position = 0.0 rsp_dict = {} try: position = args_dict['position'] rsp_dict['success'] = True rsp_dict['message'] = '' ok = True except KeyError: rsp_dict['success'] = False rsp_dict['message'] = 'position argument missing' if ok: self.autostep.move_to(position) return rsp_dict def on_move_by_command(self, args_dict): self.stopped_flag = False ok = False position = 0.0 rsp_dict = {} try: step = args_dict['step'] rsp_dict['success'] = True rsp_dict['message'] = '' ok = True except KeyError: rsp_dict['success'] = False rsp_dict['message'] = 'step argument missing' if ok: self.autostep.move_by(step) return rsp_dict def on_soft_stop_command(self, args_dict): self.stopped_flag = True with self.lock: self.tracking_mode_enabled = False self.running_motion_cmd = False self.autostep.soft_stop() return {'success': True, 'message': ''} def on_is_busy_command(self, args_dict): is_busy = self.autostep.is_busy() if is_busy or self.running_motion_cmd or self.tracking_mode_enabled: return {'success': True, 'message': '', 'is_busy': True} else: return {'success': True, 'message': '', 'is_busy': False} def on_was_stopped_command(self, args_dict): return { 'success': True, 'message': '', 'was_stopped': self.stopped_flag } def on_get_position_command(self, args_dict): position = self.autostep.get_position() return {'success': True, 'message': '', 'position': position} def on_set_position_command(self, args_dict): ok = False position = 0.0 rsp_dict = {} try: position = args_dict['position'] rsp_dict['success'] = True rsp_dict['message'] = '' ok = True except KeyError: rsp_dict['success'] = False rsp_dict['message'] = 'position argument missing' if ok: self.autostep.set_position(position) return rsp_dict def on_sinusoid_command(self, args_dict): self.stopped_flag = False ok = True param = {} rsp_dict = {'message': ''} param_keys = ['amplitude', 'period', 'phase', 'offset', 'num_cycle'] for key in param_keys: try: param[key] = args_dict[key] except KeyError: ok = False if len(rsp_dict['message']) > 0: rsp_dict['message'] += ', ' rsp_dict['message'] += '{} argument missing'.format(key) if ok: def motion_data_callback(elapsed_time, position, setpoint, sensor): if not self.have_sensor: sensor = 0.0 header = std_msgs.msg.Header() header.stamp = rospy.Time.now() self.motion_pub.publish( MotionData(header, elapsed_time, position, setpoint, sensor)) def motion_done_callback(): with self.lock: self.autostep.set_move_mode_to_jog() self.autostep.run(0.0) self.running_motion_cmd = False # Launch sinusoid in separate thread thread_args = [param, motion_data_callback, motion_done_callback] motion_thread = threading.Thread(target=self.autostep.sinusoid, args=thread_args) with self.lock: self.running_motion_cmd = True motion_thread.start() rsp_dict['success'] = True else: rsp_dict['success'] = False return rsp_dict def on_move_to_sinusoid_start_command(self, args_dict): ok = True param = {} rsp_dict = {'message': ''} param_keys = ['amplitude', 'period', 'phase', 'offset', 'num_cycle'] for key in param_keys: try: param[key] = args_dict[key] except KeyError: ok = False if len(rsp_dict['message']) > 0: rsp_dict['message'] += ', ' rsp_dict['message'] += '{} argument missing'.format(key) if ok: self.autostep.move_to_sinusoid_start(param) rsp_dict['success'] = True else: rsp_dict['success'] = False return rsp_dict def on_set_move_mode_command(self, args_dict): ok = False mode = '' rsp_dict = {} try: mode = args_dict['mode'] rsp_dict['success'] = True rsp_dict['message'] = '' ok = True except KeyError: rsp_dict['success'] = False rsp_dict['message'] = 'mode argument missing' if ok: if mode == 'max': self.autostep.set_move_mode_to_max() elif mode == 'jog': self.autostep.set_move_mode_to_jog() else: rsp_dict['success'] = False rsp_dict['message'] = "mode must be 'max' or 'jog'" return rsp_dict def on_get_jog_mode_params(self, args_dict): jog_mode_params = self.autostep.get_jog_mode_params() rsp_dict = {'success': True, 'message': '', 'params': jog_mode_params} return rsp_dict def on_set_jog_mode_params(self, args_dict): ok = False rsp_dict = {} try: jog_mode_params = args_dict['params'] rsp_dict['success'] = True rsp_dict['message'] = '' ok = True except KeyError: rsp_dict['success'] = False rsp_dict['message'] = 'position argument missing' if ok: try: self.autostep.set_jog_mode_params(jog_mode_params) self.jog_mode_params = jog_mode_params except AutostepException: rsp_dict['success'] = False rsp_dict['message'] = 'setting jog mode parameters failed' return rsp_dict def on_get_max_mode_params(self, args_dict): max_mode_params = self.autostep.get_max_mode_params() rsp_dict = {'success': True, 'message': '', 'params': max_mode_params} return rsp_dict def on_set_max_mode_params(self, args_dict): ok = False rsp_dict = {} try: max_mode_params = args_dict['params'] rsp_dict['success'] = True rsp_dict['message'] = '' ok = True except KeyError: rsp_dict['success'] = False rsp_dict['message'] = 'position argument missing' if ok: try: self.autostep.set_max_mode_params(max_mode_params) self.max_mode_params = max_mode_params except AutostepException: rsp_dict['success'] = False rsp_dict['message'] = 'setting max mode parameters failed' return rsp_dict def on_print_params_command(self, args_dict): self.autostep.print_params() return {'success': True, 'message': ''} def on_print_params_command(self, args_dict): self.autostep.print_params() return {'success': True, 'message': ''} def on_get_params_command(self, args_dict): params = self.autostep.get_params() return {'success': True, 'message': '', 'params': params} def on_run_trajectory_command(self, args_dict): self.stopped_flag = False rsp_dict = {} try: position = args_dict['position'] except KeyError: rsp_dict['success'] = False rsp_dict['message'] = 'position (array) argument missing' return rsp_dict position = scipy.array(position) dt = Autostep.TrajectoryDt velocity = scipy.zeros(position.shape) velocity[1:] = (position[1:] - position[:-1]) / dt t = dt * scipy.arange(0, position.shape[0]) t_done = t[-1] position_func = scipy.interpolate.interp1d(t, position, kind='linear') velocity_func = scipy.interpolate.interp1d(t, velocity, kind='linear') def motion_data_callback(elapsed_time, position, setpoint): header = std_msgs.msg.Header() header.stamp = rospy.Time.now() self.motion_pub.publish( MotionData(header, elapsed_time, position, setpoint, 0.0)) def motion_done_callback(): with self.lock: self.autostep.set_move_mode_to_jog() self.autostep.run(0.0) self.running_motion_cmd = False # Launch sinusoid in separate thread thread_args = [ t_done, position_func, velocity_func, False, motion_data_callback, motion_done_callback ] motion_thread = threading.Thread(target=self.autostep.run_trajectory, args=thread_args) with self.lock: self.running_motion_cmd = True motion_thread.start() return {'success': True, 'message': ''} def on_enable_tracking_mode_command(self, args_dict): self.stopped_flag = False with self.lock: self.autostep.run(0.0) self.autostep.set_move_mode_to_max() self.tracking_mode_enabled = True self.tracking_mode_is_first = True return {'success': True, 'message': ''} def on_disable_tracking_mode_command(self, args_dict): with self.lock: self.tracking_mode_enabled = False self.autostep.set_move_mode_to_jog() self.autostep.run(0.0) return {'success': True, 'message': ''} def on_tracking_data_callback(self, msg): with self.lock: tracking_mode_enabled = self.tracking_mode_enabled if self.tracking_mode_enabled: if self.tracking_mode_is_first: self.tracking_mode_is_first = False self.tracking_mode_first_update_t = rospy.get_time() self.tracking_mode_last_update_t = self.tracking_mode_first_update_t self.tracking_mode_position_start = self.autostep.get_position( ) self.tracking_mode_position = self.tracking_mode_position_start predicted_position = self.tracking_mode_position self.tracking_mode_velocity = 0.0 else: current_time = rospy.get_time() update_dt = current_time - self.tracking_mode_last_update_t predicted_position = self.tracking_mode_position + update_dt * self.tracking_mode_velocity if self.tracking_mode_absolute: position_error = msg.position - predicted_position else: position_error = msg.position - ( predicted_position - self.tracking_mode_position_start) new_velocity = self.tracking_mode_gain * position_error + msg.velocity true_position = self.autostep.run_with_feedback(new_velocity) self.tracking_mode_position = true_position self.tracking_mode_velocity = new_velocity self.tracking_mode_last_update_t = current_time #rospy.logwarn(position_error) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() elapsed_time = self.tracking_mode_last_update_t - self.tracking_mode_first_update_t self.motion_pub.publish( MotionData(header, elapsed_time, self.tracking_mode_position, predicted_position, 0.0)) def run(self): while not rospy.is_shutdown(): rospy.sleep(0.1) self.autostep.run(0.0)
plt.ylabel('period (sec)') plt.grid('on') plt.subplot(212) plt.plot(velocity_array, amplitude_list, '-o') plt.ylabel('amplitute (deg)') plt.grid('on') plt.xlabel('velocity (deg/sec)') plt.show() port = '/dev/ttyACM0' stepper = Autostep(port) stepper.set_step_mode('STEP_FS_128') stepper.set_fullstep_per_rev(200) stepper.set_move_mode_to_jog() stepper.set_gear_ratio(2.0) stepper.enable() stepper.set_position(0.0) for i, velocity in enumerate(velocity_array): period, amplitude = get_period_and_amplitude(velocity, acceleration) print('cnt {}: vel: {:1.2f} per: {:1.2f} amp: {:1.2f}'.format( i, velocity, period, amplitude)) param = { 'amplitude': amplitude, 'period': period,
gain = 5.0 sleep_dt = 0.005 step_period = 4.0 step_amplitude = 45.0 rc_period = 8.0 rc_amplitude = 80.0 t_end = 2 * step_period pos_set_func, vel_set_func, rc_set_func = get_kinematics_func( step_period, step_amplitude, rc_period, rc_amplitude) dev = Autostep(port) dev.set_step_mode('STEP_FS_64') dev.set_fullstep_per_rev(200) dev.set_gear_ratio(1.0) dev.set_jog_mode_params({'speed': 200, 'accel': 1000, 'decel': 1000}) dev.set_max_mode_params({'speed': 1000, 'accel': 30000, 'decel': 30000}) dev.set_move_mode_to_max() dev.enable() dev.print_params() dev.run(0.0) t_start = time.time() t_last = t_start t_list = [] pos_tru_list = [] pos_set_list = []