def __init__(self):
# Launch the Task Simulated-Environment
# This is the path where the simulation files are,
# the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/fetch/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="fetch_simple_description",
launch_file_name="start_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file relative to this TaskEnvironment
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file="src/openai_ros/task_envs/fetch/config",
yaml_file_name="fetchsimple_test.yaml")
super(FetchSimpleTestEnv, self).__init__(ros_ws_abspath)
rospy.logdebug("Entered FetchSimpleTestEnv Env")
self.get_params()
self.action_space = spaces.Discrete(self.n_actions)
observations_high_range = np.array(
self.upper_array_observations)
observations_low_range = np.array(
self.lower_array_observations)
self.observation_space = spaces.Box(observations_low_range, observations_high_range)
def __init__(self, control_type, ros_ws_abspath):
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="cartpole_description",
launch_file_name="put_robot_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
self.publishers_array = []
self._base_pub = rospy.Publisher(
'/cartpole_v0/foot_joint_velocity_controller/command',
Float64,
queue_size=1)
self._pole_pub = rospy.Publisher(
'/cartpole_v0/pole_joint_velocity_controller/command',
Float64,
queue_size=1)
self.publishers_array.append(self._base_pub)
self.publishers_array.append(self._pole_pub)
rospy.Subscriber("/cartpole_v0/joint_states", JointState,
self.joints_callback)
self.control_type = control_type
if self.control_type == "velocity":
self.controllers_list = [
'joint_state_controller',
'pole_joint_velocity_controller',
'foot_joint_velocity_controller',
]
elif self.control_type == "position":
self.controllers_list = [
'joint_state_controller',
'pole_joint_position_controller',
'foot_joint_position_controller',
]
elif self.control_type == "effort":
self.controllers_list = [
'joint_state_controller',
'pole_joint_effort_controller',
'foot_joint_effort_controller',
]
self.robot_name_space = "cartpole_v0"
self.reset_controls = True
# Seed the environment
self._seed()
self.steps_beyond_done = None
super(CartPoleEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=self.reset_controls)
def __init__(self, ros_ws_abspath):
"""Initializes a new CubeSingleDisk environment.
Args:
"""
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="moving_cube_description",
launch_file_name="put_cube_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Variables that we give through the constructor.
# None in this case
# Internal Vars
self.controllers_list = [
'joint_state_controller',
'inertia_wheel_roll_joint_velocity_controller'
]
self.robot_name_space = "moving_cube"
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(CubeSingleDiskEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=True)
"""
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
"""
self.gazebo.unpauseSim()
self.controllers_object.reset_controllers()
self._check_all_sensors_ready()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/moving_cube/joint_states", JointState,
self._joints_callback)
rospy.Subscriber("/moving_cube/odom", Odometry, self._odom_callback)
self._roll_vel_pub = rospy.Publisher(
'/moving_cube/inertia_wheel_roll_joint_velocity_controller/command',
Float64,
queue_size=1)
self._check_publishers_connection()
self.gazebo.pauseSim()
def __init__(self, ros_ws_abspath):
rospy.logdebug("Entered Fetch Env")
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="fetch_simple_description",
launch_file_name="put_fetchsimple_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
self.controllers_list = [
"joint_state_controller", "torso_lift_joint_position_controller",
"bellows_joint_position_controller",
"head_pan_joint_position_controller",
"head_tilt_joint_position_controller",
"shoulder_pan_joint_position_controller",
"shoulder_lift_joint_position_controller",
"upperarm_roll_joint_position_controller",
"elbow_flex_joint_position_controller",
"forearm_roll_joint_position_controller",
"wrist_flex_joint_position_controller",
"wrist_roll_joint_position_controller",
"r_gripper_finger_joint_position_controller",
"l_gripper_finger_joint_position_controller"
]
self.robot_name_space = "fetch"
self.reset_controls = True
super(FetchSimpleEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=self.reset_controls,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
# We Start all the ROS related Subscribers and publishers
self.JOINT_STATES_SUBSCRIBER = '/fetch/joint_states'
self.join_names = [
"joint0", "joint1", "joint2", "joint3", "joint4", "joint5",
"joint6"
]
self.gazebo.unpauseSim()
# Start Move Fetch Object, that checks all systems are ready
self.move_fetch_object = FetchSimpleMove()
# Wait until Fetch goes to the init pose
self.move_fetch_object.init_position()
# We pause until the next step
self.gazebo.pauseSim()
def __init__(self, ros_ws_abspath):
self.bridge = CvBridge()
rospy.logdebug("Start TurtleBot2SumoEnv INIT...")
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="turtlebot_gazebo",
launch_file_name="put_turtlebot2_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(TurtleBot2SumoEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
self._check_all_sensors_ready()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/odom", Odometry, self._odom_callback)
rospy.Subscriber("/gazebo/model_states", ModelStates,
self._model_state_callback)
#rospy.Subscriber("/camera/depth/image_raw", Image, self._camera_depth_image_raw_callback)
#rospy.Subscriber("/camera/depth/points", PointCloud2, self._camera_depth_points_callback)
rospy.Subscriber("/camera/rgb/image_raw", Image,
self._camera_rgb_image_raw_callback)
rospy.Subscriber("/kobuki/laser/scan", LaserScan,
self._laser_scan_callback)
self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self._check_publishers_connection()
self.gazebo.pauseSim()
rospy.logdebug("Finished TurtleBot2Env INIT...")
def __init__(self, ros_ws_abspath):
rospy.logdebug("========= In Fetch Env")
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="fetch_gazebo",
launch_file_name="put_fetch_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
# this object contains all object's positions!!
self.obj_positions = Obj_Pos()
self.controllers_list = []
self.robot_name_space = ""
self.reset_controls = False
super(FetchEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
# We Start all the ROS related Subscribers and publishers
self.JOINT_STATES_SUBSCRIBER = '/joint_states'
self.join_names = [
"joint0", "joint1", "joint2", "joint3", "joint4", "joint5",
"joint6"
]
self.gazebo.unpauseSim()
self._check_all_systems_ready()
self.joint_states_sub = rospy.Subscriber(self.JOINT_STATES_SUBSCRIBER,
JointState,
self.joints_callback)
self.joints = JointState()
# Start Services
self.move_fetch_object = MoveFetch()
# Wait until it has reached its Sturtup Position
self.wait_fetch_ready()
self.gazebo.pauseSim()
# Variables that we give through the constructor.
rospy.logdebug("========= Out Fetch Env")
def __init__(self):
# Launch the Task Simulated-Environment
# This is the path where the simulation files are,
# the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/fetch/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="fetch_simple_description",
launch_file_name="start_HER_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file relative to this TaskEnvironment
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/fetch/config",
yaml_file_name="fetch_push.yaml")
self.get_params()
# TODO: this must be continuous action space... don't follow the old implementation.
self.action_space = spaces.Discrete(self.n_actions)
observations_high_range = np.array([self.position_ee_max] *
self.n_observations)
observations_low_range = np.array([self.position_ee_min] *
self.n_observations)
observations_high_dist = np.array([self.max_distance])
observations_low_dist = np.array([0.0])
high = np.concatenate(
[observations_high_range, observations_high_dist])
low = np.concatenate([observations_low_range, observations_low_dist])
self.observation_space = spaces.Box(low, high)
# TODO: Clean up
# fetch_env.FetchEnv.__init__(self)
super(FetchPushEnv, self).__init__(ros_ws_abspath)
def __init__(self, ros_ws_abspath):
"""Initializes a new CubeSingleDisk environment.
Args:
"""
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="moving_cube_description",
launch_file_name="put_robot_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Variables that we give through the constructor.
# None in this case
# Internal Vars
self.controllers_list = [
'joint_state_controller',
'inertia_wheel_roll_joint_velocity_controller'
]
self.robot_name_space = "moving_cube"
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(CubeSingleDiskEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=True)
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/moving_cube/joint_states", JointState,
self._joints_callback)
rospy.Subscriber("/moving_cube/odom", Odometry, self._odom_callback)
self._roll_vel_pub = rospy.Publisher(
'/moving_cube/inertia_wheel_roll_joint_velocity_controller/command',
Float64,
queue_size=1)
self._check_all_systems_ready()
# We pause the simulation once everything is ready
self.gazebo.pauseSim()
def __init__(self):
ros_ws_abspath = rospy.get_param("/cartpole_v0/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="cartpole_description",
launch_file_name="start_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file="src/openai_ros/task_envs/cartpole_stay_up/config",
yaml_file_name="stay_up.yaml")
self.get_params()
self.action_space = spaces.Discrete(self.n_actions)
high = np.array([
2.5 * 2,
np.finfo(np.float32).max,
0.7 * 2,
np.finfo(np.float32).max])
self.observation_space = spaces.Box(-high, high)
# TODO: Remove when working
"""
cartpole_env.CartPoleEnv.__init__(
self, control_type=self.control_type
)
"""
# Here we will add any init functions prior to starting the MyRobotEnv
super(CartPoleStayUpEnv, self).__init__(control_type=self.control_type,
ros_ws_abspath=ros_ws_abspath)
def __init__(self):
"""
This Task Env is designed for having the TurtleBot2 in some kind of maze.
It will learn how to move around the maze without crashing.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/turtlebot2/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="turtlebot_gazebo",
launch_file_name="start_train_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(
rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/turtlebot2/config",
yaml_file_name="turtlebot2_goal_continuous_humanmodel.yaml")
# Here we will add any init functions prior to starting the MyRobotEnv
super(RealTurtleBot2Env, self).__init__(ros_ws_abspath)
# Only variable needed to be set here
high = numpy.array([1, 1, 1, 1])
low = numpy.array([-1, -1, -1, -1])
self.action_space = spaces.Box(low, high)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
self.success = True
#number_observations = rospy.get_param('/turtlebot2/n_observations')
# Actions and Observations
self.init_linear_forward_speed = rospy.get_param(
'/turtlebot2/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/turtlebot2/init_linear_turn_speed')
self.new_ranges = 180
self.min_range = rospy.get_param('/turtlebot2/min_range')
self.max_laser_value = rospy.get_param('/turtlebot2/max_laser_value')
self.min_laser_value = rospy.get_param('/turtlebot2/min_laser_value')
# We create two arrays based on the binary values that will be assigned
# In the discretization method.
laser_scan = self.get_laser_scan()
rospy.logdebug("laser_scan len===>" + str(len(laser_scan.ranges)))
high = numpy.hstack(
(numpy.array([0.5, 1, 0.5, 1]), 6 * numpy.ones([self.new_ranges])))
low = numpy.hstack((numpy.array([-0.5, -1, -0.5,
-1]), numpy.zeros([self.new_ranges])))
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
self.cumulated_steps = 0.0
def __init__(self):
"""
Make ShadowTc learn how pick up a ball
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/shadow_tc/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="shadow_gazebo",
launch_file_name="start_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/shadow_tc/config",
yaml_file_name="learn_to_pick_ball.yaml")
# We execute this one before because there are some functions that this
# TaskEnv uses that use variables from the parent class, like the effort limit fetch.
super(ShadowTcGetBallEnv, self).__init__(ros_ws_abspath)
# Here we will add any init functions prior to starting the MyRobotEnv
# Only variable needed to be set here
rospy.logdebug("Start ShadowTcGetBallEnv INIT...")
number_actions = rospy.get_param('/shadow_tc/n_actions')
self.action_space = spaces.Discrete(number_actions)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
self.movement_delta = rospy.get_param("/shadow_tc/movement_delta")
self.work_space_x_max = rospy.get_param("/shadow_tc/work_space/x_max")
self.work_space_x_min = rospy.get_param("/shadow_tc/work_space/x_min")
self.work_space_y_max = rospy.get_param("/shadow_tc/work_space/y_max")
self.work_space_y_min = rospy.get_param("/shadow_tc/work_space/y_min")
self.work_space_z_max = rospy.get_param("/shadow_tc/work_space/z_max")
self.work_space_z_min = rospy.get_param("/shadow_tc/work_space/z_min")
self.dec_obs = rospy.get_param(
"/shadow_tc/number_decimals_precision_obs")
self.acceptable_distance_to_ball = rospy.get_param(
"/shadow_tc/acceptable_distance_to_ball")
# We place the Maximum and minimum values of observations
# TODO: Fill when get_observations is done.
high = numpy.array([
self.work_space_x_max, self.work_space_y_max,
self.work_space_z_max, 1, 1, 1
])
low = numpy.array([
self.work_space_x_min, self.work_space_y_min,
self.work_space_z_min, 0, 0, 0
])
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.done_reward = rospy.get_param("/shadow_tc/done_reward")
self.closer_to_block_reward = rospy.get_param(
"/shadow_tc/closer_to_block_reward")
self.cumulated_steps = 0.0
rospy.logdebug("END shadow_tcGetBallEnv INIT...")
def __init__(self, ros_ws_abspath, ros_launch_file_package="coop_mapping", ros_launch_file_name="spawn_2_robots.launch"):
"""
This is my own custom environment for two TB3 robots, based on the original OpenAI ROS environments.
In this environment, the observation will be the union of the observations between both robots.
If one of them crashes, the episode ends.
The namespace for each robot is "tb3_0" and "tb3_1", which come from the "spawn_2_robots.launch" from "coop_mapping".
The namespaces are sometimes hardcoded in the functions, so take care when changing things.
"""
rospy.loginfo("Start TurtleBot3TwoRobotsEnv INIT...")
# Init namespace
ROBOT_1_NAMESPACE = '/tb3_0'
ROBOT_2_NAMESPACE = '/tb3_1'
self.robot_namespaces = [ROBOT_1_NAMESPACE, ROBOT_2_NAMESPACE]
# Init dictonaries for sensors
self.odom = {}
self.laser_scan = {}
# Init dictionaries for publishers
self._cmd_vel_pub = {}
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the two robots into the world
ROSLauncher(rospackage_name=ros_launch_file_package,
launch_file_name=ros_launch_file_name,
ros_ws_abspath=ros_ws_abspath)
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace (The environment itself is not in a namespace. The robots are.)
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(TurtleBot3TwoRobotsEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False)
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
self._check_all_sensors_ready()
# We Start all the ROS related Subscribers and publishers.
# This is hardcoded because I can't mess with callback arguments
rospy.Subscriber("/tb3_0/odom", Odometry, self._odom_callback_tb3_0)
rospy.Subscriber("/tb3_0/scan", LaserScan, self._laser_scan_callback_tb3_0)
rospy.Subscriber("/tb3_1/odom", Odometry, self._odom_callback_tb3_1)
rospy.Subscriber("/tb3_1/scan", LaserScan, self._laser_scan_callback_tb3_1)
for ns in self.robot_namespaces:
self._cmd_vel_pub[ns] = rospy.Publisher(ns + '/cmd_vel', Twist, queue_size=1)
self._check_publishers_connection(ns)
self.gazebo.pauseSim()
# Variable for crash
self._crashed = False
rospy.loginfo("Finished TurtleBot3TwoRobotsEnv INIT...")
def __init__(self):
"""
Make Wamv learn how to move straight from The starting point
to a desired point inside the designed corridor.
http://robotx.org/images/files/RobotX_2018_Task_Summary.pdf
Demonstrate Navigation Control
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/wamv/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="robotx_gazebo",
launch_file_name="start_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/wamv/config",
yaml_file_name="wamv_nav_twosets_buoys.yaml")
# Here we will add any init functions prior to starting the MyRobotEnv
super(WamvNavTwoSetsBuoysEnv, self).__init__(ros_ws_abspath)
# Only variable needed to be set here
rospy.logdebug("Start WamvNavTwoSetsBuoysEnv INIT...")
number_actions = rospy.get_param('/wamv/n_actions')
self.action_space = spaces.Discrete(number_actions)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
# Actions and Observations
self.propeller_high_speed = rospy.get_param(
'/wamv/propeller_high_speed')
self.propeller_low_speed = rospy.get_param('/wamv/propeller_low_speed')
self.max_angular_speed = rospy.get_param('/wamv/max_angular_speed')
self.max_distance_from_des_point = rospy.get_param(
'/wamv/max_distance_from_des_point')
# Get Desired Point to Get
self.desired_point = Point()
self.desired_point.x = rospy.get_param("/wamv/desired_point/x")
self.desired_point.y = rospy.get_param("/wamv/desired_point/y")
self.desired_point.z = rospy.get_param("/wamv/desired_point/z")
self.desired_point_epsilon = rospy.get_param(
"/wamv/desired_point_epsilon")
self.work_space_x_max = rospy.get_param("/wamv/work_space/x_max")
self.work_space_x_min = rospy.get_param("/wamv/work_space/x_min")
self.work_space_y_max = rospy.get_param("/wamv/work_space/y_max")
self.work_space_y_min = rospy.get_param("/wamv/work_space/y_min")
self.dec_obs = rospy.get_param("/wamv/number_decimals_precision_obs")
# We place the Maximum and minimum values of observations
high = numpy.array([
self.work_space_x_max, self.work_space_y_max, 1.57, 1.57, 3.14,
self.propeller_high_speed, self.propeller_high_speed,
self.max_angular_speed, self.max_distance_from_des_point
])
low = numpy.array([
self.work_space_x_min, self.work_space_y_min, -1 * 1.57, -1 * 1.57,
-1 * 3.14, -1 * self.propeller_high_speed,
-1 * self.propeller_high_speed, -1 * self.max_angular_speed, 0.0
])
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.done_reward = rospy.get_param("/wamv/done_reward")
self.closer_to_point_reward = rospy.get_param(
"/wamv/closer_to_point_reward")
self.cumulated_steps = 0.0
rospy.logdebug("END WamvNavTwoSetsBuoysEnv INIT...")
def __init__(self):
# Launch the Task Simulated-Environment
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/moving_cube/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="moving_cube_description",
launch_file_name="start_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/moving_cube/config",
yaml_file_name="one_disk_walk.yaml")
# Only variable needed to be set here
number_actions = rospy.get_param('/moving_cube/n_actions')
self.action_space = spaces.Discrete(number_actions)
#number_observations = rospy.get_param('/moving_cube/n_observations')
"""
We set the Observation space for the 6 observations
cube_observations = [
round(current_disk_roll_vel, 0),
round(y_distance, 1),
round(roll, 1),
round(pitch, 1),
round(y_linear_speed,1),
round(yaw, 1),
]
"""
# Actions and Observations
self.roll_speed_fixed_value = rospy.get_param(
'/moving_cube/roll_speed_fixed_value')
self.roll_speed_increment_value = rospy.get_param(
'/moving_cube/roll_speed_increment_value')
self.max_distance = rospy.get_param('/moving_cube/max_distance')
max_roll = 2 * math.pi
self.max_pitch_angle = rospy.get_param('/moving_cube/max_pitch_angle')
self.max_y_linear_speed = rospy.get_param(
'/moving_cube/max_y_linear_speed')
self.max_yaw_angle = rospy.get_param('/moving_cube/max_yaw_angle')
high = numpy.array([
self.roll_speed_fixed_value,
self.max_distance,
max_roll,
self.max_pitch_angle,
self.max_y_linear_speed,
self.max_y_linear_speed,
])
self.observation_space = spaces.Box(-high, high)
rospy.logwarn("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logwarn("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Variables that we retrieve through the param server, loded when launch training launch.
self.init_roll_vel = rospy.get_param("/moving_cube/init_roll_vel")
# Get Observations
self.start_point = Point()
self.start_point.x = rospy.get_param("/moving_cube/init_cube_pose/x")
self.start_point.y = rospy.get_param("/moving_cube/init_cube_pose/y")
self.start_point.z = rospy.get_param("/moving_cube/init_cube_pose/z")
# Rewards
self.move_distance_reward_weight = rospy.get_param(
"/moving_cube/move_distance_reward_weight")
self.y_linear_speed_reward_weight = rospy.get_param(
"/moving_cube/y_linear_speed_reward_weight")
self.y_axis_angle_reward_weight = rospy.get_param(
"/moving_cube/y_axis_angle_reward_weight")
self.end_episode_points = rospy.get_param(
"/moving_cube/end_episode_points")
self.roll_reward_weight = rospy.get_param(
"/moving_cube/roll_reward_weight")
self.cumulated_steps = 0.0
# Here we will add any init functions prior to starting the MyRobotEnv
super(MovingCubeOneDiskWalkEnv, self).__init__(ros_ws_abspath)
def __init__(self, ros_ws_abspath):
"""
Initializes a new HopperEnv environment.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /drone/down_camera/image_raw: RGB Camera facing down.
* /drone/front_camera/image_raw: RGB Camera facing front.
* /drone/imu: IMU of the drone giving acceleration and orientation relative to world.
* /drone/sonar: Sonar readings facing front
* /drone/gt_pose: Get position and orientation in Global space
* /drone/gt_vel: Get the linear velocity , the angular doesnt record anything.
Actuators Topic List:
* /cmd_vel: Move the Drone Around when you have taken off.
* /drone/takeoff: Publish into it to take off
* /drone/land: Publish to make ParrotDrone Land
Args:
"""
rospy.logdebug("Start HopperEnv INIT...")
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="legged_robots_sims",
launch_file_name="put_robot_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Variables that we give through the constructor.
# None in this case
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = [
'joint_state_controller', 'haa_joint_position_controller',
'hfe_joint_position_controller', 'kfe_joint_position_controller'
]
# It doesnt use namespace
self.robot_name_space = "monoped"
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(HopperEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
rospy.logdebug("HopperEnv unpause1...")
self.gazebo.unpauseSim()
# self.controllers_object.reset_controllers()
self._check_all_systems_ready()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/odom", Odometry, self._odom_callback)
# We use the IMU for orientation and linearacceleration detection
rospy.Subscriber("/monoped/imu/data", Imu, self._imu_callback)
# We use it to get the contact force, to know if its in the air or stumping too hard.
rospy.Subscriber("/lowerleg_contactsensor_state", ContactsState,
self._contact_callback)
# We use it to get the joints positions and calculate the reward associated to it
rospy.Subscriber("/monoped/joint_states", JointState,
self._joints_state_callback)
self.publishers_array = []
self._haa_joint_pub = rospy.Publisher(
'/monoped/haa_joint_position_controller/command',
Float64,
queue_size=1)
self._hfe_joint_pub = rospy.Publisher(
'/monoped/hfe_joint_position_controller/command',
Float64,
queue_size=1)
self._kfe_joint_pub = rospy.Publisher(
'/monoped/kfe_joint_position_controller/command',
Float64,
queue_size=1)
self.publishers_array.append(self._haa_joint_pub)
self.publishers_array.append(self._hfe_joint_pub)
self.publishers_array.append(self._kfe_joint_pub)
self._check_all_publishers_ready()
self.gazebo.pauseSim()
rospy.logdebug("Finished HopperEnv INIT...")
def __init__(self, ros_ws_abspath):
"""
Initializes a new DeepSoccer environment.
DeepSoccer doesnt use controller_manager, therefore we wont reset the
controllers in the standard fashion. For the moment we wont reset them.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /odom : Odometry readings of the Base of the Robot
* /camera/depth/image_raw: 2d Depth image of the depth sensor.
* /camera/depth/points: Pointcloud sensor readings
* /camera/rgb/image_raw: RGB camera
* /kobuki/laser/scan: Laser Readings
Actuators Topic List: /cmd_vel,
Args:
"""
rospy.logdebug("Start DeepSoccerEnv INIT...")
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="deepsoccer_gazebo",
launch_file_name="main_soccer.launch",
ros_ws_abspath=ros_ws_abspath)
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(DeepSoccerEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
self._check_all_sensors_ready()
# We Start all the ROS related Subscribers and publishers
#rospy.Subscriber("/robot1/camera1/image_raw", Image, self.image_callback)
rospy.Subscriber('/gazebo/model_states', ModelStates,
self._model_state_callback)
rospy.Subscriber('/robot1/laser/scan', LaserScan,
self._laser_scan_callback)
rospy.Subscriber('/robot1/ir/scan', LaserScan, self._ir_scan_callback)
rospy.Subscriber("/robot1/camera/image_raw", Image,
self._camera_rgb_image_raw_callback)
#rospy.Subscriber("/odom", Odometry, self._odom_callback)
#rospy.Subscriber("/camera/depth/image_raw", Image, self._camera_depth_image_raw_callback)
#rospy.Subscriber("/camera/depth/points", PointCloud2, self._camera_depth_points_callback)
#rospy.Subscriber("/kobuki/laser/scan", LaserScan, self._laser_scan_callback)
#self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
#rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.wheel1 = rospy.Publisher(
'/robot1/wheel1_velocity_controller/command',
Float64,
queue_size=5)
self.wheel2 = rospy.Publisher(
'/robot1/wheel2_velocity_controller/command',
Float64,
queue_size=5)
self.wheel3 = rospy.Publisher(
'/robot1/wheel3_velocity_controller/command',
Float64,
queue_size=5)
self.wheel4 = rospy.Publisher(
'/robot1/wheel4_velocity_controller/command',
Float64,
queue_size=5)
self.roller = rospy.Publisher(
'/robot1/roller_velocity_controller/command',
Float64,
queue_size=5)
self.stick = rospy.Publisher('/robot1/SolenoidElectromagnet/vel_cmd',
Float32,
queue_size=5)
#self.stick = rospy.Publisher('/robot1/stick_velocity_controller/command', Float64, queue_size=5)
self._check_publishers_connection()
self.gazebo.pauseSim()
rospy.logdebug("Finished MyDeepSoccerSingleEnv INIT...")
def __init__(self):
"""
This Task Env is designed for having the sumit_xl in the sumit_xl world
closed room with columns.
It will learn how to move around without crashing.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/sumit_xl/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="summit_xl_gazebo",
launch_file_name="start_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file="src/openai_ros/task_envs/sumit_xl/config",
yaml_file_name="sumit_xl_room.yaml")
# Here we will add any init functions prior to starting the MyRobotEnv
super(SumitXlRoom, self).__init__(ros_ws_abspath)
# Only variable needed to be set here
number_actions = rospy.get_param('/sumit_xl/n_actions')
self.action_space = spaces.Discrete(number_actions)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
# Actions and Observations
self.linear_forward_speed = rospy.get_param(
'/sumit_xl/linear_forward_speed')
self.linear_turn_speed = rospy.get_param('/sumit_xl/linear_turn_speed')
self.angular_speed = rospy.get_param('/sumit_xl/angular_speed')
self.init_linear_forward_speed = rospy.get_param(
'/sumit_xl/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/sumit_xl/init_linear_turn_speed')
self.new_ranges = rospy.get_param('/sumit_xl/new_ranges')
self.min_range = rospy.get_param('/sumit_xl/min_range')
self.max_laser_value = rospy.get_param('/sumit_xl/max_laser_value')
self.min_laser_value = rospy.get_param('/sumit_xl/min_laser_value')
self.max_linear_aceleration = rospy.get_param(
'/sumit_xl/max_linear_aceleration')
self.max_distance = rospy.get_param('/sumit_xl/max_distance')
# Get Desired Point to Get
self.desired_point = Point()
self.desired_point.x = rospy.get_param("/sumit_xl/desired_pose/x")
self.desired_point.y = rospy.get_param("/sumit_xl/desired_pose/y")
self.desired_point.z = rospy.get_param("/sumit_xl/desired_pose/z")
# We create the arrays for the laser readings
# We also create the arrays for the odometry readings
# We join them toeguether.
laser_scan = self.get_laser_scan()
num_laser_readings = len(laser_scan.ranges)/self.new_ranges
high_laser = numpy.full((num_laser_readings), self.max_laser_value)
low_laser = numpy.full((num_laser_readings), self.min_laser_value)
# We place the Maximum and minimum values of the X,Y and YAW of the odometry
# The odometry yaw can be any value in the circunference.
high_odometry = numpy.array(
[self.max_distance, self.max_distance, 3.14])
low_odometry = numpy.array(
[-1*self.max_distance, -1*self.max_distance, -1*3.14])
# We join both arrays
high = numpy.concatenate([high_laser, high_odometry])
low = numpy.concatenate([low_laser, low_odometry])
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>"+str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.closer_to_point_reward = rospy.get_param(
"/sumit_xl/closer_to_point_reward")
self.not_ending_point_reward = rospy.get_param(
"/sumit_xl/not_ending_point_reward")
self.end_episode_points = rospy.get_param(
"/sumit_xl/end_episode_points")
self.cumulated_steps = 0.0
def __init__(self, ros_ws_abspath):
"""
Initializes a new WamvEnv environment.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /wamv/odom: Odometry of the Base of Wamv
Actuators Topic List:
* /cmd_drive: You publish the speed of the left and right propellers.
Args:
"""
rospy.logdebug("Start WamvEnv INIT...")
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="robotx_gazebo",
launch_file_name="put_wamv_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(WamvEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
rospy.logdebug("WamvEnv unpause1...")
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
self._check_all_systems_ready()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/wamv/odom", Odometry, self._odom_callback)
self.publishers_array = []
self._cmd_drive_pub = rospy.Publisher('/cmd_drive', UsvDrive, queue_size=1)
self.publishers_array.append(self._cmd_drive_pub)
self._check_all_publishers_ready()
self.gazebo.pauseSim()
rospy.logdebug("Finished WamvEnv INIT...")
def __init__(self):
"""
This Task Env is designed for having the TurtleBot2 in some kind of maze.
It will learn how to move around the maze without crashing.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/turtlebot2/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="turtlebot_gazebo",
launch_file_name="start_wall_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/turtlebot2/config",
yaml_file_name="turtlebot2_wall.yaml")
# Here we will add any init functions prior to starting the MyRobotEnv
super(TurtleBot2WallEnv, self).__init__(ros_ws_abspath)
# Only variable needed to be set here
number_actions = rospy.get_param('/turtlebot2/n_actions')
self.action_space = spaces.Discrete(number_actions)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
#number_observations = rospy.get_param('/turtlebot2/n_observations')
"""
We set the Observation space for the 6 observations
cube_observations = [
round(current_disk_roll_vel, 0),
round(y_distance, 1),
round(roll, 1),
round(pitch, 1),
round(y_linear_speed,1),
round(yaw, 1),
]
"""
# Actions and Observations
self.linear_forward_speed = rospy.get_param(
'/turtlebot2/linear_forward_speed')
self.linear_turn_speed = rospy.get_param(
'/turtlebot2/linear_turn_speed')
self.angular_speed = rospy.get_param('/turtlebot2/angular_speed')
self.init_linear_forward_speed = rospy.get_param(
'/turtlebot2/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/turtlebot2/init_linear_turn_speed')
self.new_ranges = rospy.get_param('/turtlebot2/new_ranges')
self.min_range = rospy.get_param('/turtlebot2/min_range')
self.max_laser_value = rospy.get_param('/turtlebot2/max_laser_value')
self.min_laser_value = rospy.get_param('/turtlebot2/min_laser_value')
# Get Desired Point to Get
self.desired_point = Point()
self.desired_point.x = rospy.get_param("/turtlebot2/desired_pose/x")
self.desired_point.y = rospy.get_param("/turtlebot2/desired_pose/y")
self.desired_point.z = rospy.get_param("/turtlebot2/desired_pose/z")
# We create two arrays based on the binary values that will be assigned
# In the discretization method.
laser_scan = self.get_laser_scan()
rospy.logdebug("laser_scan len===>" + str(len(laser_scan.ranges)))
num_laser_readings = len(laser_scan.ranges) / self.new_ranges
high = numpy.full((num_laser_readings), self.max_laser_value)
low = numpy.full((num_laser_readings), self.min_laser_value)
# We only use two integers
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.forwards_reward = rospy.get_param("/turtlebot2/forwards_reward")
self.turn_reward = rospy.get_param("/turtlebot2/turn_reward")
self.end_episode_points = rospy.get_param(
"/turtlebot2/end_episode_points")
self.cumulated_steps = 0.0
def __init__(self, ros_ws_abspath):
print("Start AliengoEnv INIT...")
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="aliengo_gazebo",
launch_file_name="aliengo_empty_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
self.lowState = LowState()
self.motorState = MotorState()
self.lowCmd = LowCmd()
self.odom = Odometry()
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(AliengoEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
self.gazebo.unpauseSim()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/aliengo/odometry", Odometry, self.odomCallback)
rospy.Subscriber("/laikago_gazebo/FR_hip_controller/state", MotorState,
self.FRhipCallback)
rospy.Subscriber("/laikago_gazebo/FR_thigh_controller/state",
MotorState, self.FRthighCallback)
rospy.Subscriber("/laikago_gazebo/FR_calf_controller/state",
MotorState, self.FRcalfCallback)
rospy.Subscriber("/laikago_gazebo/FL_hip_controller/state", MotorState,
self.FLhipCallback)
rospy.Subscriber("/laikago_gazebo/FL_thigh_controller/state",
MotorState, self.FLthighCallback)
rospy.Subscriber("/laikago_gazebo/FL_calf_controller/state",
MotorState, self.FLcalfCallback)
rospy.Subscriber("/laikago_gazebo/RR_hip_controller/state", MotorState,
self.RRhipCallback)
rospy.Subscriber("/laikago_gazebo/RR_thigh_controller/state",
MotorState, self.RRthighCallback)
rospy.Subscriber("/laikago_gazebo/RR_calf_controller/state",
MotorState, self.RRcalfCallback)
rospy.Subscriber("/laikago_gazebo/RL_hip_controller/state", MotorState,
self.RLhipCallback)
rospy.Subscriber("/laikago_gazebo/RL_thigh_controller/state",
MotorState, self.RLthighCallback)
rospy.Subscriber("/laikago_gazebo/RL_calf_controller/state",
MotorState, self.RLcalfCallback)
rospy.Subscriber("/visual/FR_foot_contact/the_force", WrenchStamped,
self.FRfootCallback)
rospy.Subscriber("/visual/FL_foot_contact/the_force", WrenchStamped,
self.FLfootCallback)
rospy.Subscriber("/visual/RR_foot_contact/the_force", WrenchStamped,
self.RRfootCallback)
rospy.Subscriber("/visual/RL_foot_contact/the_force", WrenchStamped,
self.RLfootCallback)
self.lowState_pub = rospy.Publisher("/laikago_gazebo/lowState/state",
LowState,
queue_size=1)
self.servo_pub_0 = rospy.Publisher(
"/laikago_gazebo/FR_hip_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_1 = rospy.Publisher(
"/laikago_gazebo/FR_thigh_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_2 = rospy.Publisher(
"/laikago_gazebo/FR_calf_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_3 = rospy.Publisher(
"/laikago_gazebo/FL_hip_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_4 = rospy.Publisher(
"/laikago_gazebo/FL_thigh_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_5 = rospy.Publisher(
"/laikago_gazebo/FL_calf_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_6 = rospy.Publisher(
"/laikago_gazebo/RR_hip_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_7 = rospy.Publisher(
"/laikago_gazebo/RR_thigh_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_8 = rospy.Publisher(
"/laikago_gazebo/RR_calf_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_9 = rospy.Publisher(
"/laikago_gazebo/RL_hip_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_10 = rospy.Publisher(
"/laikago_gazebo/RL_thigh_controller/command",
MotorCmd,
queue_size=1)
self.servo_pub_11 = rospy.Publisher(
"/laikago_gazebo/RL_calf_controller/command",
MotorCmd,
queue_size=1)
self.gazebo.pauseSim()
rospy.logdebug("Finished AliengoEnv INIT...")
def __init__(self, ros_ws_abspath):
"""
Initializes a new TurtleBot3Env environment.
TurtleBot3 doesnt use controller_manager, therefore we wont reset the
controllers in the standard fashion. For the moment we wont reset them.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /odom : Odometry readings of the Base of the Robot
* /imu: Inertial Mesuring Unit that gives relative accelerations and orientations.
* /scan: Laser Readings
Actuators Topic List: /cmd_vel,
Args:
"""
rospy.logdebug("Start TurtleBot3Env INIT...")
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="turtlebot3_gazebo",
launch_file_name="put_robot_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(TurtleBot3Env,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False)
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
self._check_all_sensors_ready()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/odom", Odometry, self._odom_callback)
rospy.Subscriber("/imu", Imu, self._imu_callback)
rospy.Subscriber("/scan", LaserScan, self._laser_scan_callback)
self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self._check_publishers_connection()
self.gazebo.pauseSim()
rospy.logdebug("Finished TurtleBot3Env INIT...")
def __init__(self, yaml_config_file="turtlebot3_world_mapping.yaml"):
"""
This Task Env is designed for having two TurtleBot3 robots in the turtlebot3 world closed room with columns.
It will learn how to move around without crashing.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/turtlebot3/ros_ws_abspath", None)
if os.environ.get('ROS_WS') != None:
ros_ws_abspath = os.environ.get('ROS_WS')
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file="src/openai_ros/task_envs/turtlebot3_my_envs/config",
yaml_file_name=yaml_config_file)
# Depending on which environment we're in, decide to launch Gazebo with or without GUI.
gazebo_launch_file = "start_empty_tb3_world.launch"
if os.environ.get('ENV') == 'deploy' or os.environ.get('ENV') == 'dev-no-gazebo':
gazebo_launch_file = "start_empty_tb3_world_no_gui.launch"
ROSLauncher(rospackage_name="coop_mapping",
launch_file_name=gazebo_launch_file,
ros_ws_abspath=ros_ws_abspath)
# Here we will add any init functions prior to starting the MyRobotEnv
super(TurtleBot3WorldMapping2RobotsEnv, self).__init__(ros_ws_abspath,
ros_launch_file_package="coop_mapping",
ros_launch_file_name="spawn_2_robots.launch")
### ACTIONS
# Only variable needed to be set here
self.number_actions = rospy.get_param('/turtlebot3/n_actions')
self.number_robots = len(self.robot_namespaces) # should be 2
# 3x3 possible actions (a % 3 -> robot 1 action, a / 3 -> robot 2 action)
self.action_space = spaces.Discrete(
pow(self.number_actions, self.number_robots))
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-np.inf, np.inf)
### OBSERVATIONS
self.linear_forward_speed = rospy.get_param(
'/turtlebot3/linear_forward_speed')
self.linear_turn_speed = rospy.get_param(
'/turtlebot3/linear_turn_speed')
self.angular_speed = rospy.get_param('/turtlebot3/angular_speed')
self.init_linear_forward_speed = rospy.get_param(
'/turtlebot3/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/turtlebot3/init_linear_turn_speed')
self.new_ranges = rospy.get_param('/turtlebot3/new_ranges')
self.min_range = rospy.get_param('/turtlebot3/min_range')
self.max_laser_value = rospy.get_param('/turtlebot3/max_laser_value')
self.min_laser_value = rospy.get_param('/turtlebot3/min_laser_value')
"""
An observation is a MultiDiscrete element, with 4 components.
1. LaserScan rays component with R integers, where R is the number of laser scan
rays we are using for observation (one for each robot).
- Each value represents the distance to an obstacle rounded to the nearest integer (in meteres).
2. Position information with 2 integers (x, y) (one for each robot).
- Each value represents the position of the robot along a normalized axis, rouned to the nearest integer.
3. Rotation information with 1 integer (rotation along the z axis) (one for each robot).
- Each value represents the orientation in a normalized scale, rounded to the nearest integer.
4. Simplified map exploration with NxN integers, where N is the dimension of the matrix
that portrays the level of exploration in the map (one for BOTH robots).
- Each value represents the average number of pixels explored (-1 is unexplored, 1 is explored).
The value is normalized and then rounded to the nearest integer.
"""
# We create two arrays based on the binary values that will be assigned
# In the discretization method.
laser_scan = self.laser_scan[self.robot_namespaces[0]]
num_laser_readings = len(laser_scan.ranges)/self.new_ranges
high = np.full((num_laser_readings), self.max_laser_value)
low = np.full((num_laser_readings), self.min_laser_value)
self.position_num_discrete_values = 40
self.rotation_num_discrete_values = 6
self.simplified_grid_dimension = 4
self.simplified_grid_num_discrete_values = 40
laser_scan_component_shape = [
round(self.max_laser_value)] * (self.new_ranges * self.number_robots)
position_component_shape = [
self.position_num_discrete_values] * (2 * self.number_robots)
rotation_component_shape = [
self.rotation_num_discrete_values] * (1 * self.number_robots)
map_exploration_component_shape = [
self.simplified_grid_num_discrete_values] * self.simplified_grid_dimension * self.simplified_grid_dimension
multi_discrete_shape = list(itertools.chain(laser_scan_component_shape,
position_component_shape,
rotation_component_shape,
map_exploration_component_shape))
self.observation_space = spaces.MultiDiscrete(
multi_discrete_shape)
rospy.loginfo("ACTION SPACES TYPE===>"+str(self.action_space))
rospy.loginfo("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.no_crash_reward_points = rospy.get_param(
"/turtlebot3/no_crash_reward_points")
self.crash_reward_points = rospy.get_param(
"/turtlebot3/crash_reward_points")
self.exploration_multi_factor = rospy.get_param(
"/turtlebot3/exploration_multi_factor")
self.cumulated_steps = 0.0
# Init dictionary for both robots actions
self.last_action = {}
# Set the logging system
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('coop_mapping')
# Control variables for launching nodes from .launch files
self._gmapping_launch_file = pkg_path + os.path.sep + \
'launch' + os.path.sep + 'init_2_robots_mapping.launch'
self._gmapping_running = False
self._gmapping_launch = None
self._map_merge_launch_file = pkg_path + os.path.sep + \
'launch' + os.path.sep + 'init_2_robots_multi_map_merge.launch'
self._map_merge_running = False
self._map_merge_launch = None
self._hector_saver_launch_file = pkg_path + os.path.sep + \
'launch' + os.path.sep + 'init_2_robots_hector_saver.launch'
self._hector_saver_running = False
self._hector_saver_launch = None
# Variables for map comparison
self.map_data = None
self._num_white_pixels_to_explore = get_number_of_almost_white_pixels(self.actual_map_file)
# The minimum difference that has been observed
self.current_min_map_difference = None
# The thresholds to calculate map exploration
self.threshold_occupied = 65
self.threshold_free = 25
# The area in pixels that has been explored
self.previous_max_explored_area = None
self.current_max_explored_area = None
# Start subscriber to /map to save it to file
self._map_file_name = "/tmp/ros_merge_map"
self._map_updated_after_action = False
rospy.Subscriber('map', OccupancyGrid, self._map_callback)
# Logging episode information
self._first_episode = True
# TF listener to get robots position
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def __init__(self, ros_ws_abspath):
"""
Initializes a new ShadowTcEnv environment.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /imu/data
* /joint_states
Actuators Topic List:
* As actuator we will use a class SmartGrasper to interface.
We use smart_grasping_sandbox smart_grasper.py, to move and get the pose
of the ball and the tool tip.
Args:
"""
rospy.logdebug("Start ShadowTcEnv INIT...")
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="shadow_gazebo",
launch_file_name="put_shadow_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(ShadowTcEnv,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="NO_RESET_SIM")
rospy.logdebug("ShadowTcEnv unpause...")
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
self._check_all_systems_ready()
rospy.Subscriber("/imu/data", Imu, self._imu_callback)
rospy.Subscriber("/joint_states", JointState,
self._joints_state_callback)
#rospy.Subscriber('/planning_scene', PlanningScene, self._planning_scene_callback)
self._setup_smart_grasper()
self.gazebo.pauseSim()
rospy.logdebug("Finished ShadowTcEnv INIT...")
def __init__(self):
"""
This Task Env is designed for having the TurtleBot2 in some kind of maze.
It will learn how to move around the maze without crashing.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/turtlebot2/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="turtlebot_gazebo",
launch_file_name="start_goal_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(
rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/turtlebot2/config",
yaml_file_name="turtlebot2_goal_continuous_humanmodel.yaml")
# Here we will add any init functions prior to starting the MyRobotEnv
super(TurtleBot2HumanModelEnv, self).__init__(ros_ws_abspath)
# Only variable needed to be set here
high = numpy.array([1, 1, 1, 1])
low = numpy.array([-1, -1, -1, -1])
#high = numpy.array([0,1,1,1])
#low = numpy.array([0,-1,-1,-1])
self.action_space = spaces.Box(low, high)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
self.success = True
self.beta = 0
#self.beta = 1
#number_observations = rospy.get_param('/turtlebot2/n_observations')
"""
We set the Observation space for the 6 observations
cube_observations = [
round(current_disk_roll_vel, 0),
round(y_distance, 1),
round(roll, 1),
round(pitch, 1),
round(y_linear_speed,1),
round(yaw, 1),
]
"""
# Actions and Observations
self.init_linear_forward_speed = rospy.get_param(
'/turtlebot2/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/turtlebot2/init_linear_turn_speed')
self.new_ranges = 180
self.min_range = rospy.get_param('/turtlebot2/min_range')
self.max_laser_value = rospy.get_param('/turtlebot2/max_laser_value')
self.min_laser_value = rospy.get_param('/turtlebot2/min_laser_value')
# Get Desired Point to Get
self.desired_point = Point()
self.desired_point.x = rospy.get_param("/turtlebot2/desired_pose/x")
self.desired_point.y = rospy.get_param("/turtlebot2/desired_pose/y")
self.desired_point.z = rospy.get_param("/turtlebot2/desired_pose/z")
self.state_msg = ModelState()
self.state_msg.model_name = 'mobile_base'
self.state_msg.pose.position.x = 0
self.state_msg.pose.position.y = 0
self.state_msg.pose.position.z = 0
self.state_msg.pose.orientation.x = 0
self.state_msg.pose.orientation.y = 0
self.state_msg.pose.orientation.z = 0
self.state_msg.pose.orientation.w = 0
# We create two arrays based on the binary values that will be assigned
# In the discretization method.
laser_scan = self.get_laser_scan()
#print("lidar data:", laser_scan)
rospy.logdebug("laser_scan len===>" + str(len(laser_scan.ranges)))
#high = numpy.array([0.5,1,1,1,1,1,6,3.14])#,numpy.array([12,6,3.14,1,3.14,0.5,1]),6*numpy.ones([self.new_ranges]),numpy.array([12,6,3.14,1,3.14,0.5,1]),6*numpy.ones([self.new_ranges])))
high = numpy.hstack(
(numpy.array([0.5, 1, 0.5, 1]), 6 * numpy.ones([self.new_ranges])))
#high = numpy.hstack((numpy.array([0,1,0.5,1]),6*numpy.ones([self.new_ranges])))
#high = numpy.hstack((numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges])))
#low = numpy.array([-0.5,-1,-1,-1,-1,-1, 0,-3.14])#,numpy.array([-1,-1*6,-1*3.14,-1,-3.14,-0.5,-1]),numpy.zeros([self.new_ranges]),numpy.array([-1,-1*6,-1*3.14,-1,-3.14,-0.5,-1]),numpy.zeros([self.new_ranges])))
low = numpy.hstack((numpy.array([-0.5, -1, -0.5,
-1]), numpy.zeros([self.new_ranges])))
#low = numpy.hstack((numpy.array([0,-1,-0.5,-1]),numpy.zeros([self.new_ranges])))
#low = numpy.hstack((numpy.array([-1,-1]),numpy.zeros([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges])))
# We only use two integers
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.forwards_reward = rospy.get_param("/turtlebot2/forwards_reward")
self.turn_reward = rospy.get_param("/turtlebot2/turn_reward")
self.end_episode_points = rospy.get_param(
"/turtlebot2/end_episode_points")
self.cumulated_steps = 0.0
############################## goal ##############################################
self.goal_position = Pose()
self.f = open(
'/home/i2rlab/shahil_files/shahil_RL_ws_new/src/turtlebot/turtlebot_gazebo/worlds/goal/model.sdf',
'r')
self.sdff = self.f.read()
############################## Obstacle ##########################################
self.angle = numpy.linspace(-179, 179, 180) / 180 * numpy.pi
self.cos = numpy.cos(self.angle)
self.sin = numpy.sin(self.angle)
############################## Human Model ######################################
config = tf.ConfigProto(gpu_options=tf.GPUOptions(allow_growth=True))
self.sess = tf.Session(config=config)
self.S1 = tf.placeholder(tf.float32, [None, 5], 'S1')
#self.S2 = tf.placeholder(tf.float32, [None, 198, 1], 'S2')
self.S2 = tf.placeholder(tf.float32, [None, 180], 'S2')
self.keep_prob = tf.placeholder(tf.float32)
self.a_predict = self.build_c(self.S1, self.S2, self.keep_prob)
self.loader()
self.goal_space()
self.time_start = 0
def __init__(self, ros_ws_abspath):
"""
Initializes a new SumitXlEnv environment.
Execute a call to service /summit_xl/controller_manager/list_controllers
To get the list of controllers to be restrarted
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /gps/fix : GPS position Data
* /gps/fix_velocity: GPS Speed data
* /hokuyo_base/scan: Laser Readings
* /imu/data: Inertial Mesurment Unit data, orientation and acceleration
* /orbbec_astra/depth/image_raw
* /orbbec_astra/depth/points
* /orbbec_astra/rgb/image_raw
* /odom: Odometry
Actuators Topic List: /cmd_vel,
Args:
"""
print("Start SumitXlEnv INIT...")
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="summit_xl_gazebo",
launch_file_name="put_summit_xl_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
print("SPAWN DONE SumitXlEnv INIT...")
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = ["joint_read_state_controller",
"joint_blw_velocity_controller",
"joint_brw_velocity_controller",
"joint_flw_velocity_controller",
"joint_frw_velocity_controller"
]
# It doesnt use namespace
self.robot_name_space = "summit_xl"
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
print("START OpenAIROS CORE SumitXlEnv INIT...")
super(SumitXlEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
print("DONE OpenAIROS CORE SumitXlEnv INIT...")
self.gazebo.unpauseSim()
# TODO: See why this doesnt work in Summit XL
# self.controllers_object.reset_controllers()
print("START CHECK SENSORS SumitXlEnv INIT...")
self._check_all_sensors_ready()
print("DONE CHECK SENSORS SumitXlEnv INIT...")
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/gps/fix", NavSatFix, self._gps_fix_callback)
rospy.Subscriber("/gps/fix_velocity", Vector3Stamped,
self._gps_fix_velocity_callback)
rospy.Subscriber("/orbbec_astra/depth/image_raw", Image,
self._camera_depth_image_raw_callback)
rospy.Subscriber("/orbbec_astra/depth/points",
PointCloud2, self._camera_depth_points_callback)
rospy.Subscriber("/orbbec_astra/rgb/image_raw", Image,
self._camera_rgb_image_raw_callback)
rospy.Subscriber("/hokuyo_base/scan", LaserScan,
self._laser_scan_callback)
rospy.Subscriber("/imu/data", Imu, self._imu_callback)
rospy.Subscriber("/odom", Odometry, self._odom_callback)
self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
print("START CHECK PUBLISHERS SumitXlEnv INIT...")
self._check_publishers_connection()
print("DONE CHECK PUBLISHERS SumitXlEnv INIT...")
self.gazebo.pauseSim()
print("Finished SumitXlEnv INIT...")
def __init__(self):
"""
This Task Env is designed for having the husarion in the husarion world
closed room with columns.
It will learn how to move around without crashing.
"""
# Launch the Task Simulated-Environment
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/husarion/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="rosbot_gazebo",
launch_file_name="start_world_maze.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(
rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/husarion/config",
yaml_file_name="husarion_get_to_position_turtlebot_playground.yaml"
)
# Only variable needed to be set here
number_actions = rospy.get_param('/husarion/n_actions')
self.action_space = spaces.Discrete(number_actions)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
# Actions and Observations
self.init_linear_forward_speed = rospy.get_param(
'/husarion/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/husarion/init_linear_turn_speed')
self.linear_forward_speed = rospy.get_param(
'/husarion/linear_forward_speed')
self.linear_turn_speed = rospy.get_param('/husarion/linear_turn_speed')
self.angular_speed = rospy.get_param('/husarion/angular_speed')
self.new_ranges = rospy.get_param('/husarion/new_ranges')
self.max_laser_value = rospy.get_param('/husarion/max_laser_value')
self.min_laser_value = rospy.get_param('/husarion/min_laser_value')
self.work_space_x_max = rospy.get_param("/husarion/work_space/x_max")
self.work_space_x_min = rospy.get_param("/husarion/work_space/x_min")
self.work_space_y_max = rospy.get_param("/husarion/work_space/y_max")
self.work_space_y_min = rospy.get_param("/husarion/work_space/y_min")
# Get Desired Point to Get
self.desired_position = Point()
self.desired_position.x = rospy.get_param("/husarion/desired_pose/x")
self.desired_position.y = rospy.get_param("/husarion/desired_pose/y")
self.precision = rospy.get_param('/husarion/precision')
self.precision_epsilon = 1.0 / (10.0 * self.precision)
self.move_base_precision = rospy.get_param(
'/husarion/move_base_precision')
# We create the arrays for the laser readings
# We also create the arrays for the odometry readings
# We join them toeguether.
# Here we will add any init functions prior to starting the MyRobotEnv
super(HusarionGetToPosTurtleBotPlayGroundEnv,
self).__init__(ros_ws_abspath)
laser_scan = self._check_laser_scan_ready()
num_laser_readings = int(len(laser_scan.ranges) / self.new_ranges)
high_laser = numpy.full((num_laser_readings), self.max_laser_value)
low_laser = numpy.full((num_laser_readings), self.min_laser_value)
# We place the Maximum and minimum values of the X,Y and YAW of the odometry
# The odometry yaw can be any value in the circunference.
high_odometry = numpy.array(
[self.work_space_x_max, self.work_space_y_max, 3.14])
low_odometry = numpy.array(
[self.work_space_x_min, self.work_space_y_min, -1 * 3.14])
# Now we fetch the max and min of the Desired Position in 2D XY
# We use the exact same as the workspace, just because make no sense
# Consider points outside the workspace
high_des_pos = numpy.array(
[self.work_space_x_max, self.work_space_y_max])
low_des_pos = numpy.array(
[self.work_space_x_min, self.work_space_y_min])
# We join both arrays
high = numpy.concatenate([high_laser, high_odometry, high_des_pos])
low = numpy.concatenate([low_laser, low_odometry, low_des_pos])
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.closer_to_point_reward = rospy.get_param(
"/husarion/closer_to_point_reward")
self.alive_reward = rospy.get_param("/husarion/alive_reward")
self.end_episode_points = rospy.get_param(
"/husarion/end_episode_points")
self.cumulated_steps = 0.0
self.laser_filtered_pub = rospy.Publisher(
'/rosbot/laser/scan_filtered', LaserScan, queue_size=1)
def __init__(self):
"""
Make Hopper Learn how to Stay Up indefenitly
"""
# Only variable needed to be set here
"""
For this version, we consider 6 actions
1-2) Increment/Decrement haa_joint
3-4) Increment/Decrement hfe_joint
5-6) Increment/Decrement kfe_joint
"""
rospy.logdebug("Start HopperStayUpEnv INIT...")
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
# ros_ws_abspath = rospy.get_param("/monoped/ros_ws_abspath", None)
# assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
# assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
# " DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
# "/src;cd " + ros_ws_abspath + ";catkin_make"
ros_ws_abspath = "/home/roboticlab14/catkin_ws/"
ROSLauncher(rospackage_name="legged_robots_sims",
launch_file_name="start_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/hopper/config",
yaml_file_name="hopper_stay_up.yaml")
number_actions = rospy.get_param('/monoped/n_actions')
self.action_space = spaces.Discrete(number_actions)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
# Actions and Observations
self.init_joint_states = Vector3()
self.init_joint_states.x = rospy.get_param(
'/monoped/init_joint_states/haa_joint')
self.init_joint_states.y = rospy.get_param(
'/monoped/init_joint_states/hfe_joint')
self.init_joint_states.z = rospy.get_param(
'/monoped/init_joint_states/kfe_joint')
# Get Desired Point to Get
self.desired_point = Point()
self.desired_point.x = rospy.get_param("/monoped/desired_point/x")
self.desired_point.y = rospy.get_param("/monoped/desired_point/y")
self.desired_point.z = rospy.get_param("/monoped/desired_point/z")
self.accepted_error_in_des_pos = rospy.get_param(
"/monoped/accepted_error_in_des_pos")
self.desired_yaw = rospy.get_param("/monoped/desired_yaw")
self.joint_increment_value = rospy.get_param(
"/monoped/joint_increment_value")
self.init_move_time = rospy.get_param("/monoped/init_move_time", 1.0)
self.move_time = rospy.get_param("/monoped/move_time", 0.05)
self.check_position = rospy.get_param("/monoped/check_position", True)
self.accepted_joint_error = rospy.get_param(
"/monoped/accepted_joint_error")
self.update_rate = rospy.get_param("/monoped/update_rate")
self.dec_obs = rospy.get_param(
"/monoped/number_decimals_precision_obs")
self.desired_force = rospy.get_param("/monoped/desired_force")
self.max_x_pos = rospy.get_param("/monoped/max_x_pos")
self.max_y_pos = rospy.get_param("/monoped/max_y_pos")
self.min_height = rospy.get_param("/monoped/min_height")
self.max_height = rospy.get_param("/monoped/max_height")
self.distance_from_desired_point_max = rospy.get_param(
"/monoped/distance_from_desired_point_max")
self.max_incl_roll = rospy.get_param("/monoped/max_incl")
self.max_incl_pitch = rospy.get_param("/monoped/max_incl")
self.max_contact_force = rospy.get_param("/monoped/max_contact_force")
self.maximum_haa_joint = rospy.get_param("/monoped/maximum_haa_joint")
self.maximum_hfe_joint = rospy.get_param("/monoped/maximum_hfe_joint")
self.maximum_kfe_joint = rospy.get_param("/monoped/maximum_kfe_joint")
self.min_kfe_joint = rospy.get_param("/monoped/min_kfe_joint")
# We place the Maximum and minimum values of observations
self.joint_ranges_array = {
"maximum_haa": self.maximum_haa_joint,
"minimum_haa_joint": -self.maximum_haa_joint,
"maximum_hfe_joint": self.maximum_hfe_joint,
"minimum_hfe_joint": self.maximum_hfe_joint,
"maximum_kfe_joint": self.maximum_kfe_joint,
"min_kfe_joint": self.min_kfe_joint
}
high = numpy.array([
self.distance_from_desired_point_max, self.max_incl_roll,
self.max_incl_pitch, 3.14, self.max_contact_force,
self.maximum_haa_joint, self.maximum_hfe_joint,
self.maximum_kfe_joint, self.max_x_pos, self.max_y_pos,
self.max_height
])
low = numpy.array([
0.0, -1 * self.max_incl_roll, -1 * self.max_incl_pitch, -1 * 3.14,
0.0, -1 * self.maximum_haa_joint, -1 * self.maximum_hfe_joint,
self.min_kfe_joint, -1 * self.max_x_pos, -1 * self.max_y_pos,
self.min_height
])
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.weight_joint_position = rospy.get_param(
"/monoped/rewards_weight/weight_joint_position")
self.weight_contact_force = rospy.get_param(
"/monoped/rewards_weight/weight_contact_force")
self.weight_orientation = rospy.get_param(
"/monoped/rewards_weight/weight_orientation")
self.weight_distance_from_des_point = rospy.get_param(
"/monoped/rewards_weight/weight_distance_from_des_point")
self.alive_reward = rospy.get_param("/monoped/alive_reward")
self.done_reward = rospy.get_param("/monoped/done_reward")
# Here we will add any init functions prior to starting the MyRobotEnv
super(HopperStayUpEnv, self).__init__(ros_ws_abspath)
rospy.logdebug("END HopperStayUpEnv INIT...")
def __init__(self):
"""
This Task Env is designed for 2D Walrus navigation.
It will learn how to get to the goal.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/walrus/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="walrus_gazebo",
launch_file_name="playpen.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/walrus/config",
yaml_file_name="walrus_nav.yaml")
# Here we will add any init functions prior to starting the MyRobotEnv
super(WalrusNavEnv, self).__init__(ros_ws_abspath)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
# Goal parameters
self.x_goal = rospy.get_param('/walrus/x_goal')
self.y_goal = rospy.get_param('/walrus/y_goal')
self.success_radius = rospy.get_param('/walrus/success_radius')
# Action parameters
self.linear_speed_max = rospy.get_param('/walrus/linear_speed_max')
self.linear_speed_min = rospy.get_param('/walrus/linear_speed_min')
self.angular_speed_max = rospy.get_param('/walrus/angular_speed_max')
self.angular_speed_min = rospy.get_param('/walrus/angular_speed_min')
self.init_linear_forward_speed = rospy.get_param(
'/walrus/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/walrus/init_linear_turn_speed')
# Set up action space. Potential action/commanded velocity is any value between linear_speed_min and _max
#number_actions = rospy.get_param('/walrus/n_actions')
#self.action_space = spaces.Discrete(number_actions)
self.action_space = spaces.Box(
numpy.array([self.linear_speed_min, self.angular_speed_min]),
numpy.array([self.linear_speed_max, self.angular_speed_max]))
# Observation parameters
self.new_ranges = rospy.get_param('/walrus/new_ranges')
self.num_scans = rospy.get_param('/walrus/num_scans')
self.min_range = rospy.get_param('/walrus/min_range')
self.max_laser_value = rospy.get_param('/walrus/max_laser_value')
self.min_laser_value = rospy.get_param('/walrus/min_laser_value')
#self.num_imu_obs = rospy.get_param('/walrus/num_imu_obs')
# self.max_pitch_orient = rospy.get_param('/walrus/max_pitch_orient')
# self.min_pitch_orient = rospy.get_param('/walrus/min_pitch_orient')
self.max_yaw_orient = rospy.get_param('/walrus/max_yaw_orient')
self.min_yaw_orient = rospy.get_param('/walrus/min_yaw_orient')
# self.max_pitch_rate = rospy.get_param('/walrus/max_pitch_rate')
# self.min_pitch_rate = rospy.get_param('/walrus/min_pitch_rate')
self.max_x_disp = rospy.get_param('/walrus/max_x_disp')
self.min_x_disp = rospy.get_param('/walrus/min_x_disp')
self.max_y_disp = rospy.get_param('/walrus/max_y_disp')
self.min_y_disp = rospy.get_param('/walrus/min_y_disp')
self.max_linear_acceleration = rospy.get_param(
'/walrus/max_linear_acceleration')
self.max_angular_velocity = rospy.get_param(
'/walrus/max_angular_velocity')
# Set up observation space
# We create two arrays based on the range values that will be assigned
# In the discretization method.
laser_scan_l = self.get_laser_scan_l()
laser_scan_r = self.get_laser_scan_r()
#num_laser_readings = int(len(laser_scan.ranges)/self.new_ranges)
# Define high and low values for the scans
high_scan = numpy.full((self.new_ranges * self.num_scans),
self.max_laser_value)
low_scan = numpy.full((self.new_ranges * self.num_scans),
self.min_laser_value)
# Now, define high and low values for the imu measurements in a numpy array
#high_imu = numpy.array([self.max_pitch_orient, self.max_pitch_rate])
#low_imu = numpy.array([self.min_pitch_orient, self.min_pitch_rate])
high_imu = numpy.array([self.max_yaw_orient])
low_imu = numpy.array([self.min_yaw_orient])
# Now, define high and low values for the odometry measurement in a numpy array
high_disp = numpy.array([self.max_x_disp, self.max_y_disp])
low_disp = numpy.array([self.min_x_disp, self.min_y_disp])
# Define high and low values for all observations, and create the observation space to span
high = numpy.append(high_scan, high_imu)
high = numpy.append(high, high_disp)
low = numpy.append(low_scan, low_imu)
low = numpy.append(low, low_disp)
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Reward/penalty parameters
self.stay_alive_reward = rospy.get_param("/walrus/stay_alive_reward")
self.position_reward = rospy.get_param("/walrus/position_reward")
self.goal_reached_reward = rospy.get_param(
"/walrus/goal_reached_reward")
self.ang_velocity_threshold = rospy.get_param(
"/walrus/ang_velocity_threshold")
self.ang_velocity_reward = rospy.get_param(
"/walrus/ang_velocity_reward")
self.forward_velocity_reward = rospy.get_param(
"/walrus/forward_velocity_reward")
self.cumulated_steps = 0.0
def __init__(self, ros_ws_abspath):
"""
Initializes a new TurtleBot2Env environment.
Turtlebot2 doesnt use controller_manager, therefore we wont reset the
controllers in the standard fashion. For the moment we wont reset them.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /odom : Odometry readings of the Base of the Robot
* /cmd_human : Joystick readings
* /camera/depth/image_raw: 2d Depth image of the depth sensor.
* /camera/depth/points: Pointcloud sensor readings
* /camera/rgb/image_raw: RGB camera
* /kobuki/laser/scan: Laser Readings
Actuators Topic List: /cmd_vel,
Args:
"""
rospy.logdebug("Start TurtleBot2Env INIT...")
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the robot into the world
ROSLauncher(rospackage_name="turtlebot_gazebo",
launch_file_name="put_turtlebot2_in_world.launch",
ros_ws_abspath=ros_ws_abspath)
################################################### joy #########################33
ROSLauncher(rospackage_name="turtlebot_gazebo",
launch_file_name="turtlebot_joy.launch",
ros_ws_abspath=ros_ws_abspath)
###############################################################
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(TurtleBot2Env,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
self._check_all_sensors_ready()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber("/odom", Odometry, self._odom_callback)
rospy.Subscriber(
"/cmd_human", Twist,
self._joy_callback) #############################joy##########
#rospy.Subscriber("/camera/depth/image_raw", Image, self._camera_depth_image_raw_callback)
#rospy.Subscriber("/camera/depth/points", PointCloud2, self._camera_depth_points_callback)
#rospy.Subscriber("/camera/rgb/image_raw", Image, self._camera_rgb_image_raw_callback)
rospy.Subscriber("/kobuki/laser/scan", LaserScan,
self._laser_scan_callback)
self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.share_talker = rospy.Publisher('/share', Int64, queue_size=1)
self._check_publishers_connection()
self.gazebo.pauseSim()
rospy.logdebug("Finished TurtleBot2Env INIT...")
def __init__(self):
"""
This Task Env is designed for having the TurtleBot2 in some kind of maze.
It will learn how to move around the maze without crashing.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
# This parameter HAS to be set up in the MAIN launch of the AI RL script
ros_ws_abspath = rospy.get_param("/turtlebot2/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path "+ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p "+ros_ws_abspath + \
"/src;cd "+ros_ws_abspath+";catkin_make"
ROSLauncher(rospackage_name="turtlebot_gazebo",
launch_file_name="start_world_maze_loop_brick.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/turtlebot2/config",
yaml_file_name="turtlebot2_maze.yaml")
# Here we will add any init functions prior to starting the MyRobotEnv
super(TurtleBot2MazeEnv, self).__init__(ros_ws_abspath)
# Only variable needed to be set here
number_actions = rospy.get_param('/turtlebot2/n_actions')
self.action_space = spaces.Discrete(number_actions)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
#number_observations = rospy.get_param('/turtlebot2/n_observations')
"""
We set the Observation space for the 6 observations
cube_observations = [
round(current_disk_roll_vel, 0),
round(y_distance, 1),
round(roll, 1),
round(pitch, 1),
round(y_linear_speed,1),
round(yaw, 1),
]
"""
# Actions and Observations
self.dec_obs = rospy.get_param(
"/turtlebot2/number_decimals_precision_obs", 1)
self.linear_forward_speed = rospy.get_param(
'/turtlebot2/linear_forward_speed')
self.linear_turn_speed = rospy.get_param(
'/turtlebot2/linear_turn_speed')
self.angular_speed = rospy.get_param('/turtlebot2/angular_speed')
self.init_linear_forward_speed = rospy.get_param(
'/turtlebot2/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/turtlebot2/init_linear_turn_speed')
self.n_observations = rospy.get_param('/turtlebot2/n_observations')
self.min_range = rospy.get_param('/turtlebot2/min_range')
self.max_laser_value = rospy.get_param('/turtlebot2/max_laser_value')
self.min_laser_value = rospy.get_param('/turtlebot2/min_laser_value')
# We create two arrays based on the binary values that will be assigned
# In the discretization method.
#laser_scan = self._check_laser_scan_ready()
laser_scan = self.get_laser_scan()
rospy.logdebug("laser_scan len===>" + str(len(laser_scan.ranges)))
# Laser data
self.laser_scan_frame = laser_scan.header.frame_id
# Number of laser reading jumped
self.new_ranges = int(
math.ceil(
float(len(laser_scan.ranges)) / float(self.n_observations)))
rospy.logdebug("n_observations===>" + str(self.n_observations))
rospy.logdebug("new_ranges, jumping laser readings===>" +
str(self.new_ranges))
high = numpy.full((self.n_observations), self.max_laser_value)
low = numpy.full((self.n_observations), self.min_laser_value)
# We only use two integers
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.forwards_reward = rospy.get_param("/turtlebot2/forwards_reward")
self.turn_reward = rospy.get_param("/turtlebot2/turn_reward")
self.end_episode_points = rospy.get_param(
"/turtlebot2/end_episode_points")
self.cumulated_steps = 0.0
self.laser_filtered_pub = rospy.Publisher(
'/turtlebot2/laser/scan_filtered', LaserScan, queue_size=1)
self.model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
