Пример #1
0
    def __init__(self, randomize=False, rnn_xml=None, verbose=False):
        """Initialize

        Creates the environment and viewer objects required to run the neural
        network PA10 robotic arm simulation.

        Arguments:
            randomize: Determines if the test article gates will be randomized.
                (Default: False)
            rnn_xml: A XML filename containing neural network parameters. If
                None, a new neural network will be trained until convergence.
                (Default: None)
            verbose: Determines the level out debug output generated.
                (Default: False)
        """
        # Generate the XODE file
        XODE_FILENAME = 'model'  # .xode is appended automatically

        print('>>> Generating world model')
        if os.path.exists('./' + XODE_FILENAME + '.xode'):
            os.remove('./' + XODE_FILENAME + '.xode')

        xode_model = NeuralSimWorld(name=XODE_FILENAME,
                                    randomize_test_article=randomize)
        xode_model.generate()

        print('>>> Generating RNN')

        # Determine if we need to train the neural network
        if rnn_xml is not None:
            print('>>> Loading RNN from file')
            self.rnn = network.PathPlanningNetwork()
            self.rnn.load_network_from_file(rnn_xml)
        else:
            print('>>> Training new RNN')
            self.rnn = network.train_path_planning_network()
            self.rnn.save_network_to_file(constants.G_RNN_XML_OUT)

        print('>>> Starting kinematics engine')
        self.kinematics = PA10Kinematics()

        # Start environment
        print('>>> Starting environment')
        self.env = EnvironmentInterface(
            xode_filename='./' + XODE_FILENAME + '.xode',
            realtime=False,
            verbose=verbose,
            gravity=constants.G_ENVIRONMENT_GRAVITY)

        # Start viewer
        print('>>> Starting viewer')
        self.viewer = ViewerInterface(verbose=verbose)
        self.viewer.start()

        # Set up all grouped bodies in the environment
        self.env.groups = {
            'pointer': ['tooltip', 'stick'],
        }

        return
Пример #2
0
    def __init__(self, randomize=False, network=False, verbose=False):
        """Initialize

        Creates the environment, viewer, and (Phantom Omni) controller objects
        required to run the human data capture simulation.

        Arguments:
            randomize: Determines if the test article gates will be randomized.
                (Default: False)
            network: Determines if the omni connection is local or networked.
                (Default: False)
            verbose: Determines the level out debug output generated.
                (Default: False)
        """
        # Generate the XODE file
        XODE_FILENAME = 'model'  # .xode is appended automatically

        print('>>> Generating world model')
        if os.path.exists('./' + XODE_FILENAME + '.xode'):
            os.remove('./' + XODE_FILENAME + '.xode')

        xode_model = TrainingSimWorld(
            name=XODE_FILENAME, randomize_test_article=randomize)
        xode_model.generate()

        # Start environment
        print('>>> Starting environment')
        self.env = EnvironmentInterface(
            xode_filename='./' + XODE_FILENAME + '.xode',
            realtime=False,
            verbose=verbose,
            gravity=constants.G_ENVIRONMENT_GRAVITY)

        # Set up all grouped bodies in the environment
        self.env.groups = {
            'pointer': ['tooltip', 'stick'],
        }

        # Start viewer
        print('>>> Starting viewer')
        self.viewer = ViewerInterface(verbose=verbose)
        self.viewer.start()

        # Start controller
        print('>>> Starting Phantom Omni interface')
        self.omni = PhantomOmniInterface()

        if network:
            ip = raw_input('<<< Enter host ip: ')
            port = int(raw_input('<<< Enter tcp port: '))
        else:
            ip = constants.G_IP_LOCAL_DEFAULT
            port = constants.G_PORT_DEFAULT

        # Try to connect to the Phantom Omni controller
        self.omni.connect(ip, port)

        self.saved_data = np.array([])

        return
Пример #3
0
    def __init__(self, randomize=False, network=False, verbose=False):
        """Initialize

        Creates the environment, viewer, and (Phantom Omni) controller objects
        required to run the human data capture simulation.

        Arguments:
            randomize: Determines if the test article gates will be randomized.
                (Default: False)
            network: Determines if the omni connection is local or networked.
                (Default: False)
            verbose: Determines the level out debug output generated.
                (Default: False)
        """
        # Generate the XODE file
        XODE_FILENAME = 'model' # .xode is appended automatically

        print('>>> Generating world model')
        if os.path.exists('./'+XODE_FILENAME+'.xode'):
            os.remove('./'+XODE_FILENAME+'.xode')

        xode_model = TrainingSimWorld(
                name=XODE_FILENAME,
                randomize_test_article=randomize
        )
        xode_model.generate()

        # Start environment
        print('>>> Starting environment')
        self.env = EnvironmentInterface(
                xode_filename='./'+XODE_FILENAME+'.xode',
                realtime=False,
                verbose=verbose,
                gravity=constants.G_ENVIRONMENT_GRAVITY
        )

        # Set up all grouped bodies in the environment
        self.env.groups = {
            'pointer': ['tooltip', 'stick'],
        }

        # Start viewer
        print('>>> Starting viewer')
        self.viewer = ViewerInterface(verbose=verbose)
        self.viewer.start()

        # Start controller
        print('>>> Starting Phantom Omni interface')
        self.omni = PhantomOmniInterface()

        if network:
            ip = raw_input('<<< Enter host ip: ')
            port = int(raw_input('<<< Enter tcp port: '))
        else:
            ip = constants.G_IP_LOCAL_DEFAULT
            port = constants.G_PORT_DEFAULT

        # Try to connect to the Phantom Omni controller
        self.omni.connect(ip, port)

        self.saved_data = np.array([])

        return
Пример #4
0
class TrainingSimulation(object):
    """TrainingSimulation class

    Responsible for initialization of all children objects such as the
    Open Dynamics Engine environment, Phantom Omni controller, and OpenGL
    viewer. Starts the main event loop with real-time constraints.

    Attributes:
        env: The Open Dynamics Engine environment.
        omni: The Phantom Omni robotic controller connection.
        viewer: The OpenGL viewer for the ODE environment.
        saved_data: A list of tuples containing data from the simulation.

    Methods:
        start: Begins the main event loop.
    """
    env = None
    omni = None
    viewer = None
    saved_data = None

    def __init__(self, randomize=False, network=False, verbose=False):
        """Initialize

        Creates the environment, viewer, and (Phantom Omni) controller objects
        required to run the human data capture simulation.

        Arguments:
            randomize: Determines if the test article gates will be randomized.
                (Default: False)
            network: Determines if the omni connection is local or networked.
                (Default: False)
            verbose: Determines the level out debug output generated.
                (Default: False)
        """
        # Generate the XODE file
        XODE_FILENAME = 'model' # .xode is appended automatically

        print('>>> Generating world model')
        if os.path.exists('./'+XODE_FILENAME+'.xode'):
            os.remove('./'+XODE_FILENAME+'.xode')

        xode_model = TrainingSimWorld(
                name=XODE_FILENAME,
                randomize_test_article=randomize
        )
        xode_model.generate()

        # Start environment
        print('>>> Starting environment')
        self.env = EnvironmentInterface(
                xode_filename='./'+XODE_FILENAME+'.xode',
                realtime=False,
                verbose=verbose,
                gravity=constants.G_ENVIRONMENT_GRAVITY
        )

        # Set up all grouped bodies in the environment
        self.env.groups = {
            'pointer': ['tooltip', 'stick'],
        }

        # Start viewer
        print('>>> Starting viewer')
        self.viewer = ViewerInterface(verbose=verbose)
        self.viewer.start()

        # Start controller
        print('>>> Starting Phantom Omni interface')
        self.omni = PhantomOmniInterface()

        if network:
            ip = raw_input('<<< Enter host ip: ')
            port = int(raw_input('<<< Enter tcp port: '))
        else:
            ip = constants.G_IP_LOCAL_DEFAULT
            port = constants.G_PORT_DEFAULT

        # Try to connect to the Phantom Omni controller
        self.omni.connect(ip, port)

        self.saved_data = np.array([])

        return

    def start(self, fps):
        """Start

        Begin the continuous event loop for the simulation. This event loop
        can be exited using the ctrl+c keyboard interrupt. Real-time
        constraints are enforced.

        Arguments:
            fps: The value of frames per second of the simulation.
        """
        paused = False
        stopped = False

        # Define the simulation frame rate
        t = 0.0
        dt = 1.0 / float(fps) # [s]

        # Keep track of time overshoot in the case that simulation time must be
        # increased in order to maintain real-time constraints
        t_overshoot = 0.0

        while not stopped:
            t_start = time.time()

            # If the last calculation took too long, catch up
            dt_warped = dt + t_overshoot

            self.env.set_dt(dt_warped)
            self.omni.set_dt(dt_warped)

            # Determine if the viewer is stopped. Then we can quit
            if self.viewer.is_dead:
                break

            if paused:
                self.env.step(paused=True)
                continue

            # Populate the controller with the most up-to-date data
            self.omni.update()

            # Determine the input data to record
            sample_input = np.array([
                t,
            ]).flatten()

            # Capture the gate position at each time step
            for gate_idx in range(constants.G_NUM_GATES):
                gate_pos = np.array(self.env.get_body_pos('gate%d'%gate_idx)).flatten()
                gate_rot = constants.G_GATE_NORM_ROT[gate_idx]
                sample_input = np.hstack((sample_input, gate_pos, gate_rot))

            # Determine the output data to record
            sample_output = np.array([
                self.env.get_body_pos('tooltip'),
            ]).flatten()

            # Join the sample input/output
            data_sample = np.hstack((sample_input, sample_output))

            # Save the data
            self.save_data(data_sample)

            # Get the updated linear/angular velocities of the tooltip
            linear_vel = self.omni.get_linear_vel()
            angular_vel = self.omni.get_angular_vel()

            # Set the linear and angular velocities of the simulation
            self.env.set_group_linear_vel('pointer', linear_vel)
            self.env.set_group_angular_vel('pointer', angular_vel)

            # Step through the world by 1 time frame
            self.env.step()

            t += dt_warped

            # Determine the difference in virtual vs actual time
            t_warped = dt - (time.time() - t_start)

            # Attempt to enforce real-time constraints
            if t_warped >= 0.0:
                # The calculation took less time than the virtual time. Sleep
                # the rest off
                time.sleep(t_warped)
                t_overshoot = 0.0
            else:
                # The calculation took more time than the virtual time. We need
                # to catch up with the virtual time on the next time step
                t_overshoot = -t_warped

        return

    def save_data(self, data_sample):
        """Save Data

        Saves a sample of data into the simulation's saved data array.

        Arguments:
            data_sample: A sample of input/output data from a single time step.
        """
        if len(self.saved_data):
            self.saved_data = np.vstack((self.saved_data, data_sample))
        else:
            self.saved_data = data_sample.copy()

        return

    def __del__(self):
        """Delete (del)

        Attempts to shut down any active engines and kills off spawned
        class objects.
        """
        # Kill the OpenGL viewer process
        if self.viewer is not None:
            self.viewer.stop()
            del self.viewer

        # Kill the ODE environment objects
        if self.env is not None:
            self.env.stop()
            del self.env

        # Kill the Phantom Omni controller process
        if self.omni is not None:
            self.omni.disconnect()
            del self.omni

        return
Пример #5
0
class NeuralSimulation(object):
    """NeuralSimulation class

    Responsible for initialization of all children objects such as the
    Open Dynamics Engine environment and OpenGL viewer. Starts the main
    event loop with real-time constraints.

    Attributes:
        env: The Open Dynamics Engine environment.
        viewer: The OpenGL viewer for the ODE environment.

    Methods:
        start: Begins the main event loop.
    """
    env = None
    viewer = None
    rnn = None
    kinematics = None

    def __init__(self, randomize=False, rnn_xml=None, verbose=False):
        """Initialize

        Creates the environment and viewer objects required to run the neural
        network PA10 robotic arm simulation.

        Arguments:
            randomize: Determines if the test article gates will be randomized.
                (Default: False)
            rnn_xml: A XML filename containing neural network parameters. If
                None, a new neural network will be trained until convergence.
                (Default: None)
            verbose: Determines the level out debug output generated.
                (Default: False)
        """
        # Generate the XODE file
        XODE_FILENAME = 'model'  # .xode is appended automatically

        print('>>> Generating world model')
        if os.path.exists('./' + XODE_FILENAME + '.xode'):
            os.remove('./' + XODE_FILENAME + '.xode')

        xode_model = NeuralSimWorld(name=XODE_FILENAME,
                                    randomize_test_article=randomize)
        xode_model.generate()

        print('>>> Generating RNN')

        # Determine if we need to train the neural network
        if rnn_xml is not None:
            print('>>> Loading RNN from file')
            self.rnn = network.PathPlanningNetwork()
            self.rnn.load_network_from_file(rnn_xml)
        else:
            print('>>> Training new RNN')
            self.rnn = network.train_path_planning_network()
            self.rnn.save_network_to_file(constants.G_RNN_XML_OUT)

        print('>>> Starting kinematics engine')
        self.kinematics = PA10Kinematics()

        # Start environment
        print('>>> Starting environment')
        self.env = EnvironmentInterface(
            xode_filename='./' + XODE_FILENAME + '.xode',
            realtime=False,
            verbose=verbose,
            gravity=constants.G_ENVIRONMENT_GRAVITY)

        # Start viewer
        print('>>> Starting viewer')
        self.viewer = ViewerInterface(verbose=verbose)
        self.viewer.start()

        # Set up all grouped bodies in the environment
        self.env.groups = {
            'pointer': ['tooltip', 'stick'],
        }

        return

    def start(self, fps, fast_step=False):
        """Start

        Begin the continuous event loop for the simulation. This event loop
        can be exited using the ctrl+c keyboard interrupt. Real-time
        constraints are enforced. [Hz]

        Arguments:
            fps: The value of frames per second of the simulation.
            fast_step: If True, the ODE fast step algorithm will be used.
                This is faster and requires less memory but is less accurate.
                (Default: False)
        """
        paused = False
        stopped = False

        # Define the total time for the tooltip traversal
        t_total = 20.0

        # Define the simulation frame rate
        t = 0.0  # [s]
        dt = 1.0 / float(fps)  # [s]

        # Keep track of time overshoot in the case that simulation time must be
        # increased in order to maintain real-time constraints
        t_overshoot = 0.0

        # Get the initial path position (center of gate7)
        pos_start = self.env.get_body_pos('gate7')  # [m]

        # Get the first position of the PA10 at rest
        pos_init = self.env.get_body_pos('tooltip')  # [m]

        # Calculate the new required joint angles of the PA10
        #pa10_joint_angles = self.kinematics.calc_inverse_kinematics(pos_init, pos_start)

        # TODO: Move the PA10 end-effector to the starting position along the path

        # TODO: TEMP - Move the temporary end-effector pointer to the starting position
        self.env.set_group_pos('pointer', pos_start)

        # Generate long-term path from initial position
        t_input = np.linspace(start=0.0, stop=1.0, num=t_total / dt)
        t_input = np.reshape(t_input, (len(t_input), 1))

        rnn_path = self.rnn.extrapolate(t_input, [pos_start], len(t_input) - 1)

        # Add the initial condition point back onto the data
        rnn_path = np.vstack((pos_start, rnn_path))

        # Retrieve one set of standard gate position/orientation data
        file_path = pathutils.list_data_files(constants.G_TRAINING_DATA_DIR)[0]

        gate_data = datastore.retrieve(file_path)

        gate_start_idx = constants.G_GATE_IDX
        gate_end_idx = gate_start_idx + constants.G_NUM_GATE_INPUTS

        # Reshape the gate positions data
        gate_data = gate_data[0:1, gate_start_idx:gate_end_idx]
        gate_data = np.tile(gate_data, (len(rnn_path), 1))

        # Complete the rnn path data
        rnn_path = np.hstack((t_input, gate_data, rnn_path))

        # Save generated path for later examination
        datastore.store(rnn_path, constants.G_RNN_STATIC_PATH_OUT)

        # Define a variable to hold the final path (with real-time correction)
        final_path = rnn_path[:-1].copy()
        path_saved = False

        # Detect all path segments between gates in the generated path
        segments = pathutils._detect_segments(rnn_path)

        path_idx = 0

        x_path_offset = np.array([0.0, 0.0, 0.0])  # [m]
        v_curr = np.array([0.0, 0.0, 0.0])  # [m/s]
        a_max = constants.G_MAX_ACCEL  # [m/s^2]

        # Get the static table position
        x_table = self.env.get_body_pos('table')

        while not stopped:
            t_start = time.time()

            # If the last calculation took too long, catch up
            dt_warped = dt + t_overshoot

            self.env.set_dt(dt_warped)

            # Determine if the viewer is stopped. Then we can quit
            if self.viewer.is_dead:
                break

            # Pause the simulation if we are at the end
            if path_idx == len(rnn_path) - 1 or paused:
                self.env.step(paused=True, fast=fast_step)

                # If we have really hit the end of the simulation, save/plot the path
                if not paused and not path_saved:
                    # Save the final data to a file
                    datastore.store(final_path,
                                    constants.G_RNN_DYNAMIC_PATH_OUT)
                    path_saved = True

                continue

            # Not a very elegant solution to pausing at the start, but it works
            if t <= 1000.0:
                self.env.step(paused=True, fast=fast_step)
                t += dt_warped
                continue

            # Determine the current path segment
            curr_segment_idx = 0

            for segment_idx, segment_end in enumerate(segments):
                if path_idx <= segment_end:
                    curr_segment_idx = segment_idx
                    break

            x_curr = pathutils.get_path_tooltip_pos(rnn_path,
                                                    path_idx) + x_path_offset
            x_next = pathutils.get_path_tooltip_pos(
                rnn_path, path_idx + 1) + x_path_offset

            # Get the expected gate position
            x_gate_expected = pathutils.get_path_gate_pos(
                rnn_path, segments[curr_segment_idx], curr_segment_idx)

            # Get the actual gate position
            x_gate_actual = self.env.get_body_pos('gate%d' % curr_segment_idx)

            # Calculate the new position from change to new gate position
            dx_gate = x_gate_actual - (x_gate_expected + x_path_offset)
            x_new = x_next + dx_gate

            # Calculate the new velocity
            v_new = (x_new - x_curr) / dt_warped

            # Calculate the new acceleration
            a_new = (v_new - v_curr) / dt_warped

            # Calculate the acceleration vector norm
            a_new_norm = np.linalg.norm(a_new)

            # Limit the norm vector
            a_new_norm_clipped = np.clip(a_new_norm, -a_max, a_max)

            # Determine the ratio of the clipped norm, protect against divide by zero
            if a_new_norm != 0.0:
                ratio_unclipped = a_new_norm_clipped / a_new_norm
            else:
                ratio_unclipped = 0.0

            # Scale the acceleration vector by this ratio
            a_new = a_new * ratio_unclipped

            # Calculate the new change in velocity
            dv_new = a_new * dt_warped
            v_new = v_curr + dv_new

            # Calculate the new change in position
            dx_new = v_new * dt_warped
            x_new = x_curr + dx_new

            # Modify final path data with current tooltip and gate positions
            pathutils.set_path_time(final_path, path_idx, t)
            pathutils.set_path_tooltip_pos(final_path, path_idx, x_curr)

            for gate_idx in range(constants.G_NUM_GATES):
                gate_name = 'gate%d' % gate_idx
                x_gate = self.env.get_body_pos(gate_name)
                pathutils.set_path_gate_pos(final_path, path_idx, gate_idx,
                                            x_gate)

            # Store this velocity for the next time step
            v_curr = v_new

            # Recalculate the current offset
            x_path_offset += x_new - x_next

            # Perform inverse kinematics to get joint angles
            pa10_joint_angles = self.kinematics.calc_inverse_kinematics(
                x_curr, x_new)

            # TODO: TEMP - MOVE ONLY POINTER, NO PA10
            self.env.set_group_pos('pointer', x_new)

            if constants.G_TABLE_IS_OSCILLATING:
                # Move the table with y-axis oscillation
                x_table_next = shaker_table(t, x_table)
            else:
                x_table_next = x_table

            self.env.set_body_pos('table', x_table_next)

            # Step through the world by 1 time frame and actuate pa10 joints
            self.env.performAction(pa10_joint_angles, fast=fast_step)

            # Update current time after this step
            t += dt_warped
            path_idx += 1

            # Determine the difference in virtual vs actual time
            t_warped = dt - (time.time() - t_start)

            # Attempt to enforce real-time constraints
            if t_warped >= 0.0:
                # The calculation took less time than the virtual time. Sleep
                # the rest off
                time.sleep(t_warped)
                t_overshoot = 0.0
            else:
                # The calculation took more time than the virtual time. We need
                # to catch up with the virtual time on the next time step
                t_overshoot = -t_warped

        return

    def __del__(self):
        """Delete (del)

        Attempts to shut down any active engines and kills off spawned
        class objects.
        """
        # Kill the OpenGL viewer process
        if self.viewer is not None:
            self.viewer.stop()
            del self.viewer

        # Kill the ODE environment objects
        if self.env is not None:
            self.env.stop()
            del self.env

        return
Пример #6
0
    def __init__(self, randomize=False, rnn_xml=None, verbose=False):
        """Initialize

        Creates the environment and viewer objects required to run the neural
        network PA10 robotic arm simulation.

        Arguments:
            randomize: Determines if the test article gates will be randomized.
                (Default: False)
            rnn_xml: A XML filename containing neural network parameters. If
                None, a new neural network will be trained until convergence.
                (Default: None)
            verbose: Determines the level out debug output generated.
                (Default: False)
        """
        # Generate the XODE file
        XODE_FILENAME = 'model' # .xode is appended automatically

        print('>>> Generating world model')
        if os.path.exists('./'+XODE_FILENAME+'.xode'):
            os.remove('./'+XODE_FILENAME+'.xode')

        xode_model = NeuralSimWorld(
                name=XODE_FILENAME,
                randomize_test_article=randomize
        )
        xode_model.generate()

        print('>>> Generating RNN')

        # Determine if we need to train the neural network
        if rnn_xml is not None:
            print('>>> Loading RNN from file')
            self.rnn = network.PathPlanningNetwork()
            self.rnn.load_network_from_file(rnn_xml)
        else:
            print('>>> Training new RNN')
            self.rnn = network.train_path_planning_network()
            self.rnn.save_network_to_file(constants.G_RNN_XML_OUT)

        print('>>> Starting kinematics engine')
        self.kinematics = PA10Kinematics()

        # Start environment
        print('>>> Starting environment')
        self.env = EnvironmentInterface(
                xode_filename='./'+XODE_FILENAME+'.xode',
                realtime=False,
                verbose=verbose,
                gravity=constants.G_ENVIRONMENT_GRAVITY
        )

        # Start viewer
        print('>>> Starting viewer')
        self.viewer = ViewerInterface(verbose=verbose)
        self.viewer.start()

        # Set up all grouped bodies in the environment
        self.env.groups = {
            'pointer': ['tooltip', 'stick'],
        }

        return
Пример #7
0
class NeuralSimulation(object):
    """NeuralSimulation class

    Responsible for initialization of all children objects such as the
    Open Dynamics Engine environment and OpenGL viewer. Starts the main
    event loop with real-time constraints.

    Attributes:
        env: The Open Dynamics Engine environment.
        viewer: The OpenGL viewer for the ODE environment.

    Methods:
        start: Begins the main event loop.
    """
    env = None
    viewer = None
    rnn = None
    kinematics = None

    def __init__(self, randomize=False, rnn_xml=None, verbose=False):
        """Initialize

        Creates the environment and viewer objects required to run the neural
        network PA10 robotic arm simulation.

        Arguments:
            randomize: Determines if the test article gates will be randomized.
                (Default: False)
            rnn_xml: A XML filename containing neural network parameters. If
                None, a new neural network will be trained until convergence.
                (Default: None)
            verbose: Determines the level out debug output generated.
                (Default: False)
        """
        # Generate the XODE file
        XODE_FILENAME = 'model' # .xode is appended automatically

        print('>>> Generating world model')
        if os.path.exists('./'+XODE_FILENAME+'.xode'):
            os.remove('./'+XODE_FILENAME+'.xode')

        xode_model = NeuralSimWorld(
                name=XODE_FILENAME,
                randomize_test_article=randomize
        )
        xode_model.generate()

        print('>>> Generating RNN')

        # Determine if we need to train the neural network
        if rnn_xml is not None:
            print('>>> Loading RNN from file')
            self.rnn = network.PathPlanningNetwork()
            self.rnn.load_network_from_file(rnn_xml)
        else:
            print('>>> Training new RNN')
            self.rnn = network.train_path_planning_network()
            self.rnn.save_network_to_file(constants.G_RNN_XML_OUT)

        print('>>> Starting kinematics engine')
        self.kinematics = PA10Kinematics()

        # Start environment
        print('>>> Starting environment')
        self.env = EnvironmentInterface(
                xode_filename='./'+XODE_FILENAME+'.xode',
                realtime=False,
                verbose=verbose,
                gravity=constants.G_ENVIRONMENT_GRAVITY
        )

        # Start viewer
        print('>>> Starting viewer')
        self.viewer = ViewerInterface(verbose=verbose)
        self.viewer.start()

        # Set up all grouped bodies in the environment
        self.env.groups = {
            'pointer': ['tooltip', 'stick'],
        }

        return

    def start(self, fps, fast_step=False):
        """Start

        Begin the continuous event loop for the simulation. This event loop
        can be exited using the ctrl+c keyboard interrupt. Real-time
        constraints are enforced. [Hz]

        Arguments:
            fps: The value of frames per second of the simulation.
            fast_step: If True, the ODE fast step algorithm will be used.
                This is faster and requires less memory but is less accurate.
                (Default: False)
        """
        paused = False
        stopped = False

        # Define the total time for the tooltip traversal
        t_total = 20.0

        # Define the simulation frame rate
        t = 0.0 # [s]
        dt = 1.0 / float(fps) # [s]

        # Keep track of time overshoot in the case that simulation time must be
        # increased in order to maintain real-time constraints
        t_overshoot = 0.0

        # Get the initial path position (center of gate7)
        pos_start = self.env.get_body_pos('gate7') # [m]

        # Get the first position of the PA10 at rest
        pos_init = self.env.get_body_pos('tooltip') # [m]

        # Calculate the new required joint angles of the PA10
        #pa10_joint_angles = self.kinematics.calc_inverse_kinematics(pos_init, pos_start)

        # TODO: Move the PA10 end-effector to the starting position along the path

        # TODO: TEMP - Move the temporary end-effector pointer to the starting position
        self.env.set_group_pos('pointer', pos_start)

        # Generate long-term path from initial position
        t_input = np.linspace(start=0.0, stop=1.0, num=t_total/dt)
        t_input = np.reshape(t_input, (len(t_input), 1))

        rnn_path = self.rnn.extrapolate(t_input, [pos_start], len(t_input)-1)

        # Add the initial condition point back onto the data
        rnn_path = np.vstack((pos_start, rnn_path))

        # Retrieve one set of standard gate position/orientation data
        file_path = pathutils.list_data_files(constants.G_TRAINING_DATA_DIR)[0]

        gate_data = datastore.retrieve(file_path)

        gate_start_idx = constants.G_GATE_IDX
        gate_end_idx = gate_start_idx + constants.G_NUM_GATE_INPUTS

        # Reshape the gate positions data
        gate_data = gate_data[0:1,gate_start_idx:gate_end_idx]
        gate_data = np.tile(gate_data, (len(rnn_path), 1))

        # Complete the rnn path data
        rnn_path = np.hstack((t_input, gate_data, rnn_path))

        # Save generated path for later examination
        datastore.store(rnn_path, constants.G_RNN_STATIC_PATH_OUT)

        # Define a variable to hold the final path (with real-time correction)
        final_path = rnn_path[:-1].copy()
        path_saved = False

        # Detect all path segments between gates in the generated path
        segments = pathutils._detect_segments(rnn_path)

        path_idx = 0

        x_path_offset = np.array([0.0, 0.0, 0.0]) # [m]
        v_curr = np.array([0.0, 0.0, 0.0]) # [m/s]
        a_max = constants.G_MAX_ACCEL # [m/s^2]

        # Get the static table position
        x_table = self.env.get_body_pos('table')

        while not stopped:
            t_start = time.time()

            # If the last calculation took too long, catch up
            dt_warped = dt + t_overshoot

            self.env.set_dt(dt_warped)

            # Determine if the viewer is stopped. Then we can quit
            if self.viewer.is_dead:
                break

            # Pause the simulation if we are at the end
            if path_idx == len(rnn_path) - 1 or paused:
                self.env.step(paused=True, fast=fast_step)

                # If we have really hit the end of the simulation, save/plot the path
                if not paused and not path_saved:
                    # Save the final data to a file
                    datastore.store(final_path, constants.G_RNN_DYNAMIC_PATH_OUT)
                    path_saved = True

                continue

            # Not a very elegant solution to pausing at the start, but it works
            if t <= 1000.0:
                self.env.step(paused=True, fast=fast_step)
                t += dt_warped
                continue

            # Determine the current path segment
            curr_segment_idx = 0

            for segment_idx, segment_end in enumerate(segments):
                if path_idx <= segment_end:
                    curr_segment_idx = segment_idx
                    break

            x_curr = pathutils.get_path_tooltip_pos(rnn_path, path_idx) + x_path_offset
            x_next = pathutils.get_path_tooltip_pos(rnn_path, path_idx+1) + x_path_offset

            # Get the expected gate position
            x_gate_expected = pathutils.get_path_gate_pos(
                    rnn_path,
                    segments[curr_segment_idx],
                    curr_segment_idx
            )

            # Get the actual gate position
            x_gate_actual = self.env.get_body_pos('gate%d'%curr_segment_idx)

            # Calculate the new position from change to new gate position
            dx_gate = x_gate_actual - (x_gate_expected + x_path_offset)
            x_new = x_next + dx_gate

            # Calculate the new velocity
            v_new = (x_new - x_curr) / dt_warped

            # Calculate the new acceleration
            a_new = (v_new - v_curr) / dt_warped

            # Calculate the acceleration vector norm
            a_new_norm = np.linalg.norm(a_new)

            # Limit the norm vector
            a_new_norm_clipped = np.clip(a_new_norm, -a_max, a_max)

            # Determine the ratio of the clipped norm, protect against divide by zero
            if a_new_norm != 0.0:
                ratio_unclipped = a_new_norm_clipped / a_new_norm
            else:
                ratio_unclipped = 0.0

            # Scale the acceleration vector by this ratio
            a_new = a_new * ratio_unclipped

            # Calculate the new change in velocity
            dv_new = a_new * dt_warped
            v_new = v_curr + dv_new

            # Calculate the new change in position
            dx_new = v_new * dt_warped
            x_new = x_curr + dx_new

            # Modify final path data with current tooltip and gate positions
            pathutils.set_path_time(final_path, path_idx, t)
            pathutils.set_path_tooltip_pos(final_path, path_idx, x_curr)

            for gate_idx in range(constants.G_NUM_GATES):
                gate_name = 'gate%d' % gate_idx
                x_gate = self.env.get_body_pos(gate_name)
                pathutils.set_path_gate_pos(final_path, path_idx, gate_idx, x_gate)

            # Store this velocity for the next time step
            v_curr = v_new

            # Recalculate the current offset
            x_path_offset += x_new - x_next

            # Perform inverse kinematics to get joint angles
            pa10_joint_angles = self.kinematics.calc_inverse_kinematics(x_curr, x_new)

            # TODO: TEMP - MOVE ONLY POINTER, NO PA10
            self.env.set_group_pos('pointer', x_new)

            if constants.G_TABLE_IS_OSCILLATING:
                # Move the table with y-axis oscillation
                x_table_next = shaker_table(t, x_table)
            else:
                x_table_next = x_table

            self.env.set_body_pos('table', x_table_next)

            # Step through the world by 1 time frame and actuate pa10 joints
            self.env.performAction(pa10_joint_angles, fast=fast_step)

            # Update current time after this step
            t += dt_warped
            path_idx += 1

            # Determine the difference in virtual vs actual time
            t_warped = dt - (time.time() - t_start)

            # Attempt to enforce real-time constraints
            if t_warped >= 0.0:
                # The calculation took less time than the virtual time. Sleep
                # the rest off
                time.sleep(t_warped)
                t_overshoot = 0.0
            else:
                # The calculation took more time than the virtual time. We need
                # to catch up with the virtual time on the next time step
                t_overshoot = -t_warped

        return

    def __del__(self):
        """Delete (del)

        Attempts to shut down any active engines and kills off spawned
        class objects.
        """
        # Kill the OpenGL viewer process
        if self.viewer is not None:
            self.viewer.stop()
            del self.viewer

        # Kill the ODE environment objects
        if self.env is not None:
            self.env.stop()
            del self.env

        return