Exemple #1
0
    def goToPoint(self, x, y):
        print 'Going to static point:', x, y
        bound = .01
        self.long_control.setGain(self.long_ultimate_gain / 2)
        self.lateral_control.setGain(self.lateral_ultimate_gain / 2)

        # def getError():
        #     agent_id, x_actual, y_actual,z, theta_actual, pitch, roll = self.locate(self.agent_id)
        #     x_error = x - x_actual
        #     y_error = y - y_actual
        #     long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual)
        #     return long_error, lateral_error
        pos = self.locate(self.agent_id)
        agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = pos
        x_error = x - x_actual
        y_error = y - y_actual
        long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual)
        # long_error, lateral_error = getError()
        while (abs(long_error) > bound or abs(lateral_error) > bound) and pos == pos:
            pos = self.locate(self.agent_id)
            agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = pos
            x_error = x - x_actual
            y_error = y - y_actual
            long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual)
            feedback_linear = self.long_control.controllerOutput(long_error)
            # print '% 4.2f, % 4.2f' % (long_error, feedback_linear), x,y, x_actual,y_actual,theta_actual
            feedback_angular = self.lateral_control.controllerOutput(lateral_error)
            self.actuate(feedback_linear, feedback_angular)
            # self.logger = append(self.logger, [[long_error, lateral_error,
            #                                 x, y,
            #                                 x_actual, y_actual,
            #                                 theta_actual, time()]], axis=0)
        print 'Longitutional error:', long_error, 'm | Lateral error:', lateral_error, 'm'
Exemple #2
0
    def followTrajectory(self, trajectory, x_calibrate=0, y_calibrate=0):
        print 'Following Trajectory.'
        self.long_control.setGain(self.long_ultimate_gain / 2)
        self.lateral_control.setGain(self.lateral_ultimate_gain / 2)
        # self.angular_control.setGain(self.angular_ultimate_gain / 2)
        reference_pos = trajectory.getPosition()
        # print 'ref pos 1',reference_pos
        while reference_pos == reference_pos:  # check to see if x_reference is not NaN
            agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = self.locate(self.agent_id)
            # x_actual, y_actual, theta_actual = self.locate()
            x_reference, y_reference, theta_reference = reference_pos
            x_error = x_reference + x_calibrate - x_actual
            y_error = y_reference + y_calibrate - y_actual
            long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual)
            angular_error = theta_reference - theta_actual

            feedback_linear = self.long_control.controllerOutput(long_error)
            feedback_angular = self.lateral_control.controllerOutput(lateral_error)
            # feedback_angular = feedback_angular + self.angular_control.controllerOutput(angular_error)

            self.actuate(feedback_linear, feedback_angular)

            reference_pos = trajectory.getPosition()
            # print 'ref pos',reference_pos
            self.logger = append(self.logger, [[long_error, lateral_error,
                                                x_reference + x_calibrate, y_reference + y_calibrate,
                                                x_actual, y_actual,
                                                theta_actual, time(), 0]], axis=0)

        save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/' + str(datetime.now()) + ' followTrajectory', self.logger)
        self.logger = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
        print 'Reached end of trajectory.'
Exemple #3
0
    def followPath(self, path, x_calibrate=0, y_calibrate=0):
        'Following path.'
        self.long_control.setGain(self.long_ultimate_gain / 2)
        self.lateral_control.setGain(self.lateral_ultimate_gain / 2)
        # self.angular_control.setGain(self.angular_ultimate_gain / 2)
        reference_pos = path.getPosition()
        # print 'ref pos 1',reference_pos
        while reference_pos == reference_pos:  # check to see if x_reference is not NaN
            agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = self.locate(self.agent_id)
            # x_actual, y_actual, theta_actual = self.locate()
            x_reference, y_reference, theta_reference = reference_pos
            x_error = x_reference + x_calibrate - x_actual
            y_error = y_reference + y_calibrate - y_actual
            long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual)
            angular_error = theta_reference - theta_actual
            lateral_penalty = abs(lateral_error**2) * (-100)
            # angular_penalty = abs(pi/2-theta_actual) * -.4*18/pi
            vel = max(.5 + lateral_penalty, 0.05)
            print '% 4.2f, % 4.2f, % 4.2f' % (vel, abs(lateral_error), abs(pi / 2 - theta_actual))
            path.setVelocity(vel)
            feedback_linear = self.long_control.controllerOutput(long_error)
            feedback_lateral = self.lateral_control.controllerOutput(lateral_error)
            feedback_angular = self.angular_control.controllerOutput(angular_error)
            # print feedback_angular, feedback_lateral

            self.actuate(feedback_linear, feedback_lateral)

            reference_pos = path.getPosition()
            # print 'ref pos',reference_pos
            self.logger = append(self.logger, [[long_error, lateral_error,
                                                x_reference + x_calibrate, y_reference + y_calibrate,
                                                x_actual, y_actual,
                                                theta_actual, time(), vel]], axis=0)

        save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/' + str(datetime.now()) + ' followPath', self.logger)
        self.logger = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
        print 'Reached end of path.'