def __init__(self, gui, urdf_dir):
self.sim_steps = 0
self.frame_num = 0
self.obj_ids = []
self.urdf_paths = glob.glob(f"{urdf_dir}/*/*.urdf")
print(self.urdf_paths)
if gui:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
# load table and arm
p.loadURDF(pybullet_data.getDataPath()+"/table/table.urdf", 0.5,0.,-0.82, 0,0,0,1)
self.kuka = kuka.Kuka(urdfRootPath=pybullet_data.getDataPath())
# remove tray (part of kuka env)
p.removeBody(self.kuka.trayUid)
# activate apple physics
p.setGravity(0, 0, -9.8)
#p.enableJointForceTorqueSensor(self.kuka.kukaUid, 8)
#p.enableJointForceTorqueSensor(self.kuka.kukaUid, 11)
self.left_finger_index = 8
self.left_fingertip_index = 10
self.right_finger_index = 11
self.right_fingertip_index = 13
def build_kuka():
# Load Kuka robot.
robot = kuka.Kuka(urdfRootPath=pdata.getDataPath())
# Remove the tray object.
p.removeBody(robot.trayUid)
return robot
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)
self._envStepCounter = 0
p.stepSimulation()
self._observation, self._goal = self.getExtendedObservation(
), self.get_block_pos()
return np.array(self._observation), np.array(self._goal)
def reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
print(self._urdfRoot)
p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1])
self.tableUid = p.loadURDF(
os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000,
0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0)
self.blockUids = {}
for i, color in enumerate(self.cubeColors):
xpos = 0.6 + 0.05 * (i - 2) # to the left of the table
ypos = 0.1 + 0.1 * (i - 2) # closer to the camera
ang = 3.14 * 0.5 + 3.1415925438 * 0
orn = p.getQuaternionFromEuler([0, 0, ang])
self.blockUids[color] = p.loadURDF(
'./assets/cube_{}.urdf'.format(color), 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.removeBody(self._kuka.trayUid)
self._envStepCounter = 0
p.stepSimulation()
# self._observation = self.getExtendedObservation()
# return np.array(self._observation)
observation = np.array([1.])
return observation
def reset_inherited(self):
"""
this function consists of code from
kukaCamGymEnv
[pybullet_envs](https://github.com/bulletphysics/bullet3/)
used under the pybullet license available
-> https://github.com/bulletphysics/bullet3/blob/master/LICENSE.txt
"""
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.5 + 0.2 * random.random()
self.ypos = 0 + 0.25 * random.random()
ang = 3.1415925438 * random.random()
orn = p.getQuaternionFromEuler([0, 0, ang])
self.blockUid = p.loadURDF(os.path.join(self._urdfRoot, "block.urdf"), \
self.xpos, self.ypos, -0.1,
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
p.stepSimulation()
self._observation = self.getExtendedObservation()
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 = 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)
p.setGravity(0, 0, -9.81)
p.setRealTimeSimulation(0)
view_matrix = p.computeViewMatrix(
cameraEyePosition=[0, 0, 0.5],
cameraTargetPosition=[0, 0, 0],
cameraUpVector=[-1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(
fov=45.0,
aspect=1,
nearVal=0.1,
farVal=0.5)
p.loadURDF("plane.urdf")
# obj_quater_orient = p.getQuaternionFromEuler([math.pi / 2, 0, 0])
# obj = p.loadURDF("cube_small.urdf", [0, 0, 0], obj_quater_orient)
p.getCameraImage(width=256,
height=256,
viewMatrix=view_matrix,
projectionMatrix=projection_matrix)
robot = kuka.Kuka(urdfRootPath=pdata.getDataPath())
p.removeBody(robot.trayUid)
# robot.applyAction([0, 0, -0.22, math.pi/2, 0])
while 1:
p.stepSimulation()
