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)
