def __init__(self, id): self.agent_id = id self.position = [] self.velocity = [] self.trajectory = [] self.reached = False self.triangle = [] self.new_pos = [] self.inital_position = [] self.stage = [] self.triangle_list = [] self.need_transition = False self.robot_state = [] self.pos = [] self.current_stage = 0 self.fz = 0. self.tau_x = 0. self.tau_y = 0. self.tau_z = 0. self.R_euler = [[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]] self.finished = False self.geometry_name = ['dscc_simulation', 'robot2'] self.dt = 0.05 self.xi = 0. self.yi = 0. self.psi, self.theta, self.phi = 0, 0, 0 self.p, self.q, self.r = 0., 0., 0. self.radius = 0.10 # the radius of the agent self.epsilon = 0.1 # observable point distance to the center of the agent self.g = 9.81 # gravitational acc self.m = 1. # mass self.Ixx = 0.0820 self.Iyy = 0.0820 self.Izz = 0.1377 self.J = [[self.Ixx, 0, 0], [0, self.Iyy, 0], [0, 0, self.Izz]] # agent moment of intertia self.dx = 0.0001 self.dy = 0.0001 self.islast = False self.v1 = [0., 0.] self.sigma2dot = [0., 0.] self.sigma3dot = [0., 0.] self.sigma4dot = [0., 0.] self.triangle_list = triangle_plot.read_triangles( self.geometry_name[self.agent_id - 1]) for [i, j] in self.triangle_list[self.agent_id - 1]: self.xi += i self.yi += j self.xi = self.xi / 3. self.yi = self.yi / 3. self.inital_position.append([self.xi, self.yi]) self.states = vector_field.init_field( self.geometry_name[self.agent_id - 1])[1] self.stage.append( vector_field.init_field(self.geometry_name[self.agent_id - 1])[0]) self.G = vector_field.init_field(self.geometry_name[self.agent_id - 1])[2] self.v_ref, self.islast, self.vertice1 = vector_field.vector_field( self.inital_position[0][0], self.inital_position[0][1], self.current_stage, self.states, self.G) self.v_act = self.v_ref * .9 self.pos = self.inital_position[0] self.v_max = 1.0
import numpy as np import math import time # from tkinter import * from Tkinter import * import triangle_plot import vector_field import matplotlib matplotlib.use("TkAgg") gemotry_name = 'dscc_simulation' # Triangles Geometry grid_triangles = triangle_plot.read_triangles(gemotry_name) Stages = vector_field.init_field(gemotry_name) # Create the Vector Field current_stage = 0 # robot parameters if gemotry_name == 'D': pos = np.array([0.6, 9.2]) # the position of the agent else: pos = np.array([3.0, 0.85]) # the position of the agent need_transition = False v_ref, islast = vector_field.vector_field( pos[0], pos[1], current_stage) # the rotation of the agent v_act = v_ref * .9 psi, theta, phi = 0, 0, 0 p, q, r = 0., 0., 0. radius = 0.10 # the radius of the agent epsilon = 0.1 # observable point distance to the center of the agent g = 9.81 # gravitational acc m = 1. # mass Ixx = 0.0820 Iyy = 0.0820
import numpy as np import time # from tkinter import * from Tkinter import * import triangle_plot import vector_field # Triangles Geometry gemotry_name = 'D' grid_triangles = triangle_plot.read_triangles(gemotry_name) # print grid_triangles # Create the Vector Field Stages = vector_field.init_field(gemotry_name) current_stage = 0 # robot parameters if gemotry_name == 'D': pos = np.array([0.6, 9.3]) # the position of the agent else: pos = np.array([2.0, 0.5]) # the position of the agent vel = np.zeros(2) theta = 0 # the rotation of the agent radius = 0.1 # the radius of the agent radius = 0.1 # the radius of the agent # Drawing parameters pixelsize = 780 framedelay = 60 drawVels = True QUIT = False
import math import time # from tkinter import * from Tkinter import * import triangle_plot import vector_field import Tkinter as tk # Triangles Geometry # gemotry_name='C' # gemotry_name='Proposal' gemotry_name = 'robot2' # gemotry_name='dscc_simulation' grid_triangles = triangle_plot.read_triangles(gemotry_name) # Create the Vector Field [Stages, States_v, G_v] = vector_field.init_field(gemotry_name) current_stage = 0 # robot parameters if gemotry_name == 'D': pos = np.array([0.6, 9.3]) # the position of the agent # pos = np.array([2, 7.9]) # the position of the agent # current_stage = 1 else: pos = np.array([3., 1.]) # the inital position of the agent # pos = np.array([9.1, 4.]) # the inital position of the agent pos = np.array([1.07, .63]) # the inital position of the agent # vel = np.zeros(2) # [v_ref, islast, triangle] = vector_field.vector_field(pos[0], pos[1], current_stage, States_v, G_v) # the rotation of a = 0