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
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
# Triangles Geometry
# gemotry_name = 'D'
gemotry_name = 'dscc_simulation'
grid_triangles = triangle_plot.read_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([3., 1.]) # the position of the agent
need_transition = False
b = 0.2
# pos = [3., 1.]
v_act = [pos[1] - b * pos[0], -pos[0] - b * pos[1]]
def velocity_vector(x):
b = 0.2
v = [x[1] - b * x[0], -x[0] - b * x[1]]
# print 'position=', x, 'distance', math.sqrt(x[0]**2 + x[1]**2), 'ref vel=', v
if math.sqrt(v[0]**2 + v[1]**2) > 0.4: