Go to the documentation of this file.00001
00002 import roslib
00003 roslib.load_manifest( 'gpr_controller' )
00004 import rospy
00005 import rosbag
00006
00007 import matplotlib.pyplot as plt
00008
00009 from numpy import asarray, zeros
00010 import numpy as np
00011
00012 import operator
00013
00014 joint_names = ['r_shoulder_pan_joint',
00015 'r_shoulder_lift_joint',
00016 'r_upper_arm_roll_joint',
00017 'r_elbow_flex_joint',
00018 'r_forearm_roll_joint',
00019 'r_wrist_flex_joint',
00020 'r_wrist_roll_joint']
00021
00022
00023 def prod( lst ):
00024 return reduce( operator.mul, lst )
00025
00026
00027 def read_file( fname, topic ):
00028 bag = rosbag.Bag( fname )
00029 states = []
00030 time = rospy.Time.from_sec(0.0)
00031 for topic, msg, t in bag.read_messages( topics = [topic] ):
00032 if time > msg.header.stamp:
00033 print 'messages out of order'
00034 return None
00035 time = msg.header.stamp
00036 states.append( msg )
00037 bag.close()
00038 return states
00039
00040 def generate_samples( joint_states ):
00041 joint_ids = None
00042 x = []
00043 for s in joint_states:
00044 if joint_ids is None:
00045 joint_ids = map( s.name.index, joint_names )
00046
00047 p = asarray( map( lambda x: s.position[x], joint_ids ) )
00048 v = asarray( map( lambda x: s.velocity[x], joint_ids ) )
00049 a = np.zeros( (len( joint_names )) )
00050 e = asarray( map( lambda x: s.effort[x], joint_ids ) )
00051 x.append( np.concatenate( (p,v,a,e) ) )
00052
00053 x = asarray(x).transpose()
00054
00055 (_, N) = x.shape
00056 X = zeros( (len( joint_names ) * 4, N-1) )
00057 for i in range( 7 ):
00058 X[i] = x[i][1:]
00059
00060
00061 for i in range( 7, 14 ):
00062 X[i] = x[i][1:]
00063 X[i+7] = x[i][1:]-x[i][:-1]
00064
00065 j = 1
00066 plt.plot(X[0 + j])
00067 plt.plot(X[7 + j])
00068 plt.plot(X[14 + j])
00069 plt.show()
00070
00071
00072
00073 def split_per_joint( states ):
00074 for s in states:
00075 print s.header.stamp.to_sec()
00076
00077
00078 def gen_trainingset( joint_states, controller_states ):
00079 U = []
00080 X = []
00081
00082 while controller_states[0].header.stamp <= joint_states[0].header.stamp:
00083
00084 controller_states.pop(0)
00085
00086 while len( controller_states ) > 0 and len( joint_states ) > 0:
00087 U.append( asarray(joint_states[0].effort) )
00088 x = np.concatenate( ( asarray(controller_states[0].actual.positions ),
00089 asarray( controller_states[0].actual.velocities ) ) )
00090
00091
00092 X.append(x.flatten())
00093
00094 controller_states.pop(0)
00095 joint_states.pop(0)
00096
00097 print len( U )
00098 print len( X )
00099
00100 U = np.transpose(U)
00101 X = np.transpose(X)
00102
00103 return U, X
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114 def MSE( states ):
00115 error = zeros( len( states[0].error.positions ) )
00116 for s in states:
00117 error += asarray( s.error.positions ) ** 2
00118 return error / len( states )
00119
00120 def target_mean( states ):
00121 mean = zeros( len( states[0].actual.positions ) )
00122 for s in states:
00123 mean += asarray( s.actual.positions )
00124 return mean / len( states )
00125
00126
00127 def target_variance( states ):
00128 mean = target_mean( states )
00129 var = zeros( len( states[0].actual.positions ) )
00130 for s in states:
00131 var += (asarray( s.actual.positions ) - mean) ** 2
00132 return var / len( states )
00133
00134 def nMSE( states ):
00135 return MSE( states ) / target_variance( states )
00136
00137 fname = 'test.bag'
00138 controller_states = read_file( fname, 'r_arm_controller/state' )
00139 joint_states = read_file( fname, 'joint_states')
00140
00141 print '#datapoints controller %i'%len( controller_states )
00142 print '#datapoints joint_states %i'%len( joint_states )
00143
00144 generate_samples( joint_states )
00145
00146
00147
00148
00149
00150
00151
00152
00153