Fix bug of parallelized environments

[TaoHuang13]

Hi, @jiaqixuac. This PR aims to fix the bug of running parallel (vectorized) PSM environments. I will explain the cause of the found issue and how to fix that.

When running parallel environments, Pybullet will assign unique object ids to each env. For example, if there is an eval_env (called at first) and a train_env (called at second), which is the case in Stable_baselines3, Pybullet will assign ids of 0-5 to the eval_env and ids of 6-11 to the train_env. Then train_env will store 0-5 into self.obj_ids. Of course, there is no problem when initializing them. However, when you reset those envs through and sample a new goal for the train_env, the function reset(self) will call p.resetSimulation() to reset the simulation. Afterward, Pybullet will assign ids of 0-5 to the train_env. The self.obj_ids of train_env now becomes 0-11. Note that the valid ids are 0-5 now. But in _env_setup(self), the id self.obj_id of the object (like needle) will be set as self.obj_ids['rigid'][0], which belongs to 6-11 (since 0-5 is stored at first, the value at index=0 of the id list is in 0-5). This leads an error pybullet.error: getLinkState failed, since self.obj_id is not contained in the valid ids 0-5.

One possible solution is to reset self.obj_ids along with resetting the simulation. By doing this, we can guarantee that self.obj_id is always in the currently valid objective ids.

[jiaqixuac]

Pay attention to surrol_env.py and ecm_env.py.