diff --git a/pyrobot-gym/pyrobot_gym/core/__init__.py b/pyrobot-gym/pyrobot_gym/core/__init__.py index a77c66c..aaabacd 100644 --- a/pyrobot-gym/pyrobot_gym/core/__init__.py +++ b/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 diff --git a/pyrobot-gym/pyrobot_gym/robots/__init__.py b/pyrobot-gym/pyrobot_gym/robots/__init__.py index 1c92d85..f1fac9f 100644 --- a/pyrobot-gym/pyrobot_gym/robots/__init__.py +++ b/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 diff --git a/pyrobot-gym/pyrobot_gym/robots/locobot_mujoco_env.py b/pyrobot-gym/pyrobot_gym/robots/locobot_mujoco_env.py index 79063ea..43af85d 100644 --- a/pyrobot-gym/pyrobot_gym/robots/locobot_mujoco_env.py +++ b/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): diff --git a/pyrobot-gym/pyrobot_gym/tasks/__init__.py b/pyrobot-gym/pyrobot_gym/tasks/__init__.py index 3a51a07..f4493be 100644 --- a/pyrobot-gym/pyrobot_gym/tasks/__init__.py +++ b/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 diff --git a/pyrobot-gym/pyrobot_gym/tasks/task_commons.py b/pyrobot-gym/pyrobot_gym/tasks/task_commons.py index c96cde7..33810e5 100644 --- a/pyrobot-gym/pyrobot_gym/tasks/task_commons.py +++ b/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) diff --git a/src/testing_environment.py b/src/testing_environment.py index 32a0922..02ff5aa 100755 --- a/src/testing_environment.py +++ b/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() -"""