PyBullet: improve examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py
(thanks to Deirdre Quillen for the environment) Extend repeat, so gripper reaches the tray bottom. Fix near plane so Z-Buffer is visible. Add sleep in return motion, in gui mode.
This commit is contained in:
@@ -21,7 +21,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
|
|||||||
|
|
||||||
def __init__(self,
|
def __init__(self,
|
||||||
urdfRoot=pybullet_data.getDataPath(),
|
urdfRoot=pybullet_data.getDataPath(),
|
||||||
actionRepeat=50,
|
actionRepeat=80,
|
||||||
isEnableSelfCollision=True,
|
isEnableSelfCollision=True,
|
||||||
renders=False,
|
renders=False,
|
||||||
isDiscrete=False,
|
isDiscrete=False,
|
||||||
@@ -117,7 +117,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
|
|||||||
look, distance, yaw, pitch, roll, 2)
|
look, distance, yaw, pitch, roll, 2)
|
||||||
fov = 20. + self._cameraRandom*np.random.uniform(-2, 2)
|
fov = 20. + self._cameraRandom*np.random.uniform(-2, 2)
|
||||||
aspect = self._width / self._height
|
aspect = self._width / self._height
|
||||||
near = 0.1
|
near = 0.01
|
||||||
far = 10
|
far = 10
|
||||||
self._proj_matrix = p.computeProjectionMatrixFOV(
|
self._proj_matrix = p.computeProjectionMatrixFOV(
|
||||||
fov, aspect, near, far)
|
fov, aspect, near, far)
|
||||||
@@ -254,6 +254,8 @@ class KukaDiverseObjectEnv(KukaGymEnv):
|
|||||||
grasp_action = [0, 0, 0, 0, finger_angle]
|
grasp_action = [0, 0, 0, 0, finger_angle]
|
||||||
self._kuka.applyAction(grasp_action)
|
self._kuka.applyAction(grasp_action)
|
||||||
p.stepSimulation()
|
p.stepSimulation()
|
||||||
|
#if self._renders:
|
||||||
|
# time.sleep(self._timeStep)
|
||||||
finger_angle -= 0.3/100.
|
finger_angle -= 0.3/100.
|
||||||
if finger_angle < 0:
|
if finger_angle < 0:
|
||||||
finger_angle = 0
|
finger_angle = 0
|
||||||
@@ -261,6 +263,8 @@ class KukaDiverseObjectEnv(KukaGymEnv):
|
|||||||
grasp_action = [0, 0, 0.001, 0, finger_angle]
|
grasp_action = [0, 0, 0.001, 0, finger_angle]
|
||||||
self._kuka.applyAction(grasp_action)
|
self._kuka.applyAction(grasp_action)
|
||||||
p.stepSimulation()
|
p.stepSimulation()
|
||||||
|
if self._renders:
|
||||||
|
time.sleep(self._timeStep)
|
||||||
finger_angle -= 0.3/100.
|
finger_angle -= 0.3/100.
|
||||||
if finger_angle < 0:
|
if finger_angle < 0:
|
||||||
finger_angle = 0
|
finger_angle = 0
|
||||||
|
|||||||
Reference in New Issue
Block a user