def vSampleCallback(self, v_sample): #print 'v_sample', v_sample # Only use this sample if it is informative #res = (v_sample.x_axis_pose_delta*v_sample.x_axis_pose_delta + v_sample.y_axis_pose_delta*v_sample.y_axis_pose_delta)/v_sample.dt #print 'v', res if (v_sample.x_axis_pose_delta * v_sample.x_axis_pose_delta + v_sample.y_axis_pose_delta * v_sample.y_axis_pose_delta ) / v_sample.dt >= self.v_disp_threshold: self.mutex.acquire() self.v_d_L[self.v_index] = v_sample.d_L self.v_d_R[self.v_index] = v_sample.d_R self.v_dt[self.v_index] = v_sample.dt self.v_theta_angle_pose_delta[ self.v_index] = v_sample.theta_angle_pose_delta self.v_x_axis_pose_delta[self.v_index] = v_sample.x_axis_pose_delta self.v_y_axis_pose_delta[self.v_index] = v_sample.y_axis_pose_delta self.v_index += 1 # Only fit when a sufficient number of samples has been gathered if self.v_index >= (self.v_noZeros + self.noVsamples): weights = self.kl.fit_v(self.v_d_L, self.v_d_R, self.v_dt, self.v_theta_angle_pose_delta, self.v_x_axis_pose_delta, self.v_y_axis_pose_delta) weights = asarray(weights).flatten().tolist() print 'v weights', weights # Put the weights in a message and publish msg_kinematics_weights = KinematicsWeights() msg_kinematics_weights.weights = weights self.pub_v_kinematics_weights.publish(msg_kinematics_weights) # reset index self.v_index = self.v_noZeros self.mutex.release()
def vSampleCallback(self, v_sample): #print 'v_sample', v_sample # Only use this sample if it is informative #res = (v_sample.x_axis_pose_delta*v_sample.x_axis_pose_delta + v_sample.y_axis_pose_delta*v_sample.y_axis_pose_delta)/v_sample.dt #print 'v', res if (v_sample.x_axis_pose_delta*v_sample.x_axis_pose_delta + v_sample.y_axis_pose_delta*v_sample.y_axis_pose_delta)/v_sample.dt >= self.v_disp_threshold: self.mutex.acquire() self.v_d_L[self.v_index] = v_sample.d_L self.v_d_R[self.v_index] = v_sample.d_R self.v_dt[self.v_index] = v_sample.dt self.v_theta_angle_pose_delta[self.v_index] = v_sample.theta_angle_pose_delta self.v_x_axis_pose_delta[self.v_index] = v_sample.x_axis_pose_delta self.v_y_axis_pose_delta[self.v_index] = v_sample.y_axis_pose_delta self.v_index += 1 # Only fit when a sufficient number of samples has been gathered if self.v_index >= (self.v_noZeros+self.noVsamples): weights = self.kl.fit_v(self.v_d_L, self.v_d_R, self.v_dt, self.v_theta_angle_pose_delta, self.v_x_axis_pose_delta, self.v_y_axis_pose_delta) weights = asarray(weights).flatten().tolist() print 'v weights', weights # Put the weights in a message and publish msg_kinematics_weights = KinematicsWeights() msg_kinematics_weights.weights = weights self.pub_v_kinematics_weights.publish(msg_kinematics_weights) # reset index self.v_index = self.v_noZeros self.mutex.release()
def thetaDotSampleCallback(self, theta_dot_sample): #print 'theta_dot_sample', theta_dot_sample self.mutex.acquire() # Only use this sample if it is informative if (self.theta_dot_negative_index < self.theta_dot_noZeros+self.noThetaDotSamples/2) and (theta_dot_sample.theta_angle_pose_delta/theta_dot_sample.dt <= -self.theta_dot_threshold): #print 'adding negative' self.theta_dot_d_L[self.theta_dot_negative_index] = theta_dot_sample.d_L self.theta_dot_d_R[self.theta_dot_negative_index] = theta_dot_sample.d_R self.theta_dot_dt[self.theta_dot_negative_index] = theta_dot_sample.dt self.theta_dot_theta_angle_pose_delta[self.theta_dot_negative_index] = theta_dot_sample.theta_angle_pose_delta self.theta_dot_negative_index += 1 elif (self.theta_dot_positive_index < self.theta_dot_noZeros+self.noThetaDotSamples) and (theta_dot_sample.theta_angle_pose_delta/theta_dot_sample.dt >= self.theta_dot_threshold): #print 'adding positive' self.theta_dot_d_L[self.theta_dot_positive_index] = theta_dot_sample.d_L self.theta_dot_d_R[self.theta_dot_positive_index] = theta_dot_sample.d_R self.theta_dot_dt[self.theta_dot_positive_index] = theta_dot_sample.dt self.theta_dot_theta_angle_pose_delta[self.theta_dot_positive_index] = theta_dot_sample.theta_angle_pose_delta self.theta_dot_positive_index += 1 # print 'theta_dot_negative_index', self.theta_dot_negative_index, 'theta_dot_positive_index', self.theta_dot_positive_index # Only fit when a sufficient number of informative samples has been gathered if (self.theta_dot_negative_index >= (self.theta_dot_noZeros+self.noThetaDotSamples/2)) and (self.theta_dot_positive_index >= self.theta_dot_noZeros+self.noThetaDotSamples): weights = self.kl.fit_theta_dot(self.theta_dot_d_L, self.theta_dot_d_R, self.theta_dot_dt, self.theta_dot_theta_angle_pose_delta) weights = asarray(weights).flatten().tolist() print 'theta_dot weights', weights # Put the weights in a message and publish msg_kinematics_weights = KinematicsWeights() msg_kinematics_weights.weights = weights self.pub_theta_dot_kinematics_weights.publish(msg_kinematics_weights) # reset indices self.theta_dot_negative_index = self.theta_dot_noZeros self.theta_dot_positive_index = self.theta_dot_negative_index + self.noThetaDotSamples/2 self.mutex.release()
def thetaDotSampleCallback(self, theta_dot_sample): #print 'theta_dot_sample', theta_dot_sample self.mutex.acquire() # Only use this sample if it is informative if (self.theta_dot_negative_index < self.theta_dot_noZeros + self.noThetaDotSamples / 2) and ( theta_dot_sample.theta_angle_pose_delta / theta_dot_sample.dt <= -self.theta_dot_threshold): #print 'adding negative' self.theta_dot_d_L[ self.theta_dot_negative_index] = theta_dot_sample.d_L self.theta_dot_d_R[ self.theta_dot_negative_index] = theta_dot_sample.d_R self.theta_dot_dt[ self.theta_dot_negative_index] = theta_dot_sample.dt self.theta_dot_theta_angle_pose_delta[ self. theta_dot_negative_index] = theta_dot_sample.theta_angle_pose_delta self.theta_dot_negative_index += 1 elif (self.theta_dot_positive_index < self.theta_dot_noZeros + self.noThetaDotSamples) and ( theta_dot_sample.theta_angle_pose_delta / theta_dot_sample.dt >= self.theta_dot_threshold): #print 'adding positive' self.theta_dot_d_L[ self.theta_dot_positive_index] = theta_dot_sample.d_L self.theta_dot_d_R[ self.theta_dot_positive_index] = theta_dot_sample.d_R self.theta_dot_dt[ self.theta_dot_positive_index] = theta_dot_sample.dt self.theta_dot_theta_angle_pose_delta[ self. theta_dot_positive_index] = theta_dot_sample.theta_angle_pose_delta self.theta_dot_positive_index += 1 # print 'theta_dot_negative_index', self.theta_dot_negative_index, 'theta_dot_positive_index', self.theta_dot_positive_index # Only fit when a sufficient number of informative samples has been gathered if (self.theta_dot_negative_index >= (self.theta_dot_noZeros + self.noThetaDotSamples / 2)) and ( self.theta_dot_positive_index >= self.theta_dot_noZeros + self.noThetaDotSamples): weights = self.kl.fit_theta_dot( self.theta_dot_d_L, self.theta_dot_d_R, self.theta_dot_dt, self.theta_dot_theta_angle_pose_delta) weights = asarray(weights).flatten().tolist() print 'theta_dot weights', weights # Put the weights in a message and publish msg_kinematics_weights = KinematicsWeights() msg_kinematics_weights.weights = weights self.pub_theta_dot_kinematics_weights.publish( msg_kinematics_weights) # reset indices self.theta_dot_negative_index = self.theta_dot_noZeros self.theta_dot_positive_index = self.theta_dot_negative_index + self.noThetaDotSamples / 2 self.mutex.release()
def __init__(self): self.node_name = 'trajectory_learning_node' self.mutex = Lock() # Read parameters self.veh_name = self.setupParameter("~veh_name","megaman") self.theta_dot_filename_1 = self.setupParameter("~theta_dot_filename_1", "theta_dot_filename_1") self.theta_dot_filename_2 = self.setupParameter("~theta_dot_filename_2", "theta_dot_filename_2") self.v_filename_1 = self.setupParameter("~v_filename_1", "v_filename_1") self.theta_delta_1 = self.setupParameter("~theta_delta_1", 20.0*pi) self.theta_delta_2 = self.setupParameter("~theta_delta_2", -20.0*pi) self.distance_1 = self.setupParameter("~distance_1", 1.0) self.fi_theta_dot_function = self.setupParameter('~fi_theta_dot_function_param', 'Duty_fi_theta_dot_compound_linear') self.fi_v_function = self.setupParameter('~fi_v_function_param', 'Duty_fi_v_compound_linear') self.theta_dot_regularizer = self.setupParameter('~learner_theta_dot_regularizer', 0.01) self.v_regularizer = self.setupParameter('~learner_v_regularizer', 0.01) # Setup publishers self.pub_theta_dot_kinematics_weights = rospy.Publisher("~theta_dot_kinematics_weights", KinematicsWeights, latch=True, queue_size=1) self.pub_v_kinematics_weights = rospy.Publisher("~v_kinematics_weights", KinematicsWeights, latch=True, queue_size=1) if os.path.isfile(self.theta_dot_filename_1) and os.path.isfile(self.theta_dot_filename_2): theta_dot_FI_1 = genfromtxt(self.theta_dot_filename_1) theta_dot_FI_2 = genfromtxt(self.theta_dot_filename_2) theta_dot_FI = theta_dot_FI_1 theta_dot_FI[1,:] = theta_dot_FI_2[0,:] b = zeros((2,1)) b[0,0] = self.theta_delta_1 b[1,0] = self.theta_delta_2 # Find the weights for theta_dot # Least squares fit # l2 regularization. Penalize all but the first weight regularizer = zeros((2,2)) regularizer[1,1] = self.theta_dot_regularizer weights = matrix(linalg.lstsq(theta_dot_FI, b)[0].flatten()) weights = asarray(weights).flatten().tolist() print 'theta_dot weights', weights # Put the weights in a message and publish msg_kinematics_weights = KinematicsWeights() msg_kinematics_weights.weights = weights self.pub_theta_dot_kinematics_weights.publish(msg_kinematics_weights) else: print 'could not compute theta_dot weights' if os.path.isfile(self.v_filename_1): v_FI_1 = genfromtxt(self.v_filename_1) v_FI = zeros_like(v_FI_1) v_FI[0,:] = v_FI_1[1,:] b = zeros((2,1)) b[0,0] = self.distance_1 b[1,0] = 0 # Find the weights for v # Least squares fit # l2 regularization. Penalize all but the first weight regularizer = zeros((2,2)) regularizer[1,1] = self.v_regularizer weights = matrix(linalg.lstsq(v_FI, b)[0].flatten()) weights = asarray(weights).flatten().tolist() print 'v weights', weights # Put the weights in a message and publish msg_kinematics_weights = KinematicsWeights() msg_kinematics_weights.weights = weights self.pub_v_kinematics_weights.publish(msg_kinematics_weights) else: print 'could not compute v weights' rospy.loginfo("[%s] has started", self.node_name)