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)