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