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 __init__(self, *args):
super(TestKinematicsLearningNode, self).__init__(*args)
self.msg_theta_dot_weights = KinematicsWeights()
self.msg_v_weights = KinematicsWeights()
self.msg_theta_dot_received = False
self.msg_v_received = False
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)
