Spaces:
Sleeping
Sleeping
File size: 3,459 Bytes
a54010a | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 | import irsim
import numpy as np
import random
import shapely
from irsim.lib.handler.geometry_handler import GeometryFactory
from irsim.world import ObjectBase
class SIM_ENV:
def __init__(self, world_file="robot_world.yaml", save_ani=False):
self.env = irsim.make(world_file, save_ani=save_ani)
robot_info = self.env.get_robot_info(0)
self.robot_goal = robot_info.goal
def step(self, lin_velocity=0.0, ang_velocity=0.1):
self.env.step(action_id=0, action=np.array([[lin_velocity], [ang_velocity]]))
self.env.render()
scan = self.env.get_lidar_scan()
latest_scan = scan["ranges"]
robot_state = self.env.get_robot_state()
goal_vector = [
self.robot_goal[0].item() - robot_state[0].item(),
self.robot_goal[1].item() - robot_state[1].item(),
]
distance = np.linalg.norm(goal_vector)
goal = self.env.robot.arrive
pose_vector = [np.cos(robot_state[2]).item(), np.sin(robot_state[2]).item()]
cos, sin = self.cossin(pose_vector, goal_vector)
collision = self.env.robot.collision
action = [lin_velocity, ang_velocity]
reward = self.get_reward(goal, collision, action, latest_scan)
return latest_scan, distance, cos, sin, collision, goal, action, reward
def reset(self, robot_state=None, robot_goal=None, random_obstacles=True):
if robot_state is None:
robot_state = [[random.uniform(1, 9)], [random.uniform(1, 9)], [0], [0]]
self.env.robot.set_state(
state=np.array(robot_state),
init=True,
)
if random_obstacles:
self.env.random_obstacle_position(
range_low=[0, 0, -3.14],
range_high=[10, 10, 3.14],
ids=[i + 1 for i in range(7)],
non_overlapping=True,
)
if robot_goal is None:
unique = True
while unique:
robot_goal = [[random.uniform(1, 9)], [random.uniform(1, 9)], [0]]
shape = {"name": "circle", "radius": 0.4}
state = [robot_goal[0], robot_goal[1], robot_goal[2]]
gf = GeometryFactory.create_geometry(**shape)
geometry = gf.step(np.c_[state])
unique = any(
[
shapely.intersects(geometry, obj._geometry)
for obj in self.env.obstacle_list
]
)
self.env.robot.set_goal(np.array(robot_goal), init=True)
self.env.reset()
self.robot_goal = self.env.robot.goal
action = [0.0, 0.0]
latest_scan, distance, cos, sin, _, _, action, reward = self.step(
lin_velocity=action[0], ang_velocity=action[1]
)
return latest_scan, distance, cos, sin, False, False, action, reward
@staticmethod
def cossin(vec1, vec2):
vec1 = vec1 / np.linalg.norm(vec1)
vec2 = vec2 / np.linalg.norm(vec2)
cos = np.dot(vec1, vec2)
sin = vec1[0] * vec2[1] - vec1[1] * vec2[0]
return cos, sin
@staticmethod
def get_reward(goal, collision, action, laser_scan):
if goal:
return 100.0
elif collision:
return -100.0
else:
r3 = lambda x: 1.35 - x if x < 1.35 else 0.0
return action[0] - abs(action[1]) / 2 - r3(min(laser_scan)) / 2
|