def updateSim(dt):
global pos, current_stage, v_ref, theta, v_ref_pre, theta_ref_pre, u_pre, need_transition
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
x = np.matmul(R, np.array([[epsilon], [0]])).transpose()[0] + pos
print 'aa', vector_field.vector_field(x[0], x[1], current_stage)
[v_ref, islast, a] = vector_field.vector_field(x[0], x[1], current_stage)
u = np.zeros(2)
if not np.any(v_ref):
v_ref = -v_ref_pre
theta_ref = -theta_ref_pre
else:
theta_ref = math.atan2(v_ref[1], v_ref[0])
if islast and current_stage < Stages - 1:
need_transition = True
print 'need transition need transition need transition need transition need transition'
u[0] = -u_pre[0]
if need_transition:
theta_err = theta_ref - theta
if theta_err < np.pi / 8:
# if np.linalg.norm(v_ref) < 0.2 :
need_transition = False
current_stage += 1
[v_ref, _] = vector_field.vector_field(x[0], x[1], current_stage)
else:
theta_err = theta_ref - theta
theta_err = np.fmod(theta_err, 2 * np.pi)
u[1] = 0.5 * theta_err
# u[1] = min([u[1], 1])
# u[1] = max([u[1], -1])
if not need_transition:
u_pre = [0., 0.]
v_out = np.matmul(R * E2, u_pre)
v_err = 2. * (v_ref - v_out)
E2invR = np.matmul(E2inv, R.transpose())
u = np.matmul(E2invR, v_err)
# u = np.matmul(E2inv * R.transpose(), v_err)
# u[1] = (-np.sin(theta)*v_err[0]+np.cos(theta)*v_err[1])/epsilon
# u[0] = min([u[0], 1])
# u[0] = max([u[0], -1])
# u[1] = min([u[1], 1])
# u[1] = max([u[1], -1])
# u[1] = (-np.sin(theta)*v_err[0]+np.cos(theta)*v_err[1])/epsilon
# print u, need_transition
u += u_pre
# print 'u_pre', u_pre
theta += u[1] * dt # update the orintation
# theta += test * dt # update the orintation
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
posdot = np.matmul(R, np.array([u[0], 0]))
pos += posdot * dt # update the position
theta = np.fmod(theta, 2 * np.pi)
v_ref_pre = v_ref
theta_ref_pre = theta_ref
def updateSim(dt):
global pos, current_stage, vel
# find velocity
vel, islast = vector_field.vector_field(pos[0], pos[1], current_stage)
if islast and current_stage < Stages - 1:
current_stage += 1
# vel = np.array([0.5, 0.5])
pos += vel * dt #update the position
def updateSim(dt):
# print robot1.pos, robot1.current_stage, robot2.pos, robot2.current_stage
robot2.islast, robot2.v_ref, robot2.sigma2dot, robot2.sigma3dot, robot2.sigma4dot = robot2.flatoutput(
robot2.pos, robot2.current_stage)
robot1.islast, robot1.v_ref, robot1.sigma2dot, robot1.sigma3dot, robot1.sigma4dot = robot1.flatoutput(
robot1.pos, robot1.current_stage)
if robot1.finished == False:
robot1.control_inputs(robot1.islast, robot1.v_ref, robot1.sigma2dot,
robot1.sigma3dot, robot1.sigma4dot)
if robot1.islast and robot1.current_stage < robot1.stage[0] - 1:
robot1.need_transition = True
robot1.current_stage += 1
print 'need transition'
if robot1.need_transition:
robot1.need_transition = False
robot1.v_ref, robot1.islast, robot1.vertice1 = vector_field.vector_field(
robot1.pos[0], robot1.pos[1], robot1.current_stage)
if not robot1.need_transition:
robot1.pos, robot1.v_ref, robot1.v_act, robot1.phi, robot1.theta, robot1.wb = \
robot1.robot_dynamics(robot1.pos, robot1.phi, robot1.theta, robot1.v_ref, robot1.fz, robot1.tau_x,
robot1.tau_y, robot1.tau_z, robot1.p, robot1.q, robot1.r, robot1.R_euler,
robot1.v_act)
if robot2.finished == False:
robot2.control_inputs(robot2.islast, robot2.v_ref, robot2.sigma2dot,
robot2.sigma3dot, robot2.sigma4dot)
if robot2.islast and robot2.current_stage < robot2.stage[0] - 1:
robot2.need_transition = True
robot2.current_stage += 1
print 'need transition'
if robot2.need_transition:
robot2.need_transition = False
robot2.v_ref, robot2.islast, robot2.vertice1 = vector_field.vector_field(
robot2.pos[0], robot2.pos[1], robot2.current_stage)
if not robot2.need_transition:
robot2.pos, robot2.v_ref, robot2.v_act, robot2.phi, robot2.theta, robot2.wb = \
robot2.robot_dynamics(robot2.pos, robot2.phi, robot2.theta, robot2.v_ref, robot2.fz, robot2.tau_x,
robot2.tau_y, robot2.tau_z, robot2.p, robot2.q, robot2.r, robot2.R_euler,
robot2.v_act)
def updateSim(dt):
global pos, current_stage, vel, theta, vel_pre, theta_ref_pre, u_pre, need_transition
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
x = np.matmul(R, np.array([[epsilon], [0]])).transpose()[0] + pos
# find velocity
vel, islast = vector_field.vector_field(x[0], x[1], current_stage)
if np.any(vel):
theta_ref = math.atan2(vel[1], vel[0])
else:
vel = .5 * vel_pre
theta_ref = theta_ref_pre
if islast and current_stage < Stages - 1:
need_transition = True
if need_transition and np.linalg.norm(vel) < 0.2:
need_transition = False
current_stage += 1
if abs(theta_ref_pre - theta_ref) > np.pi:
theta_ref += 2 * np.pi
# u = np.matmul(E2inv * R.transpose(), vel)
theta_err = theta_ref - theta
theta_err = np.fmod(theta_err, 2 * np.pi)
# print theta_ref, theta, theta_r
u = np.zeros(2)
u[1] = 1.5 * theta_err
u[1] = min([u[1], 1])
u[1] = max([u[1], -1])
if abs(theta_err) > np.pi / 6:
u[0] = 0
else:
u[0] = .45 * np.linalg.norm(vel) / np.cos(theta_err)
u[0] = min([u[0], 1])
u[0] = max([u[0], 0])
# print 'u', u
theta += u[1] * dt # update the orintation
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
posdot = np.matmul(R, np.array([u[0], 0]))
pos += posdot * dt # update the position
theta = np.fmod(theta, 2 * np.pi)
vel_pre = vel
theta_ref_pre = theta_ref
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
def flatoutput(self, pos, current_stage):
self.pos2 = pos + [self.dx, 0]
self.pos3 = self.pos2 + [self.dx, 0]
self.pos4 = self.pos3 + [self.dx, 0]
self.pos5 = pos + [0, self.dy]
self.pos6 = self.pos5 + [self.dx, 0]
self.pos7 = self.pos6 + [self.dx, 0]
self.pos8 = self.pos5 + [0, self.dy]
self.pos9 = self.pos8 + [self.dx, 0]
self.pos10 = self.pos8 + [0, self.dy]
self.v1, self.islast, self.vertice1 = vector_field.vector_field(
self.pos[0], self.pos[1], self.current_stage, self.states, self.G)
self.v2, self.islast, self.vertice1 = vector_field.vector_field(
self.pos2[0], self.pos2[1], self.current_stage, self.states,
self.G)
self.v3, self.islast, self.vertice1 = vector_field.vector_field(
self.pos3[0], self.pos3[1], self.current_stage, self.states,
self.G)
self.v4, self.islast, self.vertice1 = vector_field.vector_field(
self.pos4[0], self.pos4[1], self.current_stage, self.states,
self.G)
self.v5, self.islast, self.vertice1 = vector_field.vector_field(
self.pos5[0], self.pos5[1], self.current_stage, self.states,
self.G)
self.v6, self.islast, self.vertice1 = vector_field.vector_field(
self.pos6[0], self.pos6[1], self.current_stage, self.states,
self.G)
self.v7, self.islast, self.vertice1 = vector_field.vector_field(
self.pos7[0], self.pos7[1], self.current_stage, self.states,
self.G)
self.v8, self.islast, self.vertice1 = vector_field.vector_field(
self.pos8[0], self.pos8[1], self.current_stage, self.states,
self.G)
self.v9, self.islast, self.vertice1 = vector_field.vector_field(
self.pos9[0], self.pos9[1], self.current_stage, self.states,
self.G)
self.v10, self.islast, self.vertice1 = vector_field.vector_field(
self.pos10[0], self.pos10[1], self.current_stage, self.states,
self.G)
if self.v1 == None or self.v2 == None or self.v3 == None or self.v4 == None or self.v5 == None:
self.finished = True
else:
self.v1dot = [(self.v2[0] - self.v1[0]) / self.dx * self.v1[0] +
(self.v5[0] - self.v1[0]) / self.dy * self.v1[1],
(self.v2[1] - self.v1[1]) / self.dx * self.v1[0] +
(self.v5[1] - self.v1[1]) / self.dy * self.v1[1]]
self.v2dot = [(self.v3[0] - self.v2[0]) / self.dx * self.v2[0] +
(self.v6[0] - self.v2[0]) / self.dy * self.v2[1],
(self.v3[1] - self.v2[1]) / self.dx * self.v2[0] +
(self.v6[1] - self.v2[1]) / self.dy * self.v2[1]]
self.v3dot = [(self.v4[0] - self.v3[0]) / self.dx * self.v3[0] +
(self.v7[0] - self.v3[0]) / self.dy * self.v3[1],
(self.v4[1] - self.v3[1]) / self.dx * self.v3[0] +
(self.v7[1] - self.v3[1]) / self.dy * self.v3[1]]
self.v5dot = [(self.v6[0] - self.v5[0]) / self.dx * self.v5[0] +
(self.v8[0] - self.v5[0]) / self.dy * self.v5[1],
(self.v6[1] - self.v5[1]) / self.dx * self.v5[0] +
(self.v8[1] - self.v5[1]) / self.dy * self.v5[1]]
self.v6dot = [(self.v7[0] - self.v6[0]) / self.dx * self.v6[0] +
(self.v9[0] - self.v6[0]) / self.dy * self.v6[1],
(self.v7[1] - self.v6[1]) / self.dx * self.v6[0] +
(self.v9[1] - self.v6[1]) / self.dy * self.v6[1]]
self.v8dot = [(self.v9[0] - self.v8[0]) / self.dx * self.v8[0] +
(self.v10[0] - self.v8[0]) / self.dy * self.v8[1],
(self.v9[1] - self.v8[1]) / self.dx * self.v8[0] +
(self.v10[1] - self.v8[1]) / self.dy * self.v8[1]]
self.v1dotdot = [
(self.v2dot[0] - self.v1dot[0]) / self.dx * self.v1[0] +
(self.v5dot[0] - self.v1dot[0]) / self.dy * self.v1[1],
(self.v2dot[1] - self.v1dot[1]) / self.dx * self.v1[0] +
(self.v5dot[1] - self.v1dot[1]) / self.dy * self.v1[1]
]
self.v2dotdot = [
(self.v3dot[0] - self.v2dot[0]) / self.dx * self.v2[0] +
(self.v6dot[0] - self.v2dot[0]) / self.dy * self.v2[1],
(self.v3dot[1] - self.v2dot[1]) / self.dx * self.v2[0] +
(self.v6dot[1] - self.v2dot[1]) / self.dy * self.v2[1]
]
self.v5dotdot = [
(self.v6dot[0] - self.v5dot[0]) / self.dx * self.v5[0] +
(self.v8dot[0] - self.v5dot[0]) / self.dy * self.v5[1],
(self.v6dot[1] - self.v5dot[1]) / self.dx * self.v5[0] +
(self.v8dot[1] - self.v5dot[1]) / self.dy * self.v5[1]
]
self.v1dotdotdot = [
(self.v2dotdot[0] - self.v1dotdot[0]) / self.dx * self.v1[0] +
(self.v5dotdot[0] - self.v1dotdot[0]) / self.dy * self.v1[1],
(self.v2dotdot[1] - self.v1dotdot[1]) / self.dx * self.v1[0] +
(self.v5dotdot[1] - self.v1dotdot[1]) / self.dy * self.v1[1]
]
self.sigma2dot = self.v1dot
self.sigma3dot = self.v1dotdot
self.sigma4dot = self.v1dotdotdot
return self.islast, self.v1, self.sigma2dot, self.sigma3dot, self.sigma4dot
def flatoutput(pos, current_stage):
print 'current stage=', current_stage
dx = 0.0001
dy = 0.0001
pos2 = pos + [dx, 0]
pos3 = pos2 + [dx, 0]
pos4 = pos3 + [dx, 0]
pos5 = pos + [0, dy]
pos6 = pos5 + [dx, 0]
pos7 = pos6 + [dx, 0]
pos8 = pos5 + [0, dy]
pos9 = pos8 + [dx, 0]
pos10 = pos8 + [0, dy]
v1, islast = vector_field.vector_field(pos[0], pos[1], current_stage)
v2, islast = vector_field.vector_field(pos2[0], pos2[1], current_stage)
v3, islast = vector_field.vector_field(pos3[0], pos3[1], current_stage)
v4, islast = vector_field.vector_field(pos4[0], pos4[1], current_stage)
v5, islast = vector_field.vector_field(pos5[0], pos5[1], current_stage)
v6, islast = vector_field.vector_field(pos6[0], pos6[1], current_stage)
v7, islast = vector_field.vector_field(pos7[0], pos7[1], current_stage)
v8, islast = vector_field.vector_field(pos8[0], pos8[1], current_stage)
v9, islast = vector_field.vector_field(pos9[0], pos9[1], current_stage)
v10, islast = vector_field.vector_field(pos10[0], pos10[1], current_stage)
v1dot = [(v2[0] - v1[0]) / dx * v1[0] + (v5[0] - v1[0]) / dy * v1[1],
(v2[1] - v1[1]) / dx * v1[0] + (v5[1] - v1[1]) / dy * v1[1]]
v2dot = [(v3[0] - v2[0]) / dx * v2[0] + (v6[0] - v2[0]) / dy * v2[1],
(v3[1] - v2[1]) / dx * v2[0] + (v6[1] - v2[1]) / dy * v2[1]]
v3dot = [(v4[0] - v3[0]) / dx * v3[0] + (v7[0] - v3[0]) / dy * v3[1],
(v4[1] - v3[1]) / dx * v3[0] + (v7[1] - v3[1]) / dy * v3[1]]
v5dot = [(v6[0] - v5[0]) / dx * v5[0] + (v8[0] - v5[0]) / dy * v5[1],
(v6[1] - v5[1]) / dx * v5[0] + (v8[1] - v5[1]) / dy * v5[1]]
v6dot = [(v7[0] - v6[0]) / dx * v6[0] + (v9[0] - v6[0]) / dy * v6[1],
(v7[1] - v6[1]) / dx * v6[0] + (v9[1] - v6[1]) / dy * v6[1]]
v8dot = [(v9[0] - v8[0]) / dx * v8[0] + (v10[0] - v8[0]) / dy * v8[1],
(v9[1] - v8[1]) / dx * v8[0] + (v10[1] - v8[1]) / dy * v8[1]]
v1dotdot = [
(v2dot[0] - v1dot[0]) / dx * v1[0] +
(v5dot[0] - v1dot[0]) / dy * v1[1],
(v2dot[1] - v1dot[1]) / dx * v1[0] + (v5dot[1] - v1dot[1]) / dy * v1[1]
]
v2dotdot = [
(v3dot[0] - v2dot[0]) / dx * v2[0] +
(v6dot[0] - v2dot[0]) / dy * v2[1],
(v3dot[1] - v2dot[1]) / dx * v2[0] + (v6dot[1] - v2dot[1]) / dy * v2[1]
]
v5dotdot = [
(v6dot[0] - v5dot[0]) / dx * v5[0] +
(v8dot[0] - v5dot[0]) / dy * v5[1],
(v6dot[1] - v5dot[1]) / dx * v5[0] + (v8dot[1] - v5dot[1]) / dy * v5[1]
]
v1dotdotdot = [(v2dotdot[0] - v1dotdot[0]) / dx * v1[0] +
(v5dotdot[0] - v1dotdot[0]) / dy * v1[1],
(v2dotdot[1] - v1dotdot[1]) / dx * v1[0] +
(v5dotdot[1] - v1dotdot[1]) / dy * v1[1]]
sigma2dot = v1dot
sigma3dot = v1dotdot
sigma4dot = v1dotdotdot
return islast, v1, sigma2dot, sigma3dot, sigma4dot
def updateSim(dt):
global g, pos, current_stage, v_ref, v_act, phi, theta, psi, need_transition, p, q, r, u, Ixx, Iyy, Izz
islast, v_ref, sigma2dot, sigma3dot, sigma4dot = flatoutput(
pos, current_stage)
beta_a = -sigma2dot[0]
beta_b = g
beta_ab = math.sqrt(beta_a**2 + beta_b**2)
beta_c = sigma2dot[1]
theta = math.atan2(beta_a, beta_b)
phi = math.atan2(beta_c, beta_ab)
psi = 0
R_euler = [[
np.cos(theta) * np.cos(psi),
np.sin(phi) * np.sin(theta) * np.cos(psi) - np.cos(phi) * np.sin(psi),
np.cos(phi) * np.sin(theta) * np.cos(psi) - np.sin(phi) * np.sin(psi)
],
[
np.cos(theta) * np.sin(psi),
np.sin(phi) * np.sin(theta) * np.sin(psi) -
np.cos(phi) * np.cos(psi),
np.cos(phi) * np.sin(theta) * np.sin(psi) -
np.sin(phi) * np.cos(psi)
],
[
-np.sin(theta),
np.sin(phi) * np.cos(theta),
np.cos(phi) * np.cos(theta)
]]
p = sigma3dot[1] * math.sqrt(sigma2dot[0]**2 + g**2) - sigma2dot[
1] * sigma3dot[0] * sigma2dot[0] / math.sqrt(sigma2dot[0]**2 + g**2)
q = -g * sigma3dot[0] / (sigma2dot[0]**2 + g**2)
r = 0
vzegond = ((sigma3dot[0]**2 + sigma2dot[0] * sigma4dot[0]) * (sigma2dot[0]**2 + g**2) - (sigma2dot[0] *
sigma3dot[0])) /\
(sigma2dot[0]**2 + g**2)**1.5
numerator1 = (sigma4dot[1] * math.sqrt(sigma2dot[0]**2 + g**2) -
sigma2dot[1] * vzegond)
numerator2 = 2. * (sigma3dot[1] * math.sqrt(sigma2dot[0]**2 + g**2) - (sigma2dot[1] * sigma3dot[0] * sigma2dot[0] /\
(math.sqrt(sigma2dot[0]**2 + g**2))))\
* (sigma3dot[1] * sigma2dot[1] + sigma3dot[0] * sigma2dot[0])
p_dot = (numerator1 + numerator2) / (sigma2dot[0]**2 + sigma2dot[1]**2 +
g**2)**2
q_dot = (-sigma4dot[0] * g * (sigma2dot[0]**2 + g**2) + 2. * g * sigma2dot[0] * sigma3dot[0]**2) /\
(sigma2dot[0]**2 + g**2)**2
r_dot = 0
# ======================================================================================================================
# Control Inputs Calculation based on Flat Outputs
# ======================================================================================================================
kv = -.9
# print 'v act', v_act
ev = math.sqrt((v_act[0] - v_ref[0])**2 + (v_act[1] - v_ref[1])**2)
# print 'velocity error=', ev
# print 'sigma2dot[0]', - m * math.sqrt(sigma2dot[0]**2 + sigma2dot[1]**2 + g**2)
fz = -kv * ev - m * math.sqrt(sigma2dot[0]**2 + sigma2dot[1]**2 + g**2)
# print 'fz vector', fz
tau_x = Ixx * p_dot + (Izz - Iyy) * r * q
tau_y = Iyy * q_dot + (Ixx - Izz) * p * r
tau_z = Izz * r_dot + (Iyy - Ixx) * p * q
# print 'tau_x,tau_y,tau_z', tau_x, tau_y, tau_z
if islast and current_stage < Stages - 1:
need_transition = True
print 'need transition'
if need_transition:
need_transition = False
current_stage += 1
v_ref, islast = vector_field.vector_field(pos[0], pos[1],
current_stage)
if not need_transition:
pos, v_ref, v_act, phi, theta, wb = robot_dynamics(
pos, phi, theta, v_ref, fz, tau_x, tau_y, tau_z, p, q, r, R_euler,
v_act)
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
Izz = 0.1377
J = [[Ixx, 0, 0], [0, Iyy, 0], [0, 0, Izz]] # agent moment of intertia
# Drawing parameters
pixelsize = 780
framedelay = 10
drawVels = True
# 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
islast = True
print 'aaa', vector_field.vector_field(pos[0], pos[1], current_stage)
[v_ref, islast, a] = vector_field.vector_field(pos[0], pos[1], current_stage)
# vv = vector_field.vector_field(pos[0], pos[1], current_stage) # the rotation of
# v= vv[0]
# the
# agent
u_pre = v_ref
v_ref_pre = v_ref
print 'vref = ', v_ref
theta = math.atan2(v_ref[1], v_ref[0])
theta_ref_pre = theta
print '/theta = ', theta
need_transition = False
radius = 0.05 # the radius of the agent
epsilon = 0.1
E1 = np.array([[0., -1.0], [1.0, 0.0]])
def flatoutput(pos, current_stage):
global dt
v1, islast = vector_field.vector_field(pos[0], pos[1], current_stage)
######################################################################################################
# sigma2:4 calculation ### vector field derivations - first method
######################################################################################################
# print 'reference velocity', v1
# velocity_angle_p1 = math.atan2(v1[1], v1[0])
# pos1 = (pos + [v1 * math.cos(velocity_angle_p1) * dt, v1 * math.sin(velocity_angle_p1) * dt])[0]
# print 'position0', pos, 'position1', pos1
# v2, islast = vector_field.vector_field(pos1[0], pos1[1], current_stage)
# print 'velocity1', v1, 'velocity2', v2
# velocity_angle_p2 = math.atan2(v2[1], v2[0])
# sigma2dot = (v2 - v1) / dt
# print 'refernce acceleration', sigma2dot
# pos2 = (pos1 + [v2 * math.cos(velocity_angle_p2) * dt, v2 * math.sin(velocity_angle_p2) * dt])[0]
# v3, islast = vector_field.vector_field(pos2[0], pos2[1], current_stage)
# # print 'v3', v3
# velocity_angle_p3 = math.atan2(v3[1], v3[0])
# v2dot = (v3 - v2) / dt
# sigma3dot = (v2dot - sigma2dot) /dt
# pos3 = (pos2 + [v3 * math.cos(velocity_angle_p3) * dt, v3 * math.sin(velocity_angle_p3) * dt])[0]
# v4, islast = vector_field.vector_field(pos3[0], pos3[1], current_stage)
# v3dot = (v4 - v3) / dt
# v2dotdot = (v3dot - v2dot) / dt
# sigma4dot = (sigma3dot - v2dotdot) /dt
######################################################################################################
# sigma2:4 calculation ### vector field derivations - second method
######################################################################################################
# dx = 0.001
# dy = 0.001
# pos2 = pos + [dx, 0]
# pos3 = pos2 + [dx, 0]
# pos4 = pos3 + [dx, 0]
# pos5 = pos + [0, dy]
# pos6 = pos5 + [dx, 0]
# pos7 = pos6 + [dx, 0]
# pos8 = pos5 + [0, dy]
# pos9 = pos8 + [dx, 0]
# pos10 = pos8 + [0, dy]
# v2, islast = vector_field.vector_field(pos2[0], pos2[1], current_stage)
# v3, islast = vector_field.vector_field(pos3[0], pos3[1], current_stage)
# v4, islast = vector_field.vector_field(pos4[0], pos4[1], current_stage)
# v5, islast = vector_field.vector_field(pos5[0], pos5[1], current_stage)
# v6, islast = vector_field.vector_field(pos6[0], pos6[1], current_stage)
# v7, islast = vector_field.vector_field(pos7[0], pos7[1], current_stage)
# v8, islast = vector_field.vector_field(pos8[0], pos8[1], current_stage)
# v9, islast = vector_field.vector_field(pos9[0], pos9[1], current_stage)
# v10, islast = vector_field.vector_field(pos10[0], pos10[1], current_stage)
# v1dot = [(v2[0] - v1[0]) / dx * v1[0] + (v2[1] - v1[1]) / dy * v1[1],
# (v5[0] - v1[0]) / dx * v1[0] + (v5[1] - v1[1]) / dy * v1[1]]
# print 'v1dot or sigma dobule dot = ', v1dot
# v2dot = [(v3[0] - v2[0]) / dx * v2[0] + (v3[1] - v2[1]) / dy * v2[1],
# (v6[0] - v2[0]) / dx * v2[0] + (v6[1] - v2[1]) / dy * v2[1]]
# v3dot = [(v4[0] - v3[0]) / dx * v3[0] + (v4[1] - v3[1]) / dy * v3[1],
# (v7[0] - v3[0]) / dx * v3[0] + (v7[1] - v3[1]) / dy * v3[1]]
# v5dot = [(v6[0] - v5[0]) / dx * v5[0] + (v6[1] - v5[1]) / dy * v5[1],
# (v8[0] - v5[0]) / dx * v5[0] + (v8[1] - v5[1]) / dy * v5[1]]
# v6dot = [(v7[0] - v6[0]) / dx * v6[0] + (v7[1] - v6[1]) / dy * v6[1],
# (v9[0] - v6[0]) / dx * v6[0] + (v9[1] - v6[1]) / dy * v6[1]]
# v8dot = [(v9[0] - v8[0]) / dx * v8[0] + (v9[1] - v8[1]) / dy * v8[1],
# (v10[0] - v8[0]) / dx * v8[0] + (v10[1] - v8[1]) / dy * v8[1]]
#
# v1dotdot = [(v2dot[0] - v1dot[0]) / dx * v1dot[0] + (v2dot[1] - v1dot[1]) / dy * v1dot[1],
# (v5dot[0] - v1dot[0]) / dx * v1dot[0] + (v5dot[1] - v1dot[1]) / dy * v1dot[1]]
# v2dotdot = [(v3dot[0] - v2dot[0]) / dx * v2dot[0] + (v3dot[1] - v2dot[1]) / dy * v2dot[1],
# (v6dot[0] - v2dot[0]) / dx * v2dot[0] + (v6dot[1] - v2dot[1]) / dy * v2dot[1]]
# v5dotdot = [(v6dot[0] - v5dot[0]) / dx * v5dot[0] + (v6dot[1] - v5dot[1]) / dy * v5dot[1],
# (v8dot[0] - v5dot[0]) / dx * v5dot[0] + (v8dot[1] - v5dot[1]) / dy * v5dot[1]]
#
# v1dotdotdot = [(v2dotdot[0] - v1dotdot[0]) / dx * v1dotdot[0] + (v2dotdot[1] - v1dotdot[1]) / dy * v1dotdot[1],
# (v5dotdot[0] - v1dotdot[0]) / dx * v1dotdot[0] + (v5dotdot[1] - v1dotdot[1]) / dy * v1dotdot[1]]
# sigma2dot = v1dot
# sigma3dot = v1dotdot
# sigma4dot = v1dotdotdot
return sigma2dot, sigma3dot, sigma4dot