Beispiel #1
0
def main(argv):
    try:
        opts, args = getopt.getopt(argv, "")
    except getopt.GetoptError:
        print 'Usage:\tlearn_offline.py <inputfile>'
        sys.exit(2)

    filename = args[0] if len(
        args) > 0 and not args[0] == "" else "./training_data.txt"
    if os.path.isfile(filename):
        print "Parsing file: {filename}".format(filename=filename)

        # Calculate the weights
        theta_dot_fi_function = 'Duty_fi_linear_no_constant'
        v_fi_function = 'Duty_fi_linear_no_constant'
        learner = Linear_learner(theta_dot_fi_function, v_fi_function)
        theta_dot_weights = learner.fit_theta_dot_from_file(filename)
        v_weights = learner.fit_v_from_file(filename)
        print "theta_dot_weights:\t[{weight1}, {weight2}]".format(
            weight1=theta_dot_weights[0, 0], weight2=theta_dot_weights[0, 1])
        print "v_weights:\t\t[{weight1}, {weight2}]".format(
            weight1=v_weights[0, 0], weight2=v_weights[0, 1])
    else:
        print "ERROR: {filename} not found.".format(filename=filename)
        sys.exit(2)
 def donot_test_linear_no_constant(self):
     theta_dot_fi_function = 'Duty_fi_linear_no_constant'
     v_fi_function = 'Duty_fi_linear_no_constant'
     learner = Linear_learner(theta_dot_fi_function, v_fi_function)
     rp = RosPack()
     filepath = rp.get_path('kinematics') + "/tests/test_training_set.txt"
     theta_dot_weights = learner.fit_theta_dot_from_file(filepath)
     v_weights = learner.fit_v_from_file(filepath)
Beispiel #3
0
    def test_linear_no_constant(self):
        theta_dot_fi_function = 'Duty_fi_linear_no_constant'
        v_fi_function = 'Duty_fi_linear_no_constant'
        learner = Linear_learner(theta_dot_fi_function, v_fi_function)
        rp = RosPack()
        filepath = rp.get_path('kinematics') + "/tests/test_training_set.txt"
        theta_dot_weights = learner.fit_theta_dot_from_file(filepath)
        v_weights = learner.fit_v_from_file(filepath)

        np.testing.assert_almost_equal(v_weights, np.matrix([1.0, 1.0]))
        np.testing.assert_almost_equal(theta_dot_weights, np.matrix([-1, 1]))
def main(argv):
    try:
        opts, args = getopt.getopt(argv,"")
    except getopt.GetoptError:
        print 'Usage:\tlearn_offline.py <inputfile>'
        sys.exit(2)

    filename = args[0] if len(args) > 0 and not args[0] == "" else "./training_data.txt"
    if os.path.isfile(filename):
        print "Parsing file: {filename}".format(filename=filename)

        # Calculate the weights
        theta_dot_fi_function = 'Duty_fi_linear_no_constant'
        v_fi_function = 'Duty_fi_linear_no_constant'
        learner = Linear_learner(theta_dot_fi_function, v_fi_function)
        theta_dot_weights = learner.fit_theta_dot_from_file(filename)
        v_weights = learner.fit_v_from_file(filename)
        print "theta_dot_weights:\t[{weight1}, {weight2}]".format(weight1=theta_dot_weights[0,0], weight2=theta_dot_weights[0,1])
        print "v_weights:\t\t[{weight1}, {weight2}]".format(weight1=v_weights[0,0], weight2=v_weights[0,1])
    else:
        print "ERROR: {filename} not found.".format(filename=filename)
        sys.exit(2)
    def __init__(self):
        self.node_name = 'kinematics_learning_node'

        self.mutex = Lock()

        # Read parameters
        fi_theta_dot_function = self.setupParameter(
            '~fi_theta_dot_function_param', 'Duty_fi_theta_dot_naive')
        fi_v_function = self.setupParameter('~fi_v_function_param',
                                            'Duty_fi_v_naive')
        self.theta_dot_noZeros = self.setupParameter(
            '~learner_number_of_zero_entries', 0)
        self.v_noZeros = self.setupParameter('~learner_number_of_zero_entries',
                                             0)
        self.noThetaDotSamples = 2 * self.setupParameter(
            '~learner_number_of_theta_dot_samples_per_direction', 1000)
        self.noVsamples = self.setupParameter('~learner_number_of_v_samples',
                                              1000)
        self.theta_dot_threshold = self.setupParameter(
            '~learner_theta_dot_threshold', 0.1)
        self.v_disp_threshold = pow(
            self.setupParameter('~learner_v_disp_threshold', 0.01), 2)
        theta_dot_regularizer = self.setupParameter(
            '~learner_theta_dot_regularizer', 0.1)
        v_regularizer = self.setupParameter('~learner_v_regularizer', 0.1)

        # Setup the kinematics learning model
        self.kl = Linear_learner.Linear_learner(fi_theta_dot_function,
                                                fi_v_function,
                                                theta_dot_regularizer,
                                                v_regularizer)

        # Setup publishers and subscribers
        self.pub_theta_dot_kinematics_weights = rospy.Publisher(
            "~theta_dot_kinematics_weights", KinematicsWeights, queue_size=1)
        self.pub_v_kinematics_weights = rospy.Publisher(
            "~v_kinematics_weights", KinematicsWeights, queue_size=1)
        self.sub_theta_dot_sample = rospy.Subscriber(
            "~theta_dot_sample", ThetaDotSample, self.thetaDotSampleCallback)
        self.sub_v_sample = rospy.Subscriber("~v_sample", Vsample,
                                             self.vSampleCallback)

        self.theta_dot_negative_index = self.theta_dot_noZeros
        self.theta_dot_positive_index = self.theta_dot_negative_index + self.noThetaDotSamples / 2
        self.v_index = self.v_noZeros

        self.theta_dot_d_L = zeros(
            (self.theta_dot_noZeros + self.noThetaDotSamples, 1))
        self.theta_dot_d_R = zeros(
            (self.theta_dot_noZeros + self.noThetaDotSamples, 1))
        self.theta_dot_dt = zeros(
            (self.theta_dot_noZeros + self.noThetaDotSamples, 1))
        self.theta_dot_dt[:self.
                          theta_dot_noZeros, :] = 1  # initialize dt=1 for the dummy zero motion samples
        self.theta_dot_theta_angle_pose_delta = zeros(
            (self.theta_dot_noZeros + self.noThetaDotSamples, 1))

        self.v_d_L = zeros((self.v_noZeros + self.noVsamples, 1))
        self.v_d_R = zeros((self.v_noZeros + self.noVsamples, 1))
        self.v_dt = zeros((self.v_noZeros + self.noVsamples, 1))
        self.v_dt[:self.
                  v_noZeros, :] = 1  # initialize dt=1 for the dummy zero motion samples
        self.v_theta_angle_pose_delta = zeros(
            (self.v_noZeros + self.noVsamples, 1))
        self.v_x_axis_pose_delta = zeros((self.v_noZeros + self.noVsamples, 1))
        self.v_y_axis_pose_delta = zeros((self.v_noZeros + self.noVsamples, 1))
        rospy.loginfo("[%s] has started", self.node_name)