Verify the effectiveness of joint angle action space

pyrobot-gym/pyrobot_gym/core/__init__.py

@@ -1,9 +1,19 @@
-from pyrobot_gym.core.robot_mujoco_env import RobotMujocoEnv
-from pyrobot_gym.core.robot_gazebo_env import RobotGazeboEnv
-from pyrobot_gym.core.robot_env import RobotEnv
+from pyrobot_gym.core import utils, rotations
 
-from pyrobot_gym.core.controllers_connection import ControllersConnection
-from pyrobot_gym.core.gazebo_connection import GazeboConnection
-from pyrobot_gym.core.openai_ros_common import StartOpenAI_ROS_Environment, ROSLauncher
+from pyrobot_gym.core.robot_mujoco_env import RobotMujocoEnv
 
-from pyrobot_gym.core import utils, rotations
+try:
+    from pyrobot_gym.core.robot_gazebo_env import RobotGazeboEnv
+    from pyrobot_gym.core.robot_env import RobotEnv
+    from pyrobot_gym.core.controllers_connection import ControllersConnection
+    from pyrobot_gym.core.gazebo_connection import GazeboConnection
+    from pyrobot_gym.core.openai_ros_common import ROSLauncher, \
+        StartOpenAI_ROS_Environment
+except ModuleNotFoundError:
+    print("Gazebo module not found!")
+    RobotGazeboEnv = None
+    RobotEnv = None
+    ControllersConnection = None
+    GazeboConnection = None
+    ROSLauncher = None
+    StartOpenAI_ROS_Environment = None

pyrobot-gym/pyrobot_gym/robots/__init__.py

@@ -1,3 +1,8 @@
 from pyrobot_gym.robots.locobot_mujoco_env import LocoBotMujocoEnv
-from pyrobot_gym.robots.locobot_gazebo_env import LocoBotGazeboEnv
-from pyrobot_gym.robots.locobot_env import LocoBotEnv
+
+try:
+    from pyrobot_gym.robots.locobot_gazebo_env import LocoBotGazeboEnv
+    from pyrobot_gym.robots.locobot_env import LocoBotEnv
+except ModuleNotFoundError:
+    LocoBotGazeboEnv = None
+    LocoBotEnv = None

pyrobot-gym/pyrobot_gym/robots/locobot_mujoco_env.py

@@ -11,13 +11,6 @@
 #from gym.envs.robotics import rotations, robot_env, utils
 from pyrobot_gym.core import robot_mujoco_env, rotations, utils
 
-import rospy
-from rospy.numpy_msg import numpy_msg
-from rospy_tutorials.msg import Floats
-from std_msgs.msg import String
-from std_msgs.msg import Bool
-
-
 
 
 class LocoBotMujocoEnv(robot_mujoco_env.RobotMujocoEnv):

pyrobot-gym/pyrobot_gym/tasks/__init__.py

@@ -1,11 +1,22 @@
 # Task: Reach
-from pyrobot_gym.tasks.mujoco_reach import LocoBotMujocoReachEnv
-from pyrobot_gym.tasks.gazebo_reach import LocoBotGazeboReachEnv
-from pyrobot_gym.tasks.reach import LocoBotReachEnv
-
 # Task: Push
-from pyrobot_gym.tasks.mujoco_push import LocoBotMujocoPushEnv
-from pyrobot_gym.tasks.gazebo_push import LocoBotGazeboPushEnv
+# Task: Push
+from pyrobot_gym.tasks.mujoco_push import LocoBotMujocoPushEnv, \
+    LocoBotMujocoPushEnv
+from pyrobot_gym.tasks.mujoco_reach import LocoBotMujocoReachEnv, \
+    LocoBotMujocoReachEnv
+from pyrobot_gym.tasks.task_commons import LoadYamlFileParamsTest, \
+    LoadYamlFileParamsTest
+from pyrobot_gym.tasks.task_envs_list import RegisterOpenAI_Ros_Env, \
+    GetAllRegisteredGymEnvs, RegisterOpenAI_Ros_Env, GetAllRegisteredGymEnvs
 
-from pyrobot_gym.tasks.task_commons import LoadYamlFileParamsTest
-from pyrobot_gym.tasks.task_envs_list import RegisterOpenAI_Ros_Env, GetAllRegisteredGymEnvs
+try:
+    from pyrobot_gym.tasks.gazebo_reach import LocoBotGazeboReachEnv
+    from pyrobot_gym.tasks.gazebo_push import LocoBotGazeboPushEnv
+    from pyrobot_gym.tasks.reach import LocoBotReachEnv, LocoBotReachEnv
+    from pyrobot_gym.tasks.gazebo_push import LocoBotGazeboPushEnv
+except:
+    LocoBotGazeboReachEnv = None
+    LocoBotGazeboPushEnv = None
+    LocoBotReachEnv = None
+    LocoBotGazeboPushEnv = None

pyrobot-gym/pyrobot_gym/tasks/task_commons.py

@@ -1,17 +1,21 @@
 #!/usr/bin/env python
-import rosparam
-import rospkg
 import os
 
-def LoadYamlFileParamsTest(rospackage_name, rel_path_from_package_to_file, yaml_file_name):
+import rospkg
 
+try:
+    import rosparam
+except ModuleNotFoundError:
+    rosparam = None
+
+
+def LoadYamlFileParamsTest(rospackage_name, rel_path_from_package_to_file, yaml_file_name):
     rospack = rospkg.RosPack()
     pkg_path = rospack.get_path(rospackage_name)
     config_dir = os.path.join(pkg_path, rel_path_from_package_to_file)
     path_config_file = os.path.join(config_dir, yaml_file_name)
 
-    paramlist=rosparam.load_file(path_config_file)
+    paramlist = rosparam.load_file(path_config_file)
 
     for params, ns in paramlist:
-        rosparam.upload_params(ns,params)
-
+        rosparam.upload_params(ns, params)

src/testing_environment.py

@@ -1,32 +1,34 @@
 #!/usr/bin/env python
+import time
+
 import gym
-#import gym_pyrobot
-import pyrobot_gym
 import numpy as np
 
-""" Sample PyRobot Manipulation in Mujoco Environment """
-actions = []
-observation = []
-in0fos = []
 
 def main():
+    import pyrobot_gym
+    pyrobot_gym
     env = gym.make('LocoBotPush-v1')
-    #env = gym.make('FetchReach-v1')
-    numItr = 100
-    initStateSpace = "random"
+    # env = gym.make('FetchReach-v1')
+    numItr = 50
     env.reset()
+
     print("Reset!")
-    while len(actions) < numItr:
-        obs = env.reset()
-        #print("ITERATION NUMBER ", len(actions))
-        env.render()
-        #print('env.action_space = {}'.format(env.action_space))i
-        #action = env.action_space.sample()
-        #action = np.array([0.02, -0.9, 0.023, 0., 0.])
-        #print('===== Action = {}'.format(action))
-        #obs = env.step(action)
+    for aid in range(4):
+        for pos_neg in range(-1, 2, 2):  # Positive and nagative
+            for i in range(numItr):
+                obs, r, d, i = env.step([
+                    float(aid == 0) * 0.1 * pos_neg,
+                    float(aid == 1) * 0.1 * pos_neg,
+                    float(aid == 2) * 0.1 * pos_neg,
+                    float(aid == 3) * 0.1 * pos_neg,
+                ])
+                print(f"Obs {obs}, Reward {r}, Done {d}, Info {i}")
+                env.render()
+                time.sleep(0.1)
+                if d:
+                    env.reset()
 
-#        reachToGoal(env, obs)
 
 def reachToGoal(env, lastObs):
     goal = lastObs['desired_goal']
@@ -39,16 +41,16 @@ def reachToGoal(env, lastObs):
     timeStep = 0
     episodeObs.append(lastObs)
     distance = np.linalg.norm(goal, axis=-1)
-    while distance >= 0.05: # and timeStep <= env._max_episode_steps:
+    while distance >= 0.05:  # and timeStep <= env._max_episode_steps:
         print("==================================")
         env.render()
         action = [0, 0, 0, 0]
         for i in range(len(goal)):
-            action[i] = goal[i]*6
+            action[i] = goal[i] * 6
             print('action[i] = {}'.format(action[i]))
             print('goal[i] = {}'.format(goal[i]))
 
-        action[len(action)-1] = 0 #remain close
+        action[len(action) - 1] = 0  # remain close
 
         obsDataNew, reward, done, info = env.step(action)
         timeStep += 1
@@ -70,7 +72,7 @@ def reachToGoal(env, lastObs):
     while True:
         env.render()
         action = [0, 0, 0, 0]
-        action[len(action)-1] = 0 # keep the gripper closed
+        action[len(action) - 1] = 0  # keep the gripper closed
 
         obsDataNew, reward, done, info = env.step(action)
         timeStep += 1
@@ -79,31 +81,9 @@ def reachToGoal(env, lastObs):
         episodeInfo.append(info)
         episodeObs.append(obsDataNew)
 
-        if timeStep >= 10000: break
+        if timeStep >= 10000:
+            break
 
-    actions.append(episodeAcs)
-    observations.append(episodeObs)
-    infos.append(episodeInfo)
 
 if __name__ == "__main__":
     main()
-
-
-
-
-
-
-
-
-
-"""
-env = gym.make('pyrobot-reach-v0')
-#env = gym.make('FetchReach-v1')
-env.reset()
-
-for _ in range(1000):
-    env.render()
-
-    env.step(env.action_space.sample())
-env.close()
-"""