00001
00002
00003 PKG = 'mtrace_tools'
00004
00005 import roslib; roslib.load_manifest(PKG)
00006 import rospy
00007 from ethercat_hardware.msg import MotorTrace
00008 from pylab import *
00009 from numpy import *
00010 import numpy
00011 import re
00012 import rosbag
00013 import getopt
00014 import time
00015
00016 class struct_of_arrays(object):
00017 def __init__(self, list_of_structs):
00018
00019
00020
00021 if type(list_of_structs).__name__ != 'list':
00022 print "object is not a list"
00023 return
00024 if len(list_of_structs) == 0:
00025 print "list is empty"
00026 return
00027
00028 names = dir(list_of_structs[0])
00029 struct_names=[]
00030 for name in names:
00031 if name[0] != '_':
00032 struct_names.append(name)
00033 self.__setattr__(name,[])
00034
00035 for struct in list_of_structs:
00036 for name in struct_names:
00037 tmp = self.__getattribute__(name)
00038 tmp.append(struct.__getattribute__(name))
00039
00040 for name in struct_names:
00041 tmp = self.__getattribute__(name)
00042 array_tmp = array(tmp)
00043 self.__setattr__(name,array_tmp)
00044
00045
00046 def pretty_duration(duration):
00047 in_future = False
00048 if (duration < 0):
00049 duration = -duration
00050 in_future = True
00051
00052 secs_per_min = 60.0
00053 secs_per_hour = secs_per_min * 60.0
00054 secs_per_day = secs_per_hour * 24.0
00055 secs_per_week = secs_per_day * 7.0
00056
00057 weeks = math.floor(duration / secs_per_week)
00058 duration -= weeks * secs_per_week
00059 days = math.floor(duration / secs_per_day)
00060 duration -= days * secs_per_day
00061 hours = math.floor(duration / secs_per_hour)
00062 duration -= hours * secs_per_hour
00063 mins = math.floor(duration / secs_per_min)
00064 duration -= mins * secs_per_min
00065 result = ""
00066 if weeks > 0:
00067 result += ("%d week%s "%(weeks, "s" if weeks > 1 else ""))
00068 if days > 0:
00069 result += ("%d day%s "%(days, "s" if days > 1 else ""))
00070 if hours > 0:
00071 result += ("%d hour%s "%(hours, "s" if hours > 1 else ""))
00072 if mins > 0:
00073 result += ("%d minute%s "%(mins, "s" if mins > 1 else ""))
00074 if len(result) > 0:
00075 result += "and "
00076 result += "%s seconds "%duration
00077 if in_future:
00078 result += "in the future"
00079 else:
00080 result += "ago"
00081 return result
00082
00083 def callback(data):
00084 print "Got data for actuator %s with %d samples" % (data.actuator_info.name, len(data.samples))
00085
00086 print "Reason : %s" % data.reason
00087 print "Data was published : %s -- %s" % (time.strftime("%a, %b %d, %I:%M %p", time.localtime(data.header.stamp.to_sec())), pretty_duration((rospy.get_rostime() - data.header.stamp).to_sec()))
00088 print "Timestamp seconds = %d " % data.header.stamp.secs
00089 size = len(data.samples)
00090 if size <= 1 :
00091 return
00092
00093
00094
00095 a = struct_of_arrays(data.samples)
00096
00097
00098 t = a.timestamp - a.timestamp[0]
00099 measured_motor_voltage = a.measured_motor_voltage
00100 supply_voltage = a.supply_voltage
00101 measured_current = a.measured_current
00102 executed_current = a.executed_current
00103 programmed_pwm = a.programmed_pwm
00104 velocity = a.velocity
00105 encoder_position = a.encoder_position
00106 encoder_errors = a.encoder_error_count
00107
00108 filtered_voltage_error = a.filtered_motor_voltage_error
00109 filtered_abs_voltage_error = a.filtered_abs_motor_voltage_error
00110 filtered_current_error = a.filtered_current_error
00111 filtered_abs_current_error = a.filtered_abs_current_error
00112
00113 actuator_info = data.actuator_info
00114 board_info = data.board_info
00115
00116 M_PI = math.pi
00117 backemf_constant = 1.0 / (actuator_info.speed_constant * 2.0 * M_PI * 1.0/60.0);
00118 resistance = actuator_info.motor_resistance
00119 board_resistance = board_info.board_resistance
00120 print data.board_info
00121 print actuator_info
00122 print "backemf_constant: %f" % backemf_constant
00123
00124
00125 A = numpy.array([measured_current, velocity, numpy.ones(len(velocity))])
00126 x = numpy.linalg.lstsq(A.transpose(), measured_motor_voltage)
00127 print "ESTIMATES: "
00128 print "resistance: %f"%x[0][0]
00129 print "backemf_constant: %f"%-x[0][1]
00130 print "voltage_offset: %f"%x[0][2]
00131 print "END OF ESTIMATES"
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146 A = numpy.array([measured_current, numpy.ones(len(measured_current))])
00147 B = supply_voltage * programmed_pwm - measured_motor_voltage;
00148 x = numpy.linalg.lstsq(A.transpose(), B)
00149 print "ESTIMATES(2):"
00150 print "board_resistance: %f"%x[0][0]
00151 print "voltage offset: %f"%x[0][1]
00152 print "END OF ESTIMATES(2)"
00153
00154
00155 board_output_voltage = supply_voltage * programmed_pwm - board_resistance * measured_current
00156 backemf_voltage = velocity * actuator_info.encoder_reduction * backemf_constant
00157 resistance_voltage = resistance * measured_current
00158 motor_model_voltage = backemf_voltage + resistance_voltage
00159
00160
00161 est_resistance = zeros(len(measured_current))
00162 est_resistance_accuracy = zeros(len(measured_current))
00163
00164 min_current_for_est = 0.02 * board_info.hw_max_current + 0.005
00165 for i in range(0,len(measured_current)-1):
00166 if (abs(measured_current[i]) > min_current_for_est):
00167 est_resistance[i] = (board_output_voltage[i] - backemf_voltage[i]) / measured_current[i]
00168 est_resistance_accuracy[i] = 1.0 / (1.0 + abs(backemf_voltage[i] / resistance_voltage[i]))
00169
00170
00171 figure(1)
00172 subplot(211)
00173 title("Motor Voltage")
00174 plot(t,measured_motor_voltage,'r-', label='Measured Motor (by ADC on board)')
00175 plot(t,board_output_voltage,'g-', label='Board Output (PWM * Vsupply - R_brd*I)')
00176 plot(t,motor_model_voltage,'b-', label='Motor Model (backEMF + R*I)')
00177 legend(loc='best')
00178 subplot(212)
00179 title("Motor Voltage Error")
00180 plot(t,measured_motor_voltage - board_output_voltage,'r-', label='Measured Motor - Board Output')
00181 plot(t,motor_model_voltage - board_output_voltage,'b-', label='Motor Model - Board Output')
00182 plot(t,a.motor_voltage_error_limit, 'k-', label='Motor Voltage Error Limit')
00183 plot(t,-1 * a.motor_voltage_error_limit, 'k-', label='__nolegend__')
00184 legend(loc='best')
00185 figure(5)
00186 title("Motor Voltage Relative Error");
00187 plot(t,filtered_voltage_error, 'r-', label='Trace : Motor vs Board Error (Filter)')
00188 plot(t,filtered_abs_voltage_error,'g-', label='Trace : Motor vs Board Error (Abs Filter)')
00189 legend(loc='best')
00190 figure(2)
00191 subplot(211)
00192 title("Motor Current")
00193 plot(t,measured_current,'b-', label='Measured')
00194 plot(t,executed_current,'r-', label='Executed')
00195 legend(loc='best')
00196 subplot(212)
00197 title("Motor Current Error")
00198 plot(t,measured_current-executed_current,'b-', label='Measured-Executed')
00199 plot(t,filtered_current_error,'r-', label='Trace : Error (Filter)')
00200 plot(t,filtered_abs_current_error,'g-', label='Trace : Error (Abs Filter)')
00201 legend(loc='best')
00202 figure(3)
00203 subplot(221)
00204 title("Supply Voltage")
00205 plot(t,supply_voltage)
00206 subplot(222)
00207 title("Back EMF (Velocity * Vsupply)")
00208 plot(t,backemf_voltage,'b-', label='Back EMF')
00209 subplot(223)
00210 title("Resistance * I")
00211 plot(t,resistance_voltage,'g-', label='Resistance * I')
00212 subplot(224)
00213 title("PWM %")
00214 plot(t,programmed_pwm * 100.0,'g-', label='PWM%')
00215 plot([t[0], t[-1]],ones(2) * -board_info.max_pwm_ratio * 100.0,'r-.', label='Min')
00216 plot([t[0], t[-1]],ones(2) * board_info.max_pwm_ratio * 100.0,'r-.', label='Max')
00217
00218 figure(4)
00219 angular_position = mod(encoder_position,2.0 * M_PI)
00220 subplot(211)
00221 title("Voltage Error vs Position")
00222 plot( angular_position, (motor_model_voltage - board_output_voltage) , "r.", label="Voltage Error")
00223 subplot(212)
00224 title("Current Error vs Position")
00225 plot( angular_position, (measured_current - executed_current) , "b.", label="Current")
00226
00227 figure(6)
00228 subplot(211)
00229 title("Motor Resistance")
00230 plot( t, est_resistance , "r.", label="Estimated Resistance")
00231 plot( [t[0], t[-1]], ones(2) * resistance , "g-", label="Resistance")
00232 legend(loc='best')
00233 subplot(212)
00234 title("Resistance Accuracy")
00235 plot( t, est_resistance_accuracy , "g-", label="Accuracy")
00236
00237 figure(7)
00238 subplot(311)
00239 title("Angular Position")
00240 plot( t, angular_position , "r-", label="Angualar position (0 to 2*PI)")
00241 legend(loc='best')
00242 subplot(312)
00243 title("Position")
00244 plot( t, encoder_position , "b-", label="Position")
00245 legend(loc='best')
00246 subplot(313)
00247 title("Velocity")
00248 plot( t, velocity , "g-", label="Velocity")
00249 legend(loc='best')
00250
00251 figure(8)
00252 title("Velocity")
00253 plot( t, velocity , "g-", label="Velocity")
00254 legend(loc='best')
00255
00256
00257 show()
00258
00259 def read_bag(bagfn):
00260 bag = rosbag.Bag(bagfn)
00261 for topic, msg, t in bag.read_messages():
00262 callback(msg)
00263
00264
00265 def _succeed(args):
00266 code, msg, val = args
00267 if code != 1:
00268 raise ROSTopicException("remote call failed: %s"%msg)
00269 return val
00270
00271 def usage():
00272 print "usage : old_mtrace_plot.py [-f filename] topic [more topics]"
00273 print " topic - Regular express of topic name to subscribe to"
00274 print " -f filename - bag file to read messages from"
00275 print "example : mtrace_plot.py l_wrist_r_motor"
00276 print "example : mtrace_plot.py -f pr1006_r_wrist_r_motor_error_2010-05-10-14-53-50.bag"
00277
00278 def mtrace_plot(argv):
00279 if len(argv) < 2:
00280 usage()
00281 return 1
00282
00283 rospy.init_node("mtrace_plot", anonymous=True)
00284
00285 optlist,argv = getopt.gnu_getopt(argv, "hf:");
00286 for opt,arg in optlist:
00287 if (opt == "-f"):
00288 read_bag(arg)
00289 elif (opt == "-h"):
00290 usage()
00291 return 0
00292 else :
00293 print "Internal error : opt = ", opt
00294 return 1
00295
00296 if len(argv) > 1:
00297 master = roslib.scriptutil.get_master()
00298 pub_topics = _succeed(master.getPublishedTopics('/rostopic', '/'))
00299 valid_topics = []
00300 for topic,topic_type in pub_topics:
00301 if topic_type == 'ethercat_hardware/MotorTrace':
00302 valid_topics.append(topic)
00303
00304 if len(valid_topics) == 0:
00305 print "Error: no valid topics being published"
00306 return 1
00307
00308 topic_list = []
00309 for arg in sys.argv[1:]:
00310 regex = re.compile(arg)
00311 for topic in valid_topics:
00312 if regex.search(topic):
00313 topic_list.append(topic)
00314 if len(topic_list) == 0:
00315 print "Error no topics names match"
00316 print "Possible topics : %s"%",".join(valid_topics)
00317 return 1
00318
00319
00320
00321 for topic in topic_list:
00322 print "subscribing to topic %s" % topic
00323 rospy.Subscriber(topic, MotorTrace, callback)
00324 rospy.spin()
00325
00326 if __name__ == '__main__':
00327 exit(mtrace_plot(sys.argv))
00328
00329
00330
00331