def get_waypoint_dist_dir(self): angle = get_direction(self.pose6D.squeeze(), self.waypointPose6D.squeeze(), 0, 0) dist = get_distance(self.pose6D.squeeze(), self.waypointPose6D.squeeze()) return torch.cat( [ dist.view(1, 1), torch.sin(angle).view(1, 1), torch.cos(angle).view(1, 1), ], dim=1, )
def planner_prediction_to_command(self, p_next): command = SimulatorActions.STOP.value p_init = self.pose6D.squeeze() d_angle_rot_th = self.angle_th pos_th = self.pos_th if get_distance(p_init, p_next) <= pos_th: return command d_angle = angle_to_pi_2_minus_pi_2( get_direction(p_init, p_next, ang_th=d_angle_rot_th, pos_th=pos_th)) if abs(d_angle) < d_angle_rot_th: command = SimulatorActions.FORWARD.value else: if (d_angle > 0) and (d_angle < pi): command = SimulatorActions.LEFT.value elif d_angle > pi: command = SimulatorActions.RIGHT.value elif (d_angle < 0) and (d_angle > -pi): command = SimulatorActions.RIGHT.value else: command = SimulatorActions.LEFT.value return command
def planner_prediction_to_command(self, p_next): command = HabitatSimActions.STOP p_init = self.pose6D.squeeze() d_angle_rot_th = self.angle_th pos_th = self.pos_th if get_distance(p_init, p_next) <= pos_th: return command d_angle = angle_to_pi_2_minus_pi_2( get_direction(p_init, p_next, ang_th=d_angle_rot_th, pos_th=pos_th) ) if abs(d_angle) < d_angle_rot_th: command = HabitatSimActions.MOVE_FORWARD else: if (d_angle > 0) and (d_angle < pi): command = HabitatSimActions.TURN_LEFT elif d_angle > pi: command = HabitatSimActions.TURN_RIGHT elif (d_angle < 0) and (d_angle > -pi): command = HabitatSimActions.TURN_RIGHT else: command = HabitatSimActions.TURN_LEFT return command