def step(self, action, dt):
"""
In the global frame, assume the agent instantaneously turns by :code:`heading`
and moves forward at :code:`speed` for :code:`dt` seconds.
Add that offset to the current position. Update the velocity in the
same way. Also update the agent's turning direction (only used by CADRL).
Args:
action (list): [delta heading angle, speed] command for this agent
dt (float): time in seconds to execute :code:`action`
"""
selected_speed = action[0]
selected_heading = wrap(action[1] + self.agent.heading_global_frame)
dx = selected_speed * np.cos(selected_heading) * dt
dy = selected_speed * np.sin(selected_heading) * dt
self.agent.pos_global_frame += np.array([dx, dy])
self.agent.vel_global_frame[0] = selected_speed * np.cos(selected_heading)
self.agent.vel_global_frame[1] = selected_speed * np.sin(selected_heading)
self.agent.speed_global_frame = selected_speed
self.agent.delta_heading_global_frame = wrap(selected_heading - self.agent.heading_global_frame)
self.agent.heading_global_frame = selected_heading
# turning dir: needed for cadrl value fn
if abs(self.agent.turning_dir) < 1e-5:
self.agent.turning_dir = 0.11 * np.sign(selected_heading)
elif self.agent.turning_dir * selected_heading < 0:
self.agent.turning_dir = max(-np.pi, min(np.pi, -self.agent.turning_dir + selected_heading))
else:
self.agent.turning_dir = np.sign(self.agent.turning_dir) * max(0.0, abs(self.agent.turning_dir)-0.1)
def step(self, action, dt):
"""
The desired change in heading divided by dt is the desired turning rate.
Clip this to remain within plus/minus max_turn_rate.
Then, propagate using the UnicycleDynamics model instead.
Should update this to call UnicycleDynamics's step instead of re-writing.
Args:
action (list): [delta heading angle, speed] command for this agent
dt (float): time in seconds to execute :code:`action`
"""
selected_speed = action[0]
turning_rate = np.clip(action[1] / dt, -self.max_turn_rate,
self.max_turn_rate)
selected_heading = wrap(turning_rate * dt +
self.agent.heading_global_frame)
dx = selected_speed * np.cos(selected_heading) * dt
dy = selected_speed * np.sin(selected_heading) * dt
self.agent.pos_global_frame += np.array([dx, dy])
self.agent.vel_global_frame[0] = selected_speed * np.cos(
selected_heading)
self.agent.vel_global_frame[1] = selected_speed * np.sin(
selected_heading)
self.agent.speed_global_frame = selected_speed
self.agent.delta_heading_global_frame = wrap(
selected_heading - self.agent.heading_global_frame)
self.agent.heading_global_frame = selected_heading
def set_state(self, px, py, vx=None, vy=None, heading=None):
""" Without doing a full reset, update the agent's current state (pos, vel, heading).
This is useful in conjunction with (:class:`~gym_collision_avoidance.envs.dynamics.ExternalDynamics.ExternalDynamics`).
For instance, if Agents in this environment should be aware of a real robot or one from a more realistic simulator (e.g., Gazebo),
you could just call set_state each sim step based on what the robot/Gazebo tells you.
If vx, vy not provided, will interpolate based on new&old position. Same for heading.
Args:
px (float or int): x position of agent in global frame right now
py (float or int): y position of agent in global frame right now
vx (float or int): x velocity of agent in global frame right now
vy (float or int): y velocity of agent in global frame right now
heading (float): angle of agent in global frame right now
"""
if vx is None or vy is None:
if self.step_num == 0:
# On first timestep, just set to zero
self.vel_global_frame = np.array([0,0])
else:
# Interpolate velocity from last pos
self.vel_global_frame = (np.array([px, py]) - self.pos_global_frame) / self.dt_nominal
else:
self.vel_global_frame = np.array([vx, vy])
if heading is None:
# Estimate heading to be the direction of the velocity vector
heading = np.arctan2(self.vel_global_frame[1], self.vel_global_frame[0])
self.delta_heading_global_frame = wrap(heading - self.heading_global_frame)
else:
self.delta_heading_global_frame = wrap(heading - self.heading_global_frame)
if heading is None:
# Estimate heading to be the direction of the velocity vector
heading = np.arctan2(self.vel_global_frame[1], self.vel_global_frame[0])
self.delta_heading_global_frame = wrap(heading - self.heading_global_frame)
else:
self.delta_heading_global_frame = wrap(heading - self.heading_global_frame)
self.pos_global_frame = np.array([px, py])
self.speed_global_frame = np.linalg.norm(self.vel_global_frame)
self.heading_global_frame = heading
def query_and_rescale_action(self, host_agent, agent_state,
other_agents_state, other_agents_actions):
""" If there's nobody around, just go straight to goal, otherwise query DNN and make heading action an offset from current heading
"""
if len(other_agents_state) > 0:
action = self.value_net.find_next_action(agent_state,
other_agents_state,
other_agents_actions)
# action[0] /= host_agent.pref_speed
action[1] = util.wrap(action[1] - host_agent.heading_global_frame)
else:
action = np.array([1.0, -self.heading_ego_frame])
return action
def find_next_action(self, obs, agents, index):
"""
Converts environment's agents representation to CADRL format.
Go at pref_speed, apply a change in heading equal to zero out current ego heading (heading to goal)
Args:
obs (dict): ignored
agents (list): of Agent objects
index (int): this agent's index in that list
Returns:
np array of shape (2,)... [spd, delta_heading]
commanded [heading delta, speed]
"""
host_agent = agents[index]
# neighbor_num = len(host_agent.neighbor_info)
neighbor_num = 2
pos_diff_sum = 0
vel_diff_sum = 0
# for idx, pos_diff in host_agent.neighbor_info.items():
# other_agent = agents[idx]
# rel_pos_to_other_global_frame = other_agent.pos_global_frame - host_agent.pos_global_frame
# pos_diff_sum += rel_pos_to_other_global_frame - pos_diff
# vel_diff_sum += other_agent.vel_global_frame #- host_agent.vel_global_frame
for i, data in enumerate(host_agent.neighbor_info):
if host_agent.neighbor_info[i] != [0, 0]:
other_agent = agents[i]
rel_pos_to_other_global_frame = other_agent.pos_global_frame - host_agent.pos_global_frame
pos_diff_sum += rel_pos_to_other_global_frame - data
vel_diff_sum += other_agent.vel_global_frame #- host_agent.vel_global_frame
vel_consensus = np.array(self.w_p * pos_diff_sum +
1 / neighbor_num * vel_diff_sum)
if index == 0:
selected_speed = host_agent.pref_speed
selected_heading = wrap(-host_agent.heading_ego_frame +
host_agent.heading_global_frame)
vel_goal = selected_speed * np.array(
[np.cos(selected_heading),
np.sin(selected_heading)])
action = vel_consensus + self.w_g * vel_goal
else:
action = vel_consensus
return action
def update_ego_frame(self):
""" Update agent's heading and velocity by converting those values from the global to ego frame.
This should be run every time :code:`step` is called (add to :code:`step`?)
"""
# Compute heading w.r.t. ref_prll, ref_orthog coordinate axes
self.agent.ref_prll, self.agent.ref_orth = self.agent.get_ref()
ref_prll_angle_global_frame = np.arctan2(self.agent.ref_prll[1],
self.agent.ref_prll[0])
self.agent.heading_ego_frame = wrap(self.agent.heading_global_frame -
ref_prll_angle_global_frame)
# Compute velocity w.r.t. ref_prll, ref_orthog coordinate axes
cur_speed = math.sqrt(self.agent.vel_global_frame[0]**2 +
self.agent.vel_global_frame[1]**
2) # much faster than np.linalg.norm
v_prll = cur_speed * np.cos(self.agent.heading_ego_frame)
v_orthog = cur_speed * np.sin(self.agent.heading_ego_frame)
self.agent.vel_ego_frame = np.array([v_prll, v_orthog])
def near_goal_smoother(self, dist_to_goal, pref_speed, heading,
raw_action):
""" Linearly ramp down speed/turning if agent is near goal, stop if close enough.
I think this is just here for convenience, but nobody uses it? We used it on the jackal for sure.
"""
kp_v = 0.5
kp_r = 1
if dist_to_goal < 2.0:
near_goal_action = np.empty((2, 1))
pref_speed = max(min(kp_v * (dist_to_goal - 0.1), pref_speed), 0.0)
near_goal_action[0] = min(raw_action[0], pref_speed)
turn_amount = max(min(kp_r * (dist_to_goal - 0.1), 1.0),
0.0) * raw_action[1]
near_goal_action[1] = wrap(turn_amount + heading)
if dist_to_goal < 0.3:
near_goal_action = np.array([0., 0.])
else:
near_goal_action = raw_action
return near_goal_action
