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 step(self, action, dt):
selected_speed = action[0]
turning_rate = np.clip(action[1] / dt, -self.max_turn_rate,
self.max_turn_rate)
linear_acc = np.clip(self.kp * (selected_speed - self.current_speed),
-self.max_linear_acc, self.max_linear_acc)
turn_acc = np.clip(
self.kp * (turning_rate - self.current_turning_rate),
-self.max_turn_acc, self.max_turn_acc)
self.current_speed += linear_acc * dt
self.current_speed = np.clip(self.current_speed, -self.max_speed,
self.max_speed)
self.current_turning_rate += turn_acc * dt
selected_heading = wrap(self.current_turning_rate * dt +
self.agent.heading_global_frame)
dx = self.current_speed * np.cos(selected_heading) * dt
dy = self.current_speed * np.sin(selected_heading) * dt
self.agent.pos_global_frame += np.array([dx, dy])
self.agent.vel_global_frame[0] = self.current_speed * np.cos(
selected_heading)
self.agent.vel_global_frame[1] = self.current_speed * np.sin(
selected_heading)
self.agent.speed_global_frame = self.current_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):
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)
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 step(self, action, dt):
selected_speed = action[0]
selected_heading = wrap(action[1] * 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 step(self, action, dt):
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):
"""
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 find_next_action(self, obs, agents, i):
host_agent = agents[i]
other_agents = agents[:i]+agents[i+1:]
agent_state = self.convert_host_agent_to_cadrl_state(host_agent)
other_agents_state, other_agents_actions = self.convert_other_agents_to_cadrl_state(host_agent, other_agents)
# value = self.value_net.find_states_values(agent_state, other_agents_state)
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 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 step(self, action, dt):
selected_speed = np.clip(
np.linalg.norm(self.agent.vel_global_frame) + action[0] * dt, 0,
self.max_vel)
turning_rate = self.agent.angular_speed_global_frame + action[1] * dt
self.agent.angular_speed_global_frame = np.clip(
turning_rate, -self.max_turn_rate, self.max_turn_rate)
selected_heading = wrap(self.agent.angular_speed_global_frame * 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)
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 update_ego_frame(self):
# 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])
self.agent.rel_goal = self.agent.goal_global_frame - self.agent.pos_global_frame
def near_goal_smoother(self, dist_to_goal, pref_speed, heading,
raw_action):
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
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])