Exemplo n.º 1
0
 def testNumericReplicator2(self):
     """Replicator should yield the same results as QR if noise = 0.0"""
     for i in xrange(50):
         p = Dynamics.RandomDistribution(20)
         f = array([random.random() for k in xrange(20)])
         r = Dynamics._numeric_QuickReplicator(p, f)
         r2 = Dynamics._numeric_Replicator(p, f, 0.0)
         self.failUnless(AlmostEqual(r, r2))
Exemplo n.º 2
0
 def testNumericReplicator4(self):
     """Little noise should only matter a little"""
     for i in xrange(50):
         p = Dynamics.RandomDistribution(100)
         f = array([random.random() for k in xrange(100)])
         r = Dynamics._numeric_QuickReplicator(p, f)
         r2 = Dynamics._numeric_Replicator(p, f, 0.01)
         self.failUnless(AlmostEqual(r, r2, 0.0101))
Exemplo n.º 3
0
 def testFitnessCorrelation(self):
     """Correlation should make a difference"""
     G = ArrayWrapper.array(CorelatedGame)
     for k in range(20):
         p = Dynamics.RandomDistribution(8)
         compare = Dynamics._QuickFitness(p, G, 3)
         fitness = Dynamics._Fitness(p, G, 0.2, 3)
         self.failIf(AlmostEqual(fitness, compare))                       
Exemplo n.º 4
0
 def testNumericReplicator3(self):
     """Noise should matter"""
     for i in xrange(50):
         p = Dynamics.RandomDistribution(100)
         f = array([random.random() for k in xrange(100)])
         r = Dynamics._numeric_QuickReplicator(p, f)
         r2 = Dynamics._numeric_Replicator(p, f, 0.5)
         self.failIf(AlmostEqual(r, r2))            
Exemplo n.º 5
0
 def testFitness2ZeroCorrelation(self):
     """Fitness2 should return the same results QuickFitness2 if e=0.0."""
     for i in range(20):
         G = array(GenRandomGame(2, 8))
         for k in range(5):
             p = Dynamics.RandomDistribution(8)
             compare = Dynamics._QuickFitness2(p, G)
             fitness = Dynamics._Fitness2(p, G, 0.0)
             self.failUnless(AlmostEqual(fitness, compare))
Exemplo n.º 6
0
 def testFitnessZeroCorrelation(self):
     """Fitness should return the same results as QuickFitness if e=0.0."""
     for i in range(5):
         G = array(GenRandomGame(4, 7))
         for k in range(5):
             p = Dynamics.RandomDistribution(7)
             compare = Dynamics._QuickFitness(p, G, 4)
             fitness = Dynamics._Fitness(p, G, 0.0, 4)
             self.failUnless(AlmostEqual(fitness, compare))
Exemplo n.º 7
0
 def testNumericFitness2Players(self):
     """Fitness2 and numeric_Fitness2 should yield the same values"""
     for i in range(20):
         G = array(GenRandomGame(2, 8))
         for k in range(5):
             p = Dynamics.RandomDistribution(8)
             compare = Dynamics._Fitness2(p, G, 0.2)
             fitness = Dynamics._numeric_Fitness2(p, G, 0.2)
             self.failUnless(AlmostEqual(fitness, compare))
Exemplo n.º 8
0
 def testFitness2Players(self):
     """Fitness should be the same as Fitness2 in the 2 player case"""
     for i in range(20):
         G = array(GenRandomGame(2, 8))
         for k in range(5):
             p = Dynamics.RandomDistribution(8)
             compare = Dynamics._Fitness2(p, G, 0.2)
             fitness = Dynamics._Fitness(p, G, 0.2, 2)
             self.failUnless(AlmostEqual(fitness, compare))    
Exemplo n.º 9
0
 def testQuickFitness2Players(self):
     """QuickFitness should not make a difference to QF2 in the 2 player case
     """
     for i in range(20):
         G = array(GenRandomGame(2, 8))
         for k in range(5):
             p = Dynamics.RandomDistribution(8)
             compare = Dynamics._QuickFitness2(p, G)
             fitness = Dynamics._QuickFitness(p, G, 2)
             self.failUnless(AlmostEqual(fitness, compare))        
Exemplo n.º 10
0
 def testSampledFitness(self):
     """SampledFitness should yield roughly the same results as Fitness"""
     for i in range(5):
         G = array(GenRandomGame(3, 8))
         p = Dynamics.RandomDistribution(8)
         fitness = Dynamics._Fitness(p, G, 0.2, 3)
         failures = 0
         for k in range(5):
             sampled = Dynamics._SampledFitness(p, G, 0.2, 3, 5000)
             if not AlmostEqual(fitness, sampled, 0.15): failures += 1
             self.failIf(failures > 1)
Exemplo n.º 11
0
 def testSampledQuickFitness(self):
     """SampledQuickFitness should yield similar results as QuickFitness"""
     for i in range(5):
         G = array(GenRandomGame(4, 7))
         p = Dynamics.RandomDistribution(7)
         fitness = Dynamics._QuickFitness(p, G, 4)
         failures = 0
         for k in range(5):
             sampled = Dynamics._SampledQuickFitness(p, G, 4, 5000)
             if not AlmostEqual(fitness, sampled, 0.1): failures += 1
             self.failIf(failures > 1)
Exemplo n.º 12
0
    def newState(self, sample, prev_sens):

        if (self.checkCali(sample)):
            return

        # else:
        # 	# TODO does this make sense??
        # 	return

        # state = [None]*sensor_no
        sample = np.subtract(sample, self.offset)

        sample[:][0] / self.g_weight
        # print("Sample: ", sample)

        prev_sens_pos = prev_sens.states[prev_sens.state_loc - 1][4]
        state = self.getState(sample, 0, prev_sens_pos)
        #saveState
        self.states.append(state.tolist())
        self.state_loc += 1

        # make quat
        self.currentQ = dyn.makequaternion(sample)
        self.quats.append(self.currentQ)

        if (self.state_loc % 100 == 0):
            self.printStates()
Exemplo n.º 13
0
def update_typical_vals(networks, int_times, rtol = 1e-9, fraction=1.0,
                        cutoff=1e-14): 
    """
    Update the typical var values for a group of networks.

    Find the maximum of each variable over the integrations. In each network
    the typical value is set to fraction of that maximum. If that maximum value
    is less than cutoff, the typical value is set to 1.

    networks    List of networks to work with
    int_times   List of corresponding integration endpoints 
                  (ie. [(0, 100), (0, 50)])
    fraction    Relative size of typical value, compared to max value over
                the integrations.
    rtol        Relative tolerance for the integrations.
    cutoff      Values below this are considered to be zero
    """
    max_vals = {}
    for net, times in zip(networks, int_times):
        traj = Dynamics.integrate(net, times, rtol=rtol, fill_traj=True)
        for var_id in net.variables.keys():
            curr_max = max(traj.get_var_traj(var_id))
            max_vals[var_id] = max(curr_max, max_vals.get(var_id, 0))

    for var_id, val in max_vals.items():
        for net in networks:
            if net.variables.has_key(var_id):
                if val > cutoff:
                    net.set_var_typical_val(var_id, val*fraction)
                else:
                    net.set_var_typical_val(var_id, 1.0)

    return max_vals
Exemplo n.º 14
0
 def testQuickReplicator2(self):
     """Population should not change when fitnesses are equal"""
     f = [1.0]*10
     for i in xrange(50):
         p = Dynamics.RandomDistribution(10)        
         r = Dynamics._QuickReplicator(p, f)
         self.failIf(not AlmostEqual(p, r))
Exemplo n.º 15
0
    def simulate(self):

        self.t = self.start

        while self.t < self.end:

            self.t += self.t_step

            # Desired Trajectory [From Input or Trajectory] and State [From Dynamics]
            self.State_d = TR.trajectory(self.t, self.State_d)

            # Calculate Actions and Forces from Controller
            self.PhiC_last, Actions = C2D.control(self.PhiC_last, self.State,
                                                  self.State_d, self.Gains,
                                                  self.Params, self.t_step)

            # Use Dynamics to Simulate New State [Transform Axes, Estimate Derivatives
            self.State = DYN.dynamics(self.State, self.Params, Actions,
                                      self.t_step)

            ## Calculation time for above measured at around 0.5 milliseconds on average

            # Record trajectory against simulation time
            self.t_vec.append(self.t)

            self.y_vec.append(self.State[0])

            self.z_vec.append(self.State[1])

            self.phi_vec.append(self.State[4])

            self.y_des_vec.append(self.State_d[0])

            self.z_des_vec.append(self.State_d[1])
Exemplo n.º 16
0
 def mpcPlot(self):
     dy = Dynamics.VehicleDynamics()
     ref = dy.reference_trajectory(
         Numpy2Torch(self.state_history[:, -1],
                     self.state_history[:, -1].shape))
     self.state_r_history = self.state_r_history.reshape([-1, 5])
     plt.figure(1)
     plt.plot(self.state_history[:, -1],
              self.state_history[:, 0],
              label="trajectory")
     # plt.plot(state_r_history[:,-1],state_r_history[:,0], label="$trajectory_r$")
     # plt.plot(self.state_history[:,-1],self.config.a_curve * np.sin(self.config.k_curve*self.state_history[:,-1]), label="reference")
     plt.plot(self.state_history[:, -1], ref[:, 0], label="reference")
     plt.legend(loc="upper right")
     plt.figure(2)
     plt.plot(self.state_history[:, -1],
              self.state_history[:, 2],
              label="trajectory")
     # plt.plot(state_r_history[:,-1],state_r_history[:,0], label="$trajectory_r$")
     # plt.plot(self.state_history[:,-1],self.config.a_curve * np.sin(self.config.k_curve*self.state_history[:,-1]), label="reference")
     plt.plot(self.state_history[:, -1], ref[:, 2], label="reference")
     plt.legend(loc="upper right")
     plt.figure(3)
     plt.plot(self.state_history[0:-1, -1], self.control_history)
     plt.show()
Exemplo n.º 17
0
def update_typical_vals(networks, int_times, rtol = 1e-9, fraction=1.0,
                        cutoff=1e-14): 
    """
    Update the typical var values for a group of networks.

    Find the maximum of each variable over the integrations. In each network
    the typical value is set to fraction of that maximum. If that maximum value
    is less than cutoff, the typical value is set to 1.

    networks    List of networks to work with
    int_times   List of corresponding integration endpoints 
                  (ie. [(0, 100), (0, 50)])
    fraction    Relative size of typical value, compared to max value over
                the integrations.
    rtol        Relative tolerance for the integrations.
    cutoff      Values below this are considered to be zero
    """
    max_vals = {}
    for net, times in zip(networks, int_times):
        traj = Dynamics.integrate(net, times, rtol=rtol, fill_traj=True)
        for var_id in net.variables.keys():
            curr_max = max(traj.get_var_traj(var_id))
            max_vals[var_id] = max(curr_max, max_vals.get(var_id, 0))

    for var_id, val in max_vals.items():
        for net in networks:
            if net.variables.has_key(var_id):
                if val > cutoff:
                    net.set_var_typical_val(var_id, val*fraction)
                else:
                    net.set_var_typical_val(var_id, 1.0)

    return max_vals
Exemplo n.º 18
0
 def testQuickFitnessFairGame(self):
     """Fitness should return equal fitnesses a 'totally fair' game"""
     G = ArrayWrapper.array(lambda a,b,c,d: 5.0)
     for i in range(10):
         r = Dynamics.RandomDistribution(7)
         f = Dynamics._QuickFitness(r, G, 4)
         for k in range(1,7):
             self.failIf(f[k] != f[0]) 
Exemplo n.º 19
0
 def testQuickFitness2FairGame(self):
     """QuickFitness2 should yield equal fitnesses in a totally fair game"""
     G = ArrayWrapper.array(lambda a,b: 0.3)
     for i in range(20):
         p = Dynamics.RandomDistribution(5)
         f = Dynamics._QuickFitness2(p, G)
         for k in range(1,5):
             self.failIf(f[k] != f[0])       
Exemplo n.º 20
0
 def testQuickReplicator3(self):
     """Different fitness values ought to have an impact"""
     p = Dynamics.UniformDistribution(10)
     f = array([1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1])
     for i in xrange(5):
         r = Dynamics._QuickReplicator(p, f)
         for k in range(len(r)-1):
             self.failIf(r[k+1]>=r[k])
         p = r
Exemplo n.º 21
0
 def testNumericReplicator1(self):
     """Results of Replicator should be sound"""
     for i in xrange(50):
         p = Dynamics.RandomDistribution(50)
         f = array([random.random() for k in xrange(50)])
         r = Dynamics._numeric_Replicator(p, f, random.random())
         for x in r:
             self.failIf(x < 0.0 or x > 1.0)
         self.failUnless(AlmostEqual(sum(r), 1.0))
Exemplo n.º 22
0
def discrete_data(net, params, pts, interval, vars=None, random=False,
                  uncert_func=typ_val_uncert(0.1, 1e-14)):
    """
    Return a set of data points for the given network generated at the given
    parameters.

    net         Network to generate data for
    params      Parameters for this evaluation of the network
    pts         Number of data points to output
    interval    Integration interval
    vars        Variables to output data for, defaults to all species in net
    random      If False data points are distributed evenly over interval
                If True they are spread randomly and uniformly over each
                variable
    uncert_func Function that takes in a trajectory and a variable id and
                returns what uncertainty should be assumed for that variable,
                either as a scalar or a list the same length as the trajectory.
    """
    # Default for vars
    if vars is None:
        vars = net.species.keys()

    # Assign observed times to each variable
    var_times = {}
    for var in vars:
        if random:
            var_times[var] = scipy.rand(pts) * (interval[1]-interval[0]) + interval[0]
        else:
            var_times[var] = scipy.linspace(interval[0], interval[1], pts)

    # Create a sorted list of the unique times in the var_times dict
    int_times = sets.Set(scipy.ravel(var_times.values()))
    int_times.add(0)
    int_times = list(int_times)
    int_times.sort()

    # Get the trajectory
    traj = Dynamics.integrate(net, int_times, params=params, fill_traj=False)

    # Build up our data dictionary
    data = {}
    for var, times in var_times.items():
        var_data = {}
        data[var] = var_data

        # Calculate our uncertainties
        var_uncerts = uncert_func(traj, var)
        for time in times:
            val = traj.get_var_val(var, time)
            if scipy.isscalar(var_uncerts):
                uncert = var_uncerts
            else:
                index = traj._get_time_index(time)
                uncert = var_uncerts[index]
            var_data[time] = (val, uncert)
    return data
Exemplo n.º 23
0
def discrete_data(net, params, pts, interval, vars=None, random=False,
                  uncert_func=typ_val_uncert(0.1, 1e-14)):
    """
    Return a set of data points for the given network generated at the given
    parameters.

    net         Network to generate data for
    params      Parameters for this evaluation of the network
    pts         Number of data points to output
    interval    Integration interval
    vars        Variables to output data for, defaults to all species in net
    random      If False data points are distributed evenly over interval
                If True they are spread randomly and uniformly over each
                variable
    uncert_func Function that takes in a trajectory and a variable id and
                returns what uncertainty should be assumed for that variable,
                either as a scalar or a list the same length as the trajectory.
    """
    # Default for vars
    if vars == None:
        vars = net.species.keys()

    # Assign observed times to each variable
    var_times = {}
    for var in vars:
        if random:
            var_times[var] = scipy.rand(pts) * (interval[1]-interval[0]) + interval[0]
        else:
            var_times[var] = scipy.linspace(interval[0], interval[1], pts)

    # Create a sorted list of the unique times in the var_times dict
    int_times = sets.Set(scipy.ravel(var_times.values()))
    int_times.add(0)
    int_times = list(int_times)
    int_times.sort()

    # Get the trajectory
    traj = Dynamics.integrate(net, int_times, params=params, fill_traj=False)

    # Build up our data dictionary
    data = {}; 
    for var, times in var_times.items():
        var_data = {}
        data[var] = var_data
        
        # Calculate our uncertainties
        var_uncerts = uncert_func(traj, var)
        for time in times:
            val = traj.get_var_val(var, time)
            if scipy.isscalar(var_uncerts):
                uncert = var_uncerts
            else:
                index = traj._get_time_index(time)
                uncert = var_uncerts[index]
            var_data[time] = (val, uncert)
    return data
Exemplo n.º 24
0
 def testQuickReplicator4(self):
     """Extinct species should stay extinct."""
     for i in xrange(50):
         f = array([random.uniform(0.5, 1.5) for k in xrange(10)])
         x = random.randint(1, 9)
         p = Dynamics.UniformDistribution(x, 0.5)
         p = concatenate((p, array([0.0] * (10-x))))
         idx = [l for l in range(10) if p[l] == 0.0]
         r = Dynamics._QuickReplicator(p, f)
         for l in idx:
             self.failIf(r[l] != 0.0)
Exemplo n.º 25
0
    def adjustVelocitiesToConstraints(self, velocities=None, block=1):
        """Modifies the velocities to be compatible with
        the distance constraints, i.e. projects out the velocity
        components along the constrained distances.

        This operation is thread-safe; it blocks other threads that
        want to access the velocities while the data is being
        updated. If this is not desired (e.g. when calling from
        a routine that handles locking itself), the optional parameter
        |block| should be set to 0."""
        import Dynamics
        if velocities is None:
            velocities = self.velocities()
        if velocities is not None:
            if block:
                self.acquireWriteStateLock()
            try:
                Dynamics.projectVelocities(self, velocities)
            finally:
                if block:
                    self.releaseWriteStateLock()
Exemplo n.º 26
0
def calculate_msd():
    parameterFile = "parameters.json"

    parser = argparse.ArgumentParser()
    parser.add_argument("-d",
                        "--directory",
                        type=str,
                        default="./",
                        help="input directory")
    #parser.add_argument("-o", "--output", type=str, help="output directory")
    #parser.add_argument("-p", "--prefix", type=str, default="cornea",help="prefix for output file")
    parser.add_argument("-s",
                        "--skip",
                        type=int,
                        default=1100,
                        help="skip this many samples")
    parser.add_argument("-m",
                        "--howmany",
                        type=int,
                        default=30,
                        help="read this many samples")
    parser.add_argument("-t",
                        "--step",
                        type=int,
                        default=4,
                        help="step snapshots with this spacing in flow field")
    parser.add_argument("-a",
                        "--average",
                        type=int,
                        default=5,
                        help="average over these many samples in flow field")

    args = parser.parse_args()

    param = read_param_CAPMD.Param(
        p.paramsFromFile(capmd.Parameters(), parameterFile))

    sheet = Dynamics(initype="fromCSV",
                     param=param,
                     datapath='output/',
                     multiopt="many")
    skip = 0
    step = 1
    howmany = 500
    sheet.readDataMany("CAPMD", skip, step, howmany, False, readtypes=[1])
    sheet.validate_initialise()
    sheet.getMSD(False)

    plt.show()
    return 0
Exemplo n.º 27
0
    def calibrate(self, calSample):
        # print("cal_sample:\n", calSample)
        n = self.num_cal_samples
        sample = np.empty((3, 2), dtype=float)
        sample[0][0] = sum(calSample[:, 0]) / n
        sample[1][0] = sum(calSample[:, 1]) / n
        sample[2][0] = sum(calSample[:, 2]) / n
        sample[0][1] = sum(calSample[:, 3]) / n
        sample[1][1] = sum(calSample[:, 4]) / n
        sample[2][1] = sum(calSample[:, 5]) / n
        #print(sample)

        currentQ = dyn.makequaternion0(sample)

        a_body = np.quaternion(0, sample[0][0], sample[1][0], sample[2][0])
        a_nav = dyn.rotatef(currentQ, a_body)  # accel in nav frame
        v = quaternion.as_float_array(a_nav)
        u = abs(v)
        x = np.where(u == np.amax(u))
        ######## TODO
        # x = np.where(v == np.amax(abs(v)))
        # print("vx0 ", v[x[0]])
        # print(x)
        v[x[0]] = v[x[0]] + dyn.g
        # if v[x[0]] < 0:
        # 	print("vx < 0")
        # 	v[x[0]] = v[x[0]] + dyn.g
        # else:
        # 	print("vx >= 0")
        # 	v[x[0]] = v[x[0]] - dyn.g
        #print(v[x[0]])
        qNav = quaternion.as_quat_array(v)
        qBody = dyn.rotateb(self.currentQ, qNav)
        #print(qBody)
        Q = quaternion.as_float_array(qBody)

        self.offset[:, 0] = Q[1:]
        self.offset[:, 1] = sample[:, 1]
        #print(offset)
        return sample
    def get_update(self, i):

        start = time.time()

        self.t += self.t_step

        # Desired Trajectory [From Input or Trajectory] and State [From Dynamics]
        self.State_d = TR.trajectory(self.t, self.State_d)

        # Calculate Actions and Forces from Controller
        self.PhiC_last, Actions = C2D.control(self.PhiC_last, self.State,
                                              self.State_d, self.Gains,
                                              self.Params, self.t_step)

        # Use Dynamics to Simulate New State [Transform Axes, Estimate Derivatives
        self.State = DYN.dynamics(self.State, self.Params, Actions,
                                  self.t_step)

        ## Calculation time for above measured at around 0.5 milliseconds on average
        end = time.time()

        print("Simulation time")
        print(end - start)

        # Update Plot
        self.t_vec.append(self.t)

        self.p_vec.append(self.State[0])

        self.p_des_vec.append(self.State_d[0])

        start = time.time()

        self.line.set_data(self.t_vec, self.p_vec)

        #self.fig.canvas.draw()

        self.fig.canvas.blit(self.ax.bbox)

        end = time.time()

        print("Plotting time")
        print(end - start)

        if self.t > 10:
            self.terminate()

        return self.line,
Exemplo n.º 29
0
    def unitary_dynamics(self, pulse, tlist, wd, drive_qb=1, dynamics=None):
        '''
        User may call this function after created solver object
        
        pulse: the pulse envelope of drive
        tlist: time array of drive
        wd: drive frequency
        drive_qb: drive qubit, start from 1, instead of 0
        '''
        if len(pulse) != len(tlist):
            raise ValueError(
                'pulse array should have the same length with t array!')

        self.generate_drive_term(tlist, wd, drive_qb)

        #self.H = np.array([self.Hdiag for i in range(len(tlist))], dtype=qt.Qobj)
        self.H = self.H0_int + pulse * self.Hd

        if dynamics == None: dynamics = self.dynamics
        desolver = Dynamics.Sesolver()
        desolver.solve(self.H, tlist, dynamics=dynamics)
        self.U = desolver.U
Exemplo n.º 30
0
    def mpcSolution(self):

        # initialize state model
        statemodel_plt = Dynamics.VehicleDynamics()
        state = self.initial_state
        # state = torch.tensor([[0.0, 0.0, self.psi_init, 0.0, 0.0]])
        state.requires_grad_(False)
        x_ref = statemodel_plt.reference_trajectory(state[:, -1])
        state_r = state.detach().clone()  # relative state
        state_r[:, 0:4] = state_r[:, 0:4] - x_ref

        self.state_history = state.detach().numpy()
        plot_length = self.config.SIMULATION_STEPS
        self.control_history = []
        self.state_r_history = state_r
        cal_time = 0
        plt.figure(0)

        for i in range(plot_length):
            x = state_r.tolist()[0]
            time_start = time.time()
            temp, control = self.solver.mpcSolver(x, self.config.NP)
            plt.plot(temp[:, -1], temp[:, 0])
            cal_time += time.time() - time_start
            u = Numpy2Torch(control[0], (-1, self.config.ACTION_DIM))

            state, state_r = step_relative(statemodel_plt, state, u)

            self.state_history = np.append(self.state_history,
                                           state.detach().numpy(),
                                           axis=0)
            self.control_history = np.append(self.control_history,
                                             u.detach().numpy())
            self.state_r_history = np.append(self.state_history,
                                             state_r.detach().numpy())
        print("MPC calculating time: {:.3f}".format(cal_time) + "s")
        self.mpcSaveTraj()
Exemplo n.º 31
0
    def newStateS0(self, sample):

        if (self.checkCali(sample)):
            return

        # state = [None]*sensor_no
        sample = np.subtract(sample, self.offset)

        sample[:][0] / self.g_weight
        # print("Sample: ", sample)

        # init state array
        state = self.getState(sample, 1, 0)
        # use old getState using accel
        #saveState
        self.states.append(state.tolist())
        self.state_loc += 1

        # use old quat method for s0
        self.currentQ = dyn.orientation(self.currentQ, sample)
        self.quats.append(self.currentQ)

        if (self.state_loc % 100 == 0):
            self.printStates()
Exemplo n.º 32
0
    def getState(self, sample, using_a, prev_sens_pos):
        state = np.zeros((3, 5), dtype=float)

        if (using_a):
            a_body = np.quaternion(0, sample[0, 0], sample[1, 0], sample[2, 0])
            a_nav = dyn.rotatef(self.currentQ, a_body)  # accel in nav frame
            a_nav_subg = dyn.subtractg(a_nav)

            # Acceleration - body
            a_body = quaternion.as_float_array(a_body)
            state[:, 4] = a_body[1:]  # position
            # Acceleration - nav
            a_nav_subg = quaternion.as_float_array(a_nav_subg)
            state[0, 2] = a_nav_subg[1]
            state[1, 2] = a_nav_subg[2]
            state[2, 2] = a_nav_subg[3]
            # Gyroscope
            state[0, 3] = sample[0, 1]
            state[1, 3] = sample[1, 1]
            state[2, 3] = sample[2, 1]
            # Velocity
            i = self.state_loc
            state[0, 1] = dyn.speed(self.states[i - 1][1][0],
                                    self.states[i - 1][2][0],
                                    dyn.w)  #speed(v0 a t)
            state[1, 1] = dyn.speed(self.states[i - 1][1][1],
                                    self.states[i - 1][2][1], dyn.w)
            state[2, 1] = dyn.speed(self.states[i - 1][1][2],
                                    self.states[i - 1][2][2], dyn.w)
            # Position
            state[0, 0] = dyn.position(self.states[i - 1][0][0],
                                       self.states[i - 1][1][0],
                                       self.states[i - 1][2][0],
                                       dyn.w)  #position(s0,v0,a,t)
            state[1, 0] = dyn.position(self.states[i - 1][0][1],
                                       self.states[i - 1][1][1],
                                       self.states[i - 1][2][1], dyn.w)
            state[2, 0] = dyn.position(self.states[i - 1][0][2],
                                       self.states[i - 1][1][2],
                                       self.states[i - 1][2][2], dyn.w)

        else:  # not using accelerometer values

            # Gyroscope
            state[0, 3] = sample[0, 1]
            state[1, 3] = sample[1, 1]
            state[2, 3] = sample[2, 1]

            # rotate old pos about prev sensor old position
            last_pos = self.states[self.state_loc][4]
            p = dyn.orbit(np.asarray(last_pos), np.asarray(prev_sens_pos),
                          self.currentQ)
            state[:, 4] = p
            # state[0,0] = p[0]
            # state[0,1] = p[1]
            # state[0,2] = p[2]

            # Velocity
            # i = self.state_loc
            # state[0,1] = dyn.speed(self.states[i-1][1][0], self.states[i-1][2][0], dyn.w) #speed(v0 a t)
            # state[1,1] = dyn.speed(self.states[i-1][1][1], self.states[i-1][2][1], dyn.w)
            # state[2,1] = dyn.speed(self.states[i-1][1][2], self.states[i-1][2][2], dyn.w)
            # # Position
            # state[0,0] = dyn.position(self.states[i-1][0][0], self.states[i-1][1][0], self.states[i-1][2][0], dyn.w) #position(s0,v0,a,t)
            # state[1,0] = dyn.position(self.states[i-1][0][1], self.states[i-1][1][1], self.states[i-1][2][1], dyn.w)
            # state[2,0] = dyn.position(self.states[i-1][0][2], self.states[i-1][1][2], self.states[i-1][2][2], dyn.w)

        return state.T
Exemplo n.º 33
0
 def testQuickFitness2DemandGame(self):
     """Test QuickFitness with the 'demand game'"""
     fitness = Dynamics._QuickFitness2(self.pop1, self.DemandGame)
     self.failUnless(fitness[2] > fitness[1] and fitness[2] >= fitness[0])       
Exemplo n.º 34
0
import Dynamics
import Trajectory
import MinimumTime
import Shortcutting
set_printoptions(precision=5)

################# Loading the environment ########################

env = Environment()  # create openrave environment
env.SetViewer('qtcoin')  # attach viewer (optional)
env.Load('/home/cuong/Dropbox/Code/mintime/robots/twodof.robot.xml')
#env.Load('/home/cuong/Dropbox/Code/mintime/robots/arm.robot.xml')
#env.Load('robots/wam4.robot.xml')
robot = env.GetRobots()[0]
env.GetPhysicsEngine().SetGravity([0, 0, 0])
params = Dynamics.GetParams(robot)

robot.SetDOFValues(zeros(robot.GetDOF()))
robot.SetDOFVelocities(zeros(robot.GetDOF()))

robot.SetDOFValues(zeros(robot.GetDOF()))
m = robot.GetLinks()[1].GetMass()  #mass of the links
l = robot.GetLinks()[1].GetGlobalCOM()[0] * 2  #length of the links
coef = 1 / 2. * m * l * l

theta1 = 0
theta2 = 1.1
ctheta2 = cos(theta2)
robot.SetDOFValues([theta1, theta2])
M_ref = coef * array([[2 * (5 / 3. + ctheta2), 2 / 3. + ctheta2],
                      [2 / 3. + ctheta2, 2 / 3.]])
Exemplo n.º 35
0
    def get_update(self, i):

        start = time.time()

        self.t += self.t_step

        # Calculate Actions and Forces from Controller
        self.PhiC_last, Actions = C2D.control(self.PhiC_last, self.State,
                                              self.State_d, self.Gains,
                                              self.Params, self.t_step)

        # Use Dynamics to Simulate New State [Transform Axes, Estimate Derivatives
        self.State = DYN.dynamics(self.State, self.Params, Actions,
                                  self.t_step)

        ## Calculation time for above measured at around 0.5 milliseconds on average

        # Update Plot
        self.t_vec.append(self.t)

        self.y_vec.append(self.State[0])

        self.z_vec.append(self.State[1])

        self.y_des_vec.append(self.State_d[0])

        self.z_des_vec.append(self.State_d[1])

        end = time.time()

        theta = self.State[4]

        attitude2D = np.array(
            ((math.cos(theta), -1 * math.sin(theta)), (math.sin(theta),
                                                       math.cos(theta))))

        self.body = attitude2D.dot(self.base_body.T)

        self.body = self.body + np.array([[self.State[0]], [self.State[1]]])

        base = self.body[:, 0]

        top = self.body[:, 1]

        wingL = self.body[:, 2]

        wingR = self.body[:, 3]

        self.head.set_data([base[0], top[0]], [base[1], top[1]])

        self.wing.set_data([wingL[0], wingR[0]], [wingL[1], wingR[1]])

        print("simulation time")
        print(end - start)

        self.line_actual.set_data(self.y_vec, self.z_vec)

        if self.t > 20:
            self.terminate()

        return self.line_actual, self.head, self.wing
# control_history = np.empty([0, 1])
# time_init = time.time()
# for i in range(config.NP_TOTAL):
#     state, control = solver.mpc_solver(x, 60)
#     x = state[1]
#     u = control[0]
#     state_history = np.append(state_history, x)
#     control_history = np.append(control_history, u)
#     x = x.tolist()
#     print("steps:{:3d}".format(i) + " | state: " + str(x))
# print("MPC calculating time: {:.3f}".format(time.time() - time_init) + "s")
# state_history = state_history.reshape(-1,config.DYNAMICS_DIM)
# # np.savetxt(os.path.join(log_dir, 'structured_MPC_state.txt'), state_history)
# # np.savetxt(os.path.join(log_dir, 'structured_MPC_control.txt'), control_history)

statemodel_plt = Dynamics.VehicleDynamics()
if config.tire_model == 'Fiala':
    state = torch.tensor([[1.0, 0.0, config.psi_init, 0.0, config.u, 0.0]])
else:
    state = torch.tensor([[0.0, 0.0, config.psi_init, 0.0, 0.0]])
state.requires_grad_(False)
x_ref = statemodel_plt.reference_trajectory(state[:, -1])
state_r = state.detach().clone()
state_r[:, 0:4] = state_r[:, 0:4] - x_ref
state_history = state.detach().numpy()
x = np.array([0.])
plot_length = 300
control_history = []
state_r_history = state_r
cal_time = 0
plt.figure()
Exemplo n.º 37
0
def plot_comparison(simu_dir, methods):
    '''
    Plot comparison figure among ADP, MPC & open-loop solution.
    Trajectory, tracking error and control signal plot

    Parameters
    ----------
    picture_dir: string
        location of figure saved.

    '''
    num_methods = len(methods)
    picture_dir = simu_dir + "/Figures"
    config = DynamicsConfig()
    trajectory_data = []
    dy = Dynamics.VehicleDynamics()
    if os.path.exists(os.path.join(simu_dir, 'structured_MPC_state.txt')) == 0:
        print('No comparison state data!')
    else:
        if 'MPC' in methods:
            mpc_state = np.loadtxt(
                os.path.join(simu_dir, 'structured_MPC_state.txt'))
            mpc_trajectory = (mpc_state[:, 4], mpc_state[:, 0])
            trajectory_data.append(mpc_trajectory)
        if 'ADP' in methods:
            adp_state = np.loadtxt(os.path.join(simu_dir, 'ADP_state.txt'))
            adp_trajectory = (adp_state[:, 4], adp_state[:, 0])
            trajectory_data.append(adp_trajectory)
        if 'OP' in methods:
            open_loop_state = np.loadtxt(
                os.path.join(simu_dir, 'Open_loop_state.txt'))
            open_loop_trajectory = (open_loop_state[:, 4], open_loop_state[:,
                                                                           0])
            trajectory_data.append(open_loop_trajectory)
        myplot(trajectory_data,
               num_methods,
               "xy",
               fname=os.path.join(picture_dir, 'trajectory.png'),
               xlabel="longitudinal position [m]",
               ylabel="Lateral position [m]",
               legend=methods,
               legend_loc="lower left")

        heading_angle = []
        if 'MPC' in methods:
            mpc_state = np.loadtxt(
                os.path.join(simu_dir, 'structured_MPC_state.txt'))
            mpc_trajectory = (mpc_state[:, 4], 180 / np.pi * mpc_state[:, 2])
            heading_angle.append(mpc_trajectory)
        if 'ADP' in methods:
            adp_state = np.loadtxt(os.path.join(simu_dir, 'ADP_state.txt'))
            adp_trajectory = (adp_state[:, 4], 180 / np.pi * adp_state[:, 2])
            heading_angle.append(adp_trajectory)
        if 'OP' in methods:
            open_loop_state = np.loadtxt(
                os.path.join(simu_dir, 'Open_loop_state.txt'))
            open_loop_trajectory = (open_loop_state[:, 4], open_loop_state[:,
                                                                           2])
            heading_angle.append(open_loop_trajectory)
        myplot(heading_angle,
               num_methods,
               "xy",
               fname=os.path.join(picture_dir, 'trajectory_heading_angle.png'),
               xlabel="longitudinal position [m]",
               ylabel="Heading angle [degree]",
               legend=methods,
               legend_loc="lower left")

        error_data = []
        print('\nMAX TRACKING ERROR:')
        print(' MAX LATERAL POSITION ERROR:')
        if 'MPC' in methods:
            mpc_ref = dy.reference_trajectory(
                Numpy2Torch(mpc_state[:, 4], mpc_state[:, 4].shape)).numpy()
            mpc_error = (mpc_state[:, 4], mpc_state[:, 0] - mpc_ref[:, 0])
            print('  Max MPC lateral error: {:0.3E}m'.format(
                float(max(abs(mpc_state[:, 0] - mpc_ref[:, 0])))))
            error_data.append(mpc_error)
        if 'ADP' in methods:
            adp_ref = dy.reference_trajectory(
                Numpy2Torch(adp_state[:, 4], adp_state[:, 4].shape)).numpy()
            adp_error = (adp_state[:, 4], adp_state[:, 0] - adp_ref[:, 0])
            print('  Max ADP lateral error: {:0.3E}m'.format(
                float(max(abs(adp_state[:, 0] - adp_ref[:, 0])))))
            error_data.append(adp_error)
        if 'OP' in methods:
            open_loop_error = (open_loop_state[:, 4],
                               open_loop_state[:, 0] - config.a_curve *
                               np.sin(config.k_curve * open_loop_state[:, 4]))
            error_data.append(open_loop_error)
        myplot(error_data,
               num_methods,
               "xy",
               fname=os.path.join(picture_dir, 'trajectory_error.png'),
               xlabel="longitudinal position error [m]",
               ylabel="Lateral position [m]",
               legend=methods,
               legend_loc="upper left")

        # mpc_error = (mpc_state[:, 4], mpc_state[:, 0] - config.a_curve * np.sin(config.k_curve * mpc_state[:, 4]))
        # open_loop_error =  (open_loop_state[:, 4], open_loop_state[:, 0] - config.a_curve * np.sin(config.k_curve * open_loop_state[:, 4]))
        # adp_error = (adp_state[:, 4], 1 * (adp_state[:, 0] - config.a_curve * np.sin(config.k_curve * adp_state[:, 4]))) # TODO: safe delete

        #error_data = [mpc_error, adp_error, open_loop_error]
        # myplot(error_data, 3, "xy",
        #        fname=os.path.join(picture_dir,'trajectory_error.png'),
        #        xlabel="longitudinal position [m]",
        #        ylabel="Lateral position error [m]",
        #        legend=["MPC", "ADP", "Open-loop"],
        #        legend_loc="lower left"
        #        )

        psi_error_data = []
        print(' MAX YAW ANGLE ERROR:')
        if 'MPC' in methods:
            mpc_psi_error = (mpc_state[:, 4],
                             180 / np.pi * (mpc_state[:, 2] - mpc_ref[:, 2]))
            print('  MPC: {:0.3E} degree'.format(
                float(max(abs(180 / np.pi *
                              (mpc_state[:, 2] - mpc_ref[:, 2]))))))
            psi_error_data.append(mpc_psi_error)
        if 'ADP' in methods:
            adp_psi_error = (adp_state[:, 4],
                             180 / np.pi * (adp_state[:, 2] - adp_ref[:, 2]))
            print('  ADP: {:0.3E} degree'.format(
                float(max(abs(180 / np.pi *
                              (adp_state[:, 2] - adp_ref[:, 2]))))))
            psi_error_data.append(adp_psi_error)
        if 'OP' in methods:
            open_loop_psi_error = (
                open_loop_state[:, 4], 180 / np.pi *
                (open_loop_state[:, 2] -
                 np.arctan(config.a_curve * config.k_curve *
                           np.cos(config.k_curve * open_loop_state[:, 4]))))
            psi_error_data.append(open_loop_psi_error)

        myplot(psi_error_data,
               num_methods,
               "xy",
               fname=os.path.join(picture_dir, 'head_angle_error.png'),
               xlabel="longitudinal position [m]",
               ylabel="head angle error [degree]",
               legend=["MPC", "ADP", "Open-loop"],
               legend_loc="lower left")

        control_plot_data = []
        if 'MPC' in methods:
            mpc_control = np.loadtxt(
                os.path.join(simu_dir, 'structured_MPC_control.txt'))
            mpc_control_tuple = (mpc_state[1:, 4], 180 / np.pi * mpc_control)
            control_plot_data.append(mpc_control_tuple)
        if 'ADP' in methods:
            adp_control = np.loadtxt(os.path.join(simu_dir, 'ADP_control.txt'))
            adp_control_tuple = (adp_state[1:, 4], 180 / np.pi * adp_control)
            control_plot_data.append(adp_control_tuple)
        if 'OP' in methods:
            open_loop_control = np.loadtxt(
                os.path.join(simu_dir, 'Open_loop_control.txt'))
            open_loop_control_tuple = (open_loop_state[1:, 4],
                                       180 / np.pi * open_loop_control)
            control_plot_data.append(open_loop_control_tuple)

        myplot(control_plot_data,
               3,
               "xy",
               fname=os.path.join(picture_dir, 'control.png'),
               xlabel="longitudinal position [m]",
               ylabel="steering angle [degree]",
               legend=["MPC", "ADP", "Open-loop"],
               legend_loc="upper left")

        if 'OP' in methods:

            y_avs_error = []
            for [i, d] in enumerate(error_data):
                y_avs_error.append(np.mean(np.abs(d[1])))
            print("Tracking error of lateral position:")
            print("MPC:{:.3e} | ".format(y_avs_error[0]) +
                  "ADP:{:.3e} | ".format(y_avs_error[1]) +
                  "Open-loop:{:.3e} | ".format(y_avs_error[2]))

            psi_avs_error = []
            for [i, d] in enumerate(psi_error_data):
                psi_avs_error.append(np.mean(np.abs(d[1])))
            print("Tracking error of heading angle:")
            print("MPC:{:.3e} | ".format(psi_avs_error[0]) +
                  "ADP:{:.3e} | ".format(psi_avs_error[1]) +
                  "Open-loop:{:.3e} | ".format(psi_avs_error[2]))

            mpc_control_error = mpc_control - open_loop_control
            adp_control_error = adp_control - open_loop_control
            print("Control error:")
            print("MPC:{:.3e} | ".format(np.mean(np.abs(mpc_control_error))) +
                  "ADP:{:.3e} | ".format(np.mean(np.abs(adp_control_error))))
Exemplo n.º 38
0
 def enforceConstraints(self, configuration=None, velocities=None):
     """Enforces the previously defined distance constraints
     by modifying the configuration and velocities."""
     import Dynamics
     Dynamics.enforceConstraints(self, configuration)
     self.adjustVelocitiesToConstraints(velocities)
Exemplo n.º 39
0
def simulation(methods, log_dir, simu_dir):
    policy = Actor(S_DIM, A_DIM)
    value = Critic(S_DIM, A_DIM)
    config = DynamicsConfig()
    solver = Solver()
    load_dir = log_dir
    policy.load_parameters(load_dir)
    value.load_parameters(load_dir)
    statemodel_plt = Dynamics.VehicleDynamics()
    plot_length = config.SIMULATION_STEPS

    # initial_state = torch.tensor([[0.5, 0.0, config.psi_init, 0.0, 0.0]])
    # baseline = Baseline(initial_state, simu_dir)
    # baseline.mpcSolution()
    # baseline.openLoopSolution()

    # Open-loop reference
    x_init = [0.0, 0.0, config.psi_init, 0.0, 0.0]
    op_state, op_control = solver.openLoopMpcSolver(x_init, config.NP_TOTAL)
    np.savetxt(os.path.join(simu_dir, 'Open_loop_control.txt'), op_control)

    for method in methods:
        cal_time = 0
        state = torch.tensor([[0.0, 0.0, config.psi_init, 0.0, 0.0]])
        # state = torch.tensor([[0.0, 0.0, 0.0, 0.0, 0.0]])
        state.requires_grad_(False)
        # x_ref = statemodel_plt.reference_trajectory(state[:, -1])
        x_ref = statemodel_plt.reference_trajectory(state[:, -1])
        state_r = state.detach().clone()
        state_r[:, 0:4] = state_r[:, 0:4] - x_ref

        state_history = state.detach().numpy()
        control_history = []

        print('\nCALCULATION TIME:')
        for i in range(plot_length):
            if method == 'ADP':
                time_start = time.time()
                u = policy.forward(state_r[:, 0:4])
                cal_time += time.time() - time_start
            elif method == 'MPC':
                x = state_r.tolist()[0]
                time_start = time.time()
                _, control = solver.mpcSolver(x, config.NP)  # todo:retreve
                cal_time += time.time() - time_start
                u = np.array(control[0],
                             dtype='float32').reshape(-1, config.ACTION_DIM)
                u = torch.from_numpy(u)
            else:
                u = np.array(op_control[i],
                             dtype='float32').reshape(-1, config.ACTION_DIM)
                u = torch.from_numpy(u)

            state, state_r = step_relative(statemodel_plt, state, u)
            # state_next, deri_state, utility, F_y1, F_y2, alpha_1, alpha_2 = statemodel_plt.step(state, u)
            # state_r_old, _, _, _, _, _, _ = statemodel_plt.step(state_r, u)
            # state_r = state_r_old.detach().clone()
            # state_r[:, [0, 2]] = state_next[:, [0, 2]]
            # x_ref = statemodel_plt.reference_trajectory(state_next[:, -1])
            # state_r[:, 0:4] = state_r[:, 0:4] - x_ref
            # state = state_next.clone().detach()
            # s = state_next.detach().numpy()
            state_history = np.append(state_history,
                                      state.detach().numpy(),
                                      axis=0)
            control_history = np.append(control_history, u.detach().numpy())

        if method == 'ADP':
            print(" ADP: {:.3f}".format(cal_time) + "s")
            np.savetxt(os.path.join(simu_dir, 'ADP_state.txt'), state_history)
            np.savetxt(os.path.join(simu_dir, 'ADP_control.txt'),
                       control_history)

        elif method == 'MPC':
            print(" MPC: {:.3f}".format(cal_time) + "s")
            np.savetxt(os.path.join(simu_dir, 'structured_MPC_state.txt'),
                       state_history)
            np.savetxt(os.path.join(simu_dir, 'structured_MPC_control.txt'),
                       control_history)

        else:
            np.savetxt(os.path.join(simu_dir, 'Open_loop_state.txt'),
                       state_history)

    adp_simulation_plot(simu_dir)
    plot_comparison(simu_dir, methods)
LR_V = 3e-3  # learning rate of value net

# tasks
TRAIN_FLAG = 1
LOAD_PARA_FLAG = 0
SIMULATION_FLAG = 1

# Set random seed
np.random.seed(0)
torch.manual_seed(0)

# initialize policy and value net, model of vehicle dynamics
config = GeneralConfig()
policy = Actor(config.STATE_DIM, config.ACTION_DIM, lr=LR_P)
value = Critic(config.STATE_DIM, 1, lr=LR_V)
vehicleDynamics = Dynamics.VehicleDynamics()
state_batch = vehicleDynamics.initialize_state()
writer = SummaryWriter()

# Training
iteration_index = 0
if LOAD_PARA_FLAG == 1:
    print(
        "********************************* LOAD PARAMETERS *********************************"
    )
    # load pre-trained parameters
    load_dir = "./Results_dir/2020-10-09-14-42-10000"
    policy.load_parameters(load_dir)
    value.load_parameters(load_dir)

if TRAIN_FLAG == 1:
Exemplo n.º 41
0
parser.add_argument("-c", "--conffile", type=str, default="plane_periodic.conf", help="configuration file")
parser.add_argument("-d", "--directory", type=str, default="/home/sh18581/Documents/Dome/samos_check/periodic_test/data_phi_1/data_v0_0.01/data_nu_0.01/",help="input directory")
parser.add_argument("-s", "--skip", type=int, default=300, help="skip this many samples")
parser.add_argument("-m", "--howmany", type=int, default=500, help="read this many samples")
parser.add_argument("-t", "--step", type=int, default=20, help="step snapshots with this spacing")
parser.add_argument("-o", "--outfile", type=str, default="correlations.p", help="pickle file name")
args = parser.parse_args()

# Choose to show as plots as well:
plotall = True

# Use the Configuration constructor in its readCSV format to generate the topology 
# fromCSV(kwargs["parampath"],kwargs["datapath"],kwargs["multiopt"])
parampath0 = args.directory + args.conffile
param = Param(parampath0)
Cells = Dynamics(initype="fromCSV",param=param,datapath=args.directory,multiopt="many")

# Now read in as desired
# def readDataMany(self,skip=0,step=1,howmany='all',Nvariable=False,readtypes = 'all'):
Cells.readDataMany("SAMoS",args.skip,1,args.howmany,False,readtypes = 'all')
Cells.validate_initialise()

# Start collecting all of this in data dictionary
data={'configuration':args.directory,'skip':args.skip,'howmany':args.howmany,'step':args.step}

# 1st basic set of analyses
# Will save as pickle file regardless

# A - Velocity distributions and mean velocity
# vav, vdist,vdist2 = getVelDist(self,bins,bins2,usetype='all',verbose=True):
# Bins are in normalised units (by mean velocity)
Exemplo n.º 42
0
def main():

	ctrl = Controller()
	dyn = Dynamics()
class VideoStreamHandler(socketserver.StreamRequestHandler):
    command = Dynamics()
    identify_features = features()
    print("Video Server Active")

    def handle(self):
        global command
        global distance
        global previous_distance
        stream_bytes = b''
        AEB = 0
        try:
            while True:

                stream_bytes += self.rfile.read(1024)
                first = stream_bytes.find(b'\xff\xd8')
                last = stream_bytes.find(b'\xff\xd9')
                if first != -1 and last != -1:
                    jpg = stream_bytes[first:last + 2]
                    stream_bytes = stream_bytes[last + 2:]

                    #read images in stream
                    img = cv2.imdecode(np.frombuffer(jpg, dtype=np.uint8),
                                       cv2.IMREAD_COLOR)

                    #Perspective Transform
                    src = np.float32([[00, 100], [320, 100], [-10, 150],
                                      [330, 150]])
                    dst = np.float32([[0, 0], [320, 0], [0, 240], [320, 240]])
                    M = cv2.getPerspectiveTransform(src, dst)
                    image = cv2.warpPerspective(img, M, (320, 240))

                    #Image Processing -Identify Features
                    img, image, stop, average_heading, sigFlag, carFlag = self.identify_features.haar(
                        img, image)

                    #Display Processed Images and views
                    cv2.imshow('Feild Of View', img)
                    cv2.imshow('Birds Eye', image)  #img

                    #HC-SR04 distance data
                    a = (previous_distance + distance) / 2
                    b = previous_distance - distance

                    if distance < 25:  #Object under threshold distance
                        AEB = 1
                        #Emergency Brakes Flag
                    else:
                        AEB = 0

                    if cv2.waitKey(3) & 0xFF == ord('q'):
                        break

                    if sigFlag == 0:  # Signal is Green

                        #Set Lane Departure limits
                        if ((average_heading < 150) and
                            (stop == 0)) and AEB == 0:
                            print("Turn left")
                            self.command.fleft()
                        elif ((average_heading > 170) and
                              (stop == 0)) and AEB == 0:
                            print("Turn right")
                            self.command.fright()
                        elif ((170 >= average_heading >= 150) and
                              (stop == 0)) and AEB == 0:
                            print("In lane")
                            self.command.forward()
                        elif stop == 1:
                            print("BRAKES ACTIVE due to Stop sign")
                            self.command.brake()
                        elif AEB == 1:
                            self.command.brake()
                            if carFlag == 1:  #If car spotted
                                print("Car Ahead at " + str(distance) + " cm")
                            else:
                                print(
                                    "Obstacle Ahead!, Emergency Brakes Active")
                        else:
                            pass
                    elif sigFlag == 1:  # Signal is Red
                        pass
                        print("Signal Red, waiting to turn Green...")
                        self.command.brake()
        finally:
            cv2.destroyAllWindows()
            self.command.halt()
            self.command.closeconn()
            sys.exit()
Exemplo n.º 44
0
import sys
import cStringIO


set_printoptions(precision=5)
save_stdout = sys.stdout


################# Loading the environment ########################

env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('/home/cuong/Dropbox/Code/mintime/robots/twodof.robot.xml')

robot=env.GetRobots()[0]
params=Dynamics.GetParams(robot)
grav=[0,9.8,0]




################# Initialize a trajectory ########################

P0=poly1d([-2.1])
P1=poly1d([1,-2.7])
pwp_traj=Trajectory.PieceWisePolyTrajectory([[P0,P1]],[1])

T=1
n_discr=100.
t_step=T/n_discr
a=time.clock()