def __init__(self, config, gpu_count=0):
#assert(self.config["envname"] == self.__class__.__name__ or self.config["envname"] == "TestEnv")
self.config = self.parse_config(config)
SemanticRobotEnv.__init__(self, self.config, gpu_count,
scene_type="building",
tracking_camera=tracking_camera)
self.robot_introduce(Husky(self.config, env=self))
self.scene_introduce()
self.total_reward = 0
self.total_frame = 0
self.flag_timeout = 1
self.visualid = -1
self.lastid = None
self.gui = self.config["mode"] == "gui"
if self.gui:
self.visualid = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.2, 0.2, 0.2], rgbaColor=[1, 0, 0, 0.7])
self.colisionid = p.createCollisionShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.2, 0.2, 0.2])
self.lastid = None
self.obstacle_dist = 100
self.semantic_flagIds = []
debug_semantic = 1
if debug_semantic:
for i in range(self.semantic_pos.shape[0]):
pos = self.semantic_pos[i]
pos[2] += 0.2 # make flag slight above object
visualId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.1, 0.1, 0.1], rgbaColor=[1, 0, 0, 0.7])
flagId = p.createMultiBody(baseVisualShapeIndex=visualId, baseCollisionShapeIndex=-1, basePosition=pos)
self.semantic_flagIds.append(flagId)
def step(self, action):
obs, rew, env_done, info = SemanticRobotEnv.step(self,action=action)
self.close_semantic_ids = self.get_close_semantic_pos(dist_max=1.0, orn_max=np.pi/5)
for i in self.close_semantic_ids:
flagId = self.semantic_flagIds[i]
p.changeVisualShape(flagId, -1, rgbaColor=[0, 1, 0, 1])
return obs,rew,env_done,info