def compute(self, setpoint):
# Updating initial conditions
x_0 = numpy.array([
self.agent.position[0], self.agent.position[1],
self.agent.velocity[0], self.agent.velocity[1]
])
self.q = numpy.hstack([
-self.Q.dot(setpoint[0:self.nx]),
-self.Q_0.dot(setpoint[self.nx:2 * self.nx]),
numpy.dot(
sparse.kron(sparse.eye(self.N - 1), -self.Q).toarray(),
setpoint[2 * self.nx:]),
numpy.zeros(self.N * self.nu)
])
self.l[:self.nx] = -x_0
self.u[:self.nx] = -x_0
# Predict future states with constant velocity, i.e. no acceleration
for k in range(self.N):
agent_k = Agent(
self.agent.position + k * self.agent.velocity * self.Ts,
self.agent.velocity, numpy.zeros(2), self.agent.radius)
for i, collider in enumerate(self.colliders):
collider_k = Agent(
collider.position + k * collider.velocity * self.Ts,
collider.velocity, numpy.zeros(2), collider.radius)
# Discovering ORCA half-space
v0, n = orca(agent_k, collider_k, self.tau, self.Ts)
self.A[self.orca_rows_idx + i * self.N + k,
self.nx + k * self.nx + 2] = n[0]
self.A[self.orca_rows_idx + i * self.N + k,
self.nx + k * self.nx + 3] = n[1]
self.l[self.orca_rows_idx + i * self.N + k] = -numpy.inf
self.u[self.orca_rows_idx + i * self.N + k] = numpy.dot(n, v0)
self.problem.update(q=self.q, l=self.l, u=self.u, Ax=self.A.data)
result = self.problem.solve()
if result.info.status == 'solved':
# return the first resulting velocity after control action
return [
result.x[(self.nx + 2):(self.nx + 4)],
result.x[-self.N * self.nu:-(self.N - 1) * self.nu]
]
else:
print(result.info.status)
damping = 3
new_acceleration = (1 - damping) * self.agent.acceleration
return [
self.agent.velocity + new_acceleration * self.Ts,
new_acceleration
]
running = True
accum = 0
all_lines = [[]] * len(agents)
while running:
accum += clock.tick(FPS)
while accum >= dt * 1000:
accum -= dt * 1000
new_vels = [None] * len(agents)
for i, agent in enumerate(agents):
candidates = agents[:i] + agents[i + 1:]
# print(candidates)
new_vels[i], all_lines[i] = orca(agent, candidates, tau, dt)
# print(i, agent.velocity)
for i, agent in enumerate(agents):
agent.velocity = new_vels[i]
agent.position += agent.velocity * dt
screen.fill(pygame.Color(0, 0, 0))
for agent in agents[1:]:
draw_orca_circles(agents[0], agent)
for agent, color in zip(agents, itertools.cycle(colors)):
draw_agent(agent, color)
draw_velocity(agent)
# print(sqrt(norm_sq(agent.velocity)))
# pygame.draw.line(screen, pygame.Color(255, 0, 255), rint(a.position * scale + O).astype(int), rint((a.position + a.pref_velocity) * scale + O).astype(int), 1)
running = True
accum = 0
all_lines = [[]] * len(agents)
while running:
accum += clock.tick(FPS)
while accum >= dt * 1000:
accum -= dt * 1000
new_vels = [None] * len(agents)
for i, agent in enumerate(agents):
candidates = agents[:i] + agents[i + 1:]
# print(candidates)
new_vels[i], all_lines[i] = orca(agent, candidates, tau, dt)
# print(i, agent.velocity)
for i, agent in enumerate(agents):
agent.velocity = new_vels[i]
agent.position += agent.velocity * dt
screen.fill(pygame.Color(0, 0, 0))
for agent in agents[1:]:
draw_orca_circles(agents[0], agent)
for agent, color in zip(agents, itertools.cycle(colors)):
draw_agent(agent, color)
draw_velocity(agent)
# print(sqrt(norm_sq(agent.velocity)))
sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0)
sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1)
sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2)
sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3)
pub = []
pub.append(rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10))
pub.append(rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10))
pub.append(rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10))
pub.append(rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10))
t = 0
while not rospy.is_shutdown():
agents = update_agents(agents)
for i, agent in enumerate(agents):
candidates = agents[:i] + agents[i + 1:]
agent.velocity, _ = orca(agent, candidates, tau, Ts)
for i in xrange(len(X)):
vel = Twist()
vel.linear.x = agents[i].velocity[0]
vel.linear.y = agents[i].velocity[1]
pub[i].publish(vel)
if t % 100:
print(X)
#print(agents[0].velocity)
t += 1
rospy.sleep(Ts)
def command(self, rate):
goal = PointStamped()
base_goal = PointStamped()
goal.header.frame_id = "odom"
goal.header.stamp = rospy.Time()
if len(self.goals) > 3:
theta = 1
atheta = 0.1
else:
theta = 4
atheta = 0.2
if len(self.goals) < 1 or self.currentGoalIndex >= len(self.goals):
if len(self.goals) < 1:
self.publish(0, 0)
else:
rospy.loginfo("Objetivos alcanzados")
self.publish(0, 0)
self.shutdown()
rospy.signal_shutdown("Finished")
else:
goal.point.y = self.goals[self.currentGoalIndex][1]['y']
goal.point.x = self.goals[self.currentGoalIndex][1]['x']
goal.point.z = 0.0
try:
base_goal = self.listener.transformPoint(
'base_footprint', goal)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("Problem TF")
return
angular = math.atan2(base_goal.point.y, base_goal.point.x)
degrees = math.degrees(angular)
if degrees > 5:
angular = 0.3
linear = 0.1
elif degrees < -5:
angular = -0.3
linear = 0.1
else:
angular = degrees * 0.3 * 0.35
linear = 0.4
if math.sqrt((base_goal.point.x)**2 +
(base_goal.point.y)**2) < 0.5:
if self.currentGoalIndex + 1 == len(self.goals):
if math.sqrt((base_goal.point.x)**2 +
(base_goal.point.y)**2) < 0.3:
self.currentGoalIndex = self.currentGoalIndex + 1
else:
self.currentGoalIndex = self.currentGoalIndex + 1
x_vel = math.cos(angular) * linear
y_vel = math.sin(angular) * linear
self.robotAgent = Agent(
(0, 0),
(self.robotAgent.velocity[0], self.robotAgent.velocity[1]),
0.25, 0.001, (x_vel, y_vel))
[newVel, lines] = orca(self.robotAgent, self.collidingAgents,
theta, atheta)
x_vel = newVel[0]
y_vel = newVel[1]
self.robotAgent = Agent((0, 0), (x_vel, y_vel), 0.25, 0.001,
(x_vel, y_vel))
linear = math.sqrt(x_vel**2.0 + y_vel**2.0)
linear = min(linear, 0.4)
angular = angular + (math.atan2(y_vel, x_vel) - angular) * 10
angular = min(angular, 0.3)
angular = max(angular, -0.3)
self.publish(linear, angular)