From 51b7e1040f0581dbb5a2e3f2eeb38ac55f112b18 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 26 Aug 2017 13:13:53 -0700 Subject: [PATCH] more fixes in the pybullet gym environments: use main instead of demo_run, add missing main to some eaxmples. pip install pybullet train: python -m pybullet_envs.examples.train_pybullet_cartpole enjoy: python -m pybullet_envs.examples.enjoy_pybullet_cartpole enjoy pretrained environments: python -m pybullet_envs.examples.enjoy_TF_AntBulletEnv_v0_2017may python -m pybullet_envs.examples.enjoy_TF_HalfCheetahBulletEnv_v0_2017may python -m pybullet_envs.examples.enjoy_TF_AntBulletEnv_v0_2017may python -m pybullet_envs.examples.enjoy_TF_HopperBulletEnv_v0_2017may python -m pybullet_envs.examples.enjoy_TF_HumanoidBulletEnv_v0_2017may python -m pybullet_envs.examples.enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may python -m pybullet_envs.examples.enjoy_TF_InvertedPendulumBulletEnv_v0_2017may python -m pybullet_envs.examples.enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may python -m pybullet_envs.examples.enjoy_TF_Walker2DBulletEnv_v0_2017may Run some gym environment test: python -m pybullet_envs.examples.racecarGymEnvTest Create/Import a specific Gym environment: python import pybullet_envs env = gym.make("AntBulletEnv-v0") env = gym.make("HalfCheetahBulletEnv-v0") env = gym.make("HopperBulletEnv-v0") env = gym.make("HumanoidBulletEnv-v0") env = gym.make("Walker2DBulletEnv-v0") env = gym.make("InvertedDoublePendulumBulletEnv-v0") env = gym.make("InvertedPendulumBulletEnv-v0") env = gym.make("MinitaurBulletEnv-v0") env = gym.make("RacecarBulletEnv-v0") env = gym.make("KukaBulletEnv-v0") env = gym.make("CartPoleBulletEnv-v0") --- .../gym/pybullet_envs/bullet/kukaGymEnv.py | 7 ++- .../gym/pybullet_envs/examples/__init__.py | 23 +++++++ .../enjoy_TF_AntBulletEnv_v0_2017may.py | 4 +- ...njoy_TF_HalfCheetahBulletEnv_v0_2017may.py | 4 +- .../enjoy_TF_HopperBulletEnv_v0_2017may.py | 4 +- .../enjoy_TF_HumanoidBulletEnv_v0_2017may.py | 4 +- ...ertedDoublePendulumBulletEnv_v0_2017may.py | 4 +- ...TF_InvertedPendulumBulletEnv_v0_2017may.py | 4 +- ...rtedPendulumSwingupBulletEnv_v0_2017may.py | 4 +- .../enjoy_TF_Walker2DBulletEnv_v0_2017may.py | 4 +- .../pybullet_envs/examples/kukaGymEnvTest.py | 57 +++++++++-------- .../examples/racecarGymEnvTest.py | 63 ++++++++++--------- .../examples/racecarZEDGymEnvTest.py | 57 +++++++++-------- .../examples/simpleHumanoidGymEnvTest.py | 37 ++++++----- 14 files changed, 159 insertions(+), 117 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index b0a3da4d9..b2e96eeeb 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py @@ -1,3 +1,4 @@ +import os import math import gym from gym import spaces @@ -52,15 +53,15 @@ class KukaGymEnv(gym.Env): p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) - p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1]) + p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"),[0,0,-1]) - p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) + p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) xpos = 0.5 +0.05*random.random() ypos = 0 +0.05*random.random() ang = 3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) - self.blockUid =p.loadURDF("block.urdf", xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) + self.blockUid =p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","block.urdf"), xpos,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) diff --git a/examples/pybullet/gym/pybullet_envs/examples/__init__.py b/examples/pybullet/gym/pybullet_envs/examples/__init__.py index e69de29bb..ce46ab989 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/examples/__init__.py @@ -0,0 +1,23 @@ +from . import enjoy_TF_AntBulletEnv_v0_2017may +from . import enjoy_TF_HalfCheetahBulletEnv_v0_2017may +from . import enjoy_TF_AntBulletEnv_v0_2017may +from . import enjoy_TF_HopperBulletEnv_v0_2017may +from . import enjoy_TF_HumanoidBulletEnv_v0_2017may +from . import enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may +from . import enjoy_TF_InvertedPendulumBulletEnv_v0_2017may +from . import enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may +from . import enjoy_TF_Walker2DBulletEnv_v0_2017may +from . import enjoy_kuka_grasping +from . import enjoy_pybullet_cartpole +from . import enjoy_pybullet_racecar +from . import enjoy_pybullet_zed_racecar +from . import kukaGymEnvTest +from . import racecarGymEnvTest +from . import racecarZEDGymEnvTest +from . import simpleHumanoidGymEnvTest +from . import train_kuka_cam_grasping +from . import train_kuka_grasping +from . import train_pybullet_cartpole +from . import train_pybullet_racecar +from . import train_pybullet_zed_racecar + diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py index 02691e0fc..85a362d4c 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py @@ -28,7 +28,7 @@ class SmallReactivePolicy: x = np.dot(x, weights_final_w) + weights_final_b return x -def demo_run(): +def main(): env = gym.make("AntBulletEnv-v0") cid = p.connect(p.GUI) @@ -311,4 +311,4 @@ weights_final_w = np.array([ weights_final_b = np.array([ -0.0680, +0.1401, -0.0628, -0.1317, +0.1489, +0.1844, -0.1147, +0.0137]) if __name__=="__main__": - demo_run() + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py index 9a9408060..a5f7eb455 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py @@ -27,7 +27,7 @@ class SmallReactivePolicy: x = np.dot(x, weights_final_w) + weights_final_b return x -def demo_run(): +def main(): env = gym.make("HalfCheetahBulletEnv-v0") cid = p.connect(p.GUI) @@ -311,4 +311,4 @@ weights_final_w = np.array([ weights_final_b = np.array([ +0.1367, +0.1107, -0.0148, +0.1158, -0.0820, +0.3047]) if __name__=="__main__": - demo_run() + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py index 0340fe5db..38120268c 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py @@ -30,7 +30,7 @@ class SmallReactivePolicy: x = np.dot(x, weights_final_w) + weights_final_b return x -def demo_run(): +def main(): env = gym.make("HopperBulletEnv-v0") cid = p.connect(p.GUI) @@ -301,4 +301,4 @@ weights_final_w = np.array([ weights_final_b = np.array([ +0.4868, -0.0987, -0.0946]) if __name__=="__main__": - demo_run() + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py index 1545813f9..b75c908ad 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py @@ -28,7 +28,7 @@ class SmallReactivePolicy: x = np.dot(x, weights_final_w) + weights_final_b return x -def demo_run(): +def main(): env = gym.make("HumanoidBulletEnv-v0") cid = p.connect(p.GUI) @@ -517,4 +517,4 @@ weights_final_w = np.array([ weights_final_b = np.array([ -0.0356, +0.0776, -0.0344, +0.1375, +0.1048, +0.3648, +0.3240, +0.1319, +0.1161, +0.3373, +0.3193, +0.0120, +0.0253, -0.2434, -0.1291, +0.1042, -0.2448]) if __name__=="__main__": - demo_run() + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py index ee6a0906b..122182b57 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py @@ -27,7 +27,7 @@ class SmallReactivePolicy: x = np.dot(x, weights_final_w) + weights_final_b return x -def demo_run(): +def main(): env = gym.make("InvertedDoublePendulumBulletEnv-v0") cid = p.connect(p.GUI) @@ -179,4 +179,4 @@ weights_final_w = np.array([ weights_final_b = np.array([ +0.0190]) if __name__=="__main__": - demo_run() + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py index 6550f1efb..93aab652b 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py @@ -27,7 +27,7 @@ class SmallReactivePolicy: x = np.dot(x, weights_final_w) + weights_final_b return x -def demo_run(): +def main(): print("create env") env = gym.make("InvertedPendulumBulletEnv-v0") print("connecting") @@ -174,4 +174,4 @@ weights_final_w = np.array([ weights_final_b = np.array([ +0.0352]) if __name__=="__main__": - demo_run() + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py index 165d8097d..34a2d4447 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py @@ -27,7 +27,7 @@ class SmallReactivePolicy: x = np.dot(x, weights_final_w) + weights_final_b return x -def demo_run(): +def main(): env = gym.make("InvertedPendulumSwingupBulletEnv-v0") cid = p.connect(p.GUI) @@ -175,4 +175,4 @@ weights_final_w = np.array([ weights_final_b = np.array([ +0.2753]) if __name__=="__main__": - demo_run() + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py index 62fa07fa0..2451b689a 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py @@ -27,7 +27,7 @@ class SmallReactivePolicy: x = np.dot(x, weights_final_w) + weights_final_b return x -def demo_run(): +def main(): env = gym.make("Walker2DBulletEnv-v0") cid = p.connect(p.GUI) @@ -305,4 +305,4 @@ weights_final_w = np.array([ weights_final_b = np.array([ +0.0090, +0.2404, +0.1022, +0.1035, +0.0621, -0.0160]) if __name__=="__main__": - demo_run() + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py index 40ae7e9ec..65120c5f8 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py @@ -7,31 +7,34 @@ os.sys.path.insert(0,parentdir) from pybullet_envs.bullet.kukaGymEnv import KukaGymEnv import time +def main(): -environment = KukaGymEnv(renders=True) - - -motorsIds=[] -#motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537)) -#motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0)) -#motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2)) -#motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0)) -#motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) - -dv = 0.001 -motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0)) -motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0)) -motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv)) -motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0)) -motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) - -done = False -while (not done): - - action=[] - for motorId in motorsIds: - action.append(environment._p.readUserDebugParameter(motorId)) - - state, reward, done, info = environment.step2(action) - obs = environment.getExtendedObservation() - + environment = KukaGymEnv(renders=True) + + + motorsIds=[] + #motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537)) + #motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0)) + #motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2)) + #motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0)) + #motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) + + dv = 0.001 + motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv)) + motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) + + done = False + while (not done): + + action=[] + for motorId in motorsIds: + action.append(environment._p.readUserDebugParameter(motorId)) + + state, reward, done, info = environment.step2(action) + obs = environment.getExtendedObservation() + +if __name__=="__main__": + main() \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py index fb439042d..aa78baa32 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py @@ -7,33 +7,38 @@ os.sys.path.insert(0,parentdir) from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv isDiscrete = False -environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete) -environment.reset() +def main(): + + environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete) + environment.reset() + + targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) + steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0) + + while (True): + targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider) + steeringAngle = environment._p.readUserDebugParameter(steeringSlider) + if (isDiscrete): + discreteAction = 0 + if (targetVelocity<-0.33): + discreteAction=0 + else: + if (targetVelocity>0.33): + discreteAction=6 + else: + discreteAction=3 + if (steeringAngle>-0.17): + if (steeringAngle>0.17): + discreteAction=discreteAction+2 + else: + discreteAction=discreteAction+1 + action=discreteAction + else: + action=[targetVelocity,steeringAngle] + state, reward, done, info = environment.step(action) + obs = environment.getExtendedObservation() + print("obs") + print(obs) -targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) -steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0) - -while (True): - targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider) - steeringAngle = environment._p.readUserDebugParameter(steeringSlider) - if (isDiscrete): - discreteAction = 0 - if (targetVelocity<-0.33): - discreteAction=0 - else: - if (targetVelocity>0.33): - discreteAction=6 - else: - discreteAction=3 - if (steeringAngle>-0.17): - if (steeringAngle>0.17): - discreteAction=discreteAction+2 - else: - discreteAction=discreteAction+1 - action=discreteAction - else: - action=[targetVelocity,steeringAngle] - state, reward, done, info = environment.step(action) - obs = environment.getExtendedObservation() - print("obs") - print(obs) +if __name__=="__main__": + main() \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py index 7e64c7bb3..176846651 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py @@ -5,31 +5,36 @@ parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv -print ("hello") -environment = RacecarZEDGymEnv(renders=True) -targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) -steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0) +def main(): + + environment = RacecarZEDGymEnv(renders=True) + + targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) + steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0) + + while (True): + targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider) + steeringAngle = environment._p.readUserDebugParameter(steeringSlider) + discreteAction = 0 + if (targetVelocity<-0.33): + discreteAction=0 + else: + if (targetVelocity>0.33): + discreteAction=6 + else: + discreteAction=3 + if (steeringAngle>-0.17): + if (steeringAngle>0.17): + discreteAction=discreteAction+2 + else: + discreteAction=discreteAction+1 + action=discreteAction + + state, reward, done, info = environment.step(action) + obs = environment.getExtendedObservation() + print("obs") + print(obs) -while (True): - targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider) - steeringAngle = environment._p.readUserDebugParameter(steeringSlider) - discreteAction = 0 - if (targetVelocity<-0.33): - discreteAction=0 - else: - if (targetVelocity>0.33): - discreteAction=6 - else: - discreteAction=3 - if (steeringAngle>-0.17): - if (steeringAngle>0.17): - discreteAction=discreteAction+2 - else: - discreteAction=discreteAction+1 - action=discreteAction - - state, reward, done, info = environment.step(action) - obs = environment.getExtendedObservation() - print("obs") - print(obs) +if __name__=="__main__": + main() \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py index a533f05f4..abbe68339 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/simpleHumanoidGymEnvTest.py @@ -5,22 +5,27 @@ parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) from pybullet_envs.bullet.simpleHumanoidGymEnv import SimpleHumanoidGymEnv -print ("hello") -environment = SimpleHumanoidGymEnv(renders=True) -environment._p.setGravity(0,0,0) +def main(): + + environment = SimpleHumanoidGymEnv(renders=True) -motorsIds=[] -for motor in environment._humanoid.motor_names: - motorsIds.append(environment._p.addUserDebugParameter(motor,-1,1,0)) + environment._p.setGravity(0,0,0) + + motorsIds=[] + for motor in environment._humanoid.motor_names: + motorsIds.append(environment._p.addUserDebugParameter(motor,-1,1,0)) + + while (True): + + action=[] + for motorId in motorsIds: + action.append(environment._p.readUserDebugParameter(motorId)) + + state, reward, done, info = environment.step(action) + obs = environment.getExtendedObservation() + print("obs") + print(obs) -while (True): - - action=[] - for motorId in motorsIds: - action.append(environment._p.readUserDebugParameter(motorId)) - - state, reward, done, info = environment.step(action) - obs = environment.getExtendedObservation() - print("obs") - print(obs) +if __name__=="__main__": + main() \ No newline at end of file