def reset(self): #print("KukaGymEnv _reset") self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0) self.xpos = 0.55 - 0.07 * random.random() + 0.07 self.ypos = -0.1 * random.random() + 0.1 # ang = 3.14*0.5+3.1415925438*random.random() # self.xpos= 0.55 # self.ypos=0.08 # self.ang=self._block_angle+3.14*random.random() self.ang = self._block_angle + 3.14 * random.random() orn = p.getQuaternionFromEuler([0, 0, self.ang]) self.blockUid = p.loadURDF(os.path.join(self._urdfRoot, "block.urdf"), self.xpos, self.ypos, -0.15, orn[0], orn[1], orn[2], orn[3]) p.setGravity(0, 0, -10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 for down in range(250): getdownAction = [0, 0, -0.001] self._kuka.applyAction(getdownAction, terminate=False) p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
def reset(self): #print("KukaGymEnv _reset") self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0) xpos = 0.55 + 0.12 * random.random() ypos = 0 + 0.2 * random.random() ang = 3.14 * 0.5 + 3.1415925438 * random.random() orn = p.getQuaternionFromEuler([0, 0, ang]) self.blockUid = p.loadURDF(os.path.join(self._urdfRoot, "block.urdf"), xpos, ypos, -0.15, orn[0], orn[1], orn[2], orn[3]) p.setGravity(0, 0, -10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) # p.getNumJoints(self._kuka.kukaUid) # 14 # for i in range(14): # print('######## p.getLinsStates,linkIndex:',i, p.getLinkState(self._kuka.kukaUid,i)) self._envStepCounter = 0 p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
def __init__(self, gui=False, max_steps=50): self.max_steps = max_steps p.connect(p.GUI if gui else p.DIRECT) def load_urdf(urdf, position, orientation=(0, 0, 0, 1)): return p.loadURDF(urdf, *(position + orientation)) self.table = load_urdf("data/table/table.urdf", position=(0.5, 0, -0.82)) self.target = load_urdf("target.urdf", position=(0.5, 0, 0)) self.kuka = kuka.Kuka() self.action_space = spaces.Box(-1, 1, shape=(7, ))
def _reset(self): """Environment reset called at the beginning of an episode. """ # Set the camera settings. look = [0.23, 0.2, 0.54] distance = 1. pitch = -56 + self._cameraRandom * np.random.uniform(-3, 3) yaw = 245 + self._cameraRandom * np.random.uniform(-3, 3) roll = 0 self._view_matrix = p.computeViewMatrixFromYawPitchRoll( look, distance, yaw, pitch, roll, 2) fov = 20. + self._cameraRandom * np.random.uniform(-2, 2) aspect = self._width / self._height near = 0.01 far = 10 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, near, far) self._attempted_grasp = False self._env_step = 0 self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0) p.setGravity(0, 0, -10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() # Choose the objects in the bin. urdfList = self._get_random_object(self._numObjects, self._isTest) self._objectUids = self._randomly_place_objects(urdfList) self._observation = self._get_observation() return np.array(self._observation)
def _reset(self): """Environment reset called at the beginning of an episode. """ # Set the camera settings. look = [0.23, 0.2, 0.54] distance = 2. pitch = -36 + self._cameraRandom * np.random.uniform(-3, 3) yaw = 245 + self._cameraRandom * np.random.uniform(-3, 3) roll = 0 self._view_matrix = p.computeViewMatrixFromYawPitchRoll( look, distance, yaw, pitch, roll, 2) fov = 20. + self._cameraRandom * np.random.uniform(-2, 2) aspect = self._width / self._height near = 0.01 far = 20 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, near, far) # x = np.random.normal(-1.05, 0.04, 1) # z = np.random.normal(0.68, 0.04, 1) # lookat_x = np.random.normal(0.1, 0.02, 1) # pos = [x, 0, z] # lookat = [lookat_x, 0, 0] # print(pos) # vec = [-0.5, 0, 1] # self._view_matrix = p.computeViewMatrix(pos, lookat, vec) # fov = np.random.normal(45, 2, 1) # self._proj_matrix = p.computeProjectionMatrixFOV( # fov=fov, aspect=4. / 3., nearVal=0.01, farVal=2.5) self._attempted_grasp = False self._env_step = 0 self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) plane = p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) table = p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0) # self.planeId = plane[0] # self.tableId = table[0] p.setGravity(0, 0, -10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self.goal = [0.6, 0.4, -0.19] self._action_angle = 0 direction = np.array([ np.random.choice([ np.random.random_integers(-20, -5), np.random.random_integers(5, 20), ]), np.random.choice([ np.random.random_integers(-20, -5), np.random.random_integers(5, 20), ]), np.random.random_integers(70, 100), ]) self.light = { "diffuse": np.random.uniform(0.4, 0.6), "ambient": np.random.uniform(0.4, 0.6), "spec": np.random.uniform(0.4, 0.7), "dir": direction, "col": np.random.uniform([0.9, 0.9, 0.9], [1, 1, 1]), } wood_color = np.random.normal([170, 150, 140], 8) wall_color = np.random.normal([230, 240, 250], 8) tex1 = p.loadTexture( noise.createAndSave( tmp_dir + "/wall-{}.png".format(self.envId), "cloud", wall_color, )) tex2 = p.loadTexture( noise.createAndSave( tmp_dir + "/table-{}.png".format(self.envId), "cloud", wood_color, )) p.changeVisualShape(plane, -1, textureUniqueId=tex2) p.changeVisualShape(table, -1, textureUniqueId=tex1) self._envStepCounter = 0 p.stepSimulation() # Choose the objects in the bin. urdfList = self._get_random_object(self._numObjects, self._isTest) self._objectUids, self.obj_pos = self._randomly_place_objects(urdfList) print('_objectUids:', self._objectUids) self._observation = self._get_observation() return np.array(self._observation)