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