【openai_ros】4 - Exploring the OpenAI Structure: RoboCube. Part 2

Unit 3: How to create a Robot Environment for a new Robot

SUMMARY

Estimated time to completion: 2 hours

In this unit, you will learn how to create a Robot Environment for a Moving Cube with a single disk in the roll axis using the OpenAI ROS structure. We will call it MyCubeSingleDiskEnv.

END OF SUMMARY

So, you now understand how the CartPole Environment structure works. Now, we need to create one from scratch for the One-Disk-Moving-Cube. There are three main steps:

• We need to create a Robot Environment that has the basic functions needed to use the RoboCube.
• We need to decide how we move the RoboCube and how to get the sensor data.
• We need to create functions that allow environments that inherit from our class to retrieve sensor data and access the cube’s functionality, without knowing all the ROS related stuff.

With this created, you will have a Robot Environment that can be used by any Task Environment that you create afterwards.

Step 0- Create a package to place all your code in

We will first create a package that will hold all the code that you generate in the next chapters.

Execute in WebShell #1

roscd; cd ../src; 

catkin_create_pkg my_moving_cube_pkg rospy openai_ros

cd my_moving_cube_pkg/

mkdir scripts; cd scripts

Let’s also create a Python file that will contain our code for the Robot Environment:

touch my_cube_single_disk_env.py; chmod +x my_cube_single_disk_env.py

Step 1- Create a basic Robot Environment

To create our Robot Environment, we will start from a Robot Environment template (see the code below). You can also find this template inside the openai_ros package under the openai_ros/templates directory.

template_my_robot_env.py

from openai_ros import robot_gazebo_env


class MyRobotEnv(robot_gazebo_env.RobotGazeboEnv):
    """Superclass for all Robot environments.
    """

    def __init__(self):
        """Initializes a new Robot environment.
        """
        # Variables that we give through the constructor.

        # Internal Vars
        self.controllers_list = ['my_robot_controller1','my_robot_controller2', ..., 'my_robot_controllerX']

        self.robot_name_space = "my_robot_namespace"

        reset_controls_bool = True or False
        
        # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
        
        super(MyRobotEnv, self).__init__(controllers_list=self.controllers_list,
                                                robot_name_space=self.robot_name_space,
                                                reset_controls=reset_controls_bool)

    # Methods needed by the RobotGazeboEnv
    # ----------------------------
    
    

    def _check_all_systems_ready(self):
        """
        Checks that all the sensors, publishers and other simulation systems are
        operational.
        """
        # TODO
        return True
    
    # Methods that the TaskEnvironment will need to define here as virtual
    # because they will be used in RobotGazeboEnv GrandParentClass and defined in the
    # TaskEnvironment.
    # ----------------------------
    def _set_init_pose(self):
        """Sets the Robot in its init pose
        """
        raise NotImplementedError()
    
    
    def _init_env_variables(self):
        """Inits variables needed to be initialised each time we reset at the start
        of an episode.
        """
        raise NotImplementedError()

    def _compute_reward(self, observations, done):
        """Calculates the reward to give based on the observations given.
        """
        raise NotImplementedError()

    def _set_action(self, action):
        """Applies the given action to the simulation.
        """
        raise NotImplementedError()

    def _get_obs(self):
        raise NotImplementedError()

    def _is_done(self, observations):
        """Checks if episode done based on observations given.
        """
        raise NotImplementedError()
        
    # Methods that the TaskEnvironment will need.
    # ----------------------------

You can see here that the template is divided into four parts:

1. Initialization of the Robot Environment class (in the example above, MyRobotEnv). It inherits from the RobotGazeboEnv.
2. Definition of the virtual methods needed by the Gazebo Environment, which were declared virtually inside RobotGazeboEnv.
3. Virtual definition of methods that the Task Environment will need to define here as virtual because they will be used in the RobotGazeboEnv GrandParentClass and defined in the Task Environment.
4. Definition of methods that the Task Environment will need to use from the class.

Let’s go step by step with our own example.

Step 2- Initialization of the class MyRobotEnv

Open the my_cube_single_disk_env.py file on the IDE and copy the following text inside:

#! /usr/bin/env python

from openai_ros import robot_gazebo_env

class MyCubeSingleDiskEnv(robot_gazebo_env.RobotGazeboEnv):
    """Superclass for all CubeSingleDisk environments.
    """

    def __init__(self, init_roll_vel):
        """Initializes a new CubeSingleDisk environment.
        """
        # Variables that we give through the constructor.
        self.init_roll_vel = init_roll_vel

        self.controllers_list = ['my_robot_controller1','my_robot_controller2', ..., 'my_robot_controllerX']

        self.robot_name_space = "my_robot_namespace"

        reset_controls_bool = True or False
        
        # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
        super(MyCubeSingleDiskEnv, self).__init__(controllers_list=self.controllers_list,
                                                robot_name_space=self.robot_name_space,
                                                reset_controls=reset_controls_bool)

In the previous code, we are creating the class for the Cubli Robot Environment. We are calling that class: MyCubeSingleDiskEnv.

As you can see, we just took the first lines of the template above and modified a few things to accomodate for the Cubli robot. Let’s review them:

Here we import the robot_gazebo_env from the python module folder openai_ros. Inside robot_gazebo_env, we find the class RobotGazeboEnv. In the class definition of MyCubeSingleDiskEnv, we give inheritance to robot_gazebo_env.RobotGazeboEnv. That means to look for the python module file robot_gazebo_env, and inside, get the class RobotGazeboEnv.

from openai_ros import robot_gazebo_env

class MyCubeSingleDiskEnv(robot_gazebo_env.RobotGazeboEnv):

Then, we add a variable in the init function. These variables are the ones you want the Task Environment to pass to this new Robot Environment. We need it to pass the speed at the start of each episode to set the roll disk. For this scenario, it will most likely always be 0.0, but it could change depending on the Task Environment.

How to decide which variables will be required?.. well it depends on how good you know the robot and what will be required from it.

def __init__(self, init_roll_vel):
    # Variables that we give through the constructor.
    self.init_roll_vel = init_roll_vel

Now we define some variables that need to be passed to the RobotGazeboEnv super constructor method init. These are: controllers_list, robot_name_space, and reset_controls. These variables are used by the RobotGazeboEnv to know which controllers to reset each time a learning episode starts. THOSE VARIABLES ARE ALWAYS MANDATORY, for any openai_ros problem you want to solve.

In order to know the list of controllers for a given robot you have two different options:

1. Check the launch file of the simulation and figure out the controllers that are loaded.
2. We can get a list of the controllers available in a simulation by calling the service that provides the list of controllers.

Let’s use the last option to the Cubli simulation.

Execute in WebShell #1

First be sure that the simulation is unpaused, otherwise nothing will apear in the rosservice call to list controllers.

rosservice call /gazebo/unpause_physics “{}”

rosservice call /moving_cube/controller_manager/list_controllers "{}"

WebShell #1 Output

controller:
  -
    name: "joint_state_controller"
    state: "running"
    type: "joint_state_controller/JointStateController"
    claimed_resources:
      -
        hardware_interface: "hardware_interface::JointStateInterface"
        resources: []
  -
    name: "inertia_wheel_roll_joint_velocity_controller"
    state: "running"
    type: "effort_controllers/JointVelocityController"
    claimed_resources:
      -
        hardware_interface: "hardware_interface::EffortJointInterface"
        resources: [inertia_wheel_roll_joint]

So, based on the output provided by the command, we can identify two different controllers being loaded:

• joint_state_controller
• inertia_wheel_roll_joint_velocity_controller

Hence, the list of controllers in the init function should look something like this:

self.controllers_list = ['joint_state_controller','inertia_wheel_roll_joint_velocity_controller']

Next, to get the namespace used in the controllers, just execute the following command to see all the controllers’ namespace:

Execute in WebShell #1

rostopic list | grep controller

WebShell #1 Output

/moving_cube/inertia_wheel_roll_joint_velocity_controller/command
/moving_cube/inertia_wheel_roll_joint_velocity_controller/pid/parameter_descriptions
/moving_cube/inertia_wheel_roll_joint_velocity_controller/pid/parameter_updates
/moving_cube/inertia_wheel_roll_joint_velocity_controller/state

So, as a robot_name_space you need to indicate all the elements that appear before the name of the controllers. In our case, we got the following topic:

/moving_cube/inertia_wheel_roll_joint_velocity_controller/command

And we got the name of the controller: inertia_wheel_roll_joint_velocity_controller

Hence, what is before the controller name is:

/moving_cube

So the robot_name_space must be:

self.robot_name_space = "moving_cube"

Next step, we decide if we want to reset the controllers or not when a new episode starts. We recommend in general to do it because otherwise Gazebo can have strange behaviors:

reset_controls_bool = True

And finally, you pass it to the creation init function:

super(MyCubeSingleDiskEnv, self).__init__(controllers_list=self.controllers_list,
                                                robot_name_space=self.robot_name_space,
                                                reset_controls=reset_controls_bool)

Step 3- Definition of the Virtual Methods needed by the RobotGazeboEnv

As we indicated in the previous unit, the RobotGazeboEnv has a virtual function that needs to be defined by the Robot Environment, because it has access to the robot ROS topics, in order to get the information from it.

We need to define the following function:

• _check_all_systems_ready ()

_check_all_systems_ready() definition

The first function we need to define is the _check_all_systems_ready. This function checks that all the sensors, publishers and other simulation systems are operational. This function will be called by the Gazebo Environment during the reseting of the simulation. Since sometimes the simulations have strange behaviors, we need to ensure with this function that everything is running before we continue with the next episode.

def _check_all_systems_ready(self):
    """
    Checks that all the sensors, publishers and other simulation systems are
    operational.
    """
    # TODO
    return True

But before defining the _check_all_systems_ready, we have to return to the init method and create all the subscribers and publishers for the MyCubeSingleDiskEnv so that the check_all_systems_ready can work.

Modify the init function of your class with the following code:

    def __init__(self, init_roll_vel):
        """Initializes a new CubeSingleDisk environment.
        """
        # Variables that we give through the constructor.
        self.init_roll_vel = init_roll_vel

        self.controllers_list = ['joint_state_controller',
                                 'inertia_wheel_roll_joint_velocity_controller'
                                 ]

        self.robot_name_space = "moving_cube"

        reset_controls_bool = True
        
        # We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
        super(MyCubeSingleDiskEnv, self).__init__(controllers_list=self.controllers_list,
                                                robot_name_space=self.robot_name_space,
                                                reset_controls=reset_controls_bool)


        """
        To check any topic we need to have the simulations running, we need to do two things:
        1) Unpause the simulation: without that the stream of data doesn't flow. This is for simulations
        that are pause for whatever reason
        2) If the simulation was running already for some reason, we need to reset the controllers.
        This has to do with the fact that some plugins with tf don't understand the reset of the simulation
        and need to be reset 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()

On the previous code, we create the Subscribers to the joint states and odometry of the robot. We also create a Publisher that will allow us to publish a command to the joint.

However, the important part is the unpauseSim(), _check_all_sensors_ready(), and unpauseSim() calls. These are key to being able to reset the controllers and read the sensors. We use the objects created in the RobotGazeboEnv parent class so that we have access to it without having to know how it works.

self.gazebo.unpauseSim()
self.controllers_object.reset_controllers()
self._check_all_sensors_ready()

On the previous code, first we unpause the simulation (required to reset it). Then, we reset the controllers and make the first test to see if the sensors are working. Checking all this is key for AI learning because we need a reliable sensor and controller communication.

Note that inside this MyCubeSingleDiskEnv we use _check_all_sensors_ready which is an internal function, while the RobotGazeboEnv parent class will call the _check_all_systems_ready. We could also just use one function, but it is separeted here to show the diference in who uses which function.

We have to define the methods inside this class. Hence, add the following code to your MyCubeSingleDiskEnv class, as members of the class (check the template above if you don’t know where to put this code).

    def _check_all_systems_ready(self):
        """
        Checks that all the sensors, publishers and other simulation systems are
        operational.
        """
        self._check_all_sensors_ready()
        self._check_publishers_connection()
        return True


    # CubeSingleDiskEnv virtual methods
    # ----------------------------

    def _check_all_sensors_ready(self):
        self._check_joint_states_ready()
        self._check_odom_ready()
        rospy.logdebug("ALL SENSORS READY")

    def _check_joint_states_ready(self):
        self.joints = None
        while self.joints is None and not rospy.is_shutdown():
            try:
                self.joints = rospy.wait_for_message("/moving_cube/joint_states", JointState, timeout=1.0)
                rospy.logdebug("Current moving_cube/joint_states READY=>" + str(self.joints))

            except:
                rospy.logerr("Current moving_cube/joint_states not ready yet, retrying for getting joint_states")
        return self.joints

    def _check_odom_ready(self):
        self.odom = None
        while self.odom is None and not rospy.is_shutdown():
            try:
                self.odom = rospy.wait_for_message("/moving_cube/odom", Odometry, timeout=1.0)
                rospy.logdebug("Current /moving_cube/odom READY=>" + str(self.odom))

            except:
                rospy.logerr("Current /moving_cube/odom not ready yet, retrying for getting odom")

        return self.odom

In the case of the OneDiskCube, for sensors, we only have the odometry that tells us where the Cube Body is in the simulated world (/moving_cube/odom), and how the disk joint is (speed, position, efforts) through the /moving_cube/joint_states.

Once we have this, we know that ROS can establish a connection to these topics and they’re ready, so we can declare the susbcribers now. So, let’s go back to the init method, we had defined the subscribers as follows:

rospy.Subscriber("/moving_cube/joint_states", JointState, self._joints_callback)
rospy.Subscriber("/moving_cube/odom", Odometry, self._odom_callback)

Then, in order for those subscribers to work, we need to add the necessary imports that define the messages of the topics. Add the following lines at the beginning of your Python file:

import rospy
from sensor_msgs.msg import JointState
from nav_msgs.msg import Odometry

Now, we also have to declare the topics callbacks, which will start to store the sensor data in self.joints and self.odom elements of the class. Those callbacks allow our Robot Environment class to have the most updated sensor data for the learning algorithms, even when we pause the simulation.

Add the following code to the Python file as functions members of the class:

    def _joints_callback(self, data):
        self.joints = data
    
    def _odom_callback(self, data):
        self.odom = data

For the publisher we have added the line:

self._roll_vel_pub = rospy.Publisher('/moving_cube/inertia_wheel_roll_joint_velocity_controller/command',
                                             Float64, queue_size=1)

This also requires that we add the necessary imports at the beginning of the Python file. Add the following line:

from std_msgs.msg import Float64

And now we define the _check_publishers_connection, to check that our publisher is ready to receive the speed commands and doesn’t lose any messages:

    def _check_publishers_connection(self):
        """
        Checks that all the publishers are working
        :return:
        """
        rate = rospy.Rate(10)  # 10hz
        while self._roll_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown():
            rospy.logdebug("No susbribers to _roll_vel_pub yet so we wait and try again")
            try:
                rate.sleep()
            except rospy.ROSInterruptException:
                # This is to avoid error when world is rested, time when backwards.
                pass
        rospy.logdebug("_roll_vel_pub Publisher Connected")

        rospy.logdebug("All Publishers READY")

Step 4- Definition of functions that will be called by the TaskEnv to sense/actuate the robot

And now we have to define a function that will move the disks, publishing in our self._roll_vel_pub publisher.

Let’s call this function move_joints(). Why? Because… yes! Seriously, you can provide to this function whatever the name you want. But. You must remember its name because you will need it in the Task Environment in order to send a command to the robot.

Also, you must define which parameters your function is going to need. You can decide that here by deciding which type of actuation are you going to allow the RL algorithms to take. In our case, we allow the RL to provide a rolling speed (of the internal motor of the robot).

Just for clarity, we have divided the move_joints() function into two parts, one of the parts being implemented by the additional function wait_until_roll_is_in_vel().

FINAL NOTE: The move_joints() method will be used internally in MyCubeSingleDiskEnv and also in the Task Environment.

Add the following code to your Robot Environment Python file. The functions are too members of the class:

    # Methods that the TaskEnvironment will need.
    # ----------------------------
    
    def move_joints(self, roll_speed):
        joint_speed_value = Float64()
        joint_speed_value.data = roll_speed
        rospy.logdebug("Single Disk Roll Velocity>>" + str(joint_speed_value))
        self._roll_vel_pub.publish(joint_speed_value)
        self.wait_until_roll_is_in_vel(joint_speed_value.data)
    
    def wait_until_roll_is_in_vel(self, velocity):
    
        rate = rospy.Rate(10)
        start_wait_time = rospy.get_rostime().to_sec()
        end_wait_time = 0.0
        epsilon = 0.1
        v_plus = velocity + epsilon
        v_minus = velocity - epsilon
        while not rospy.is_shutdown():
            joint_data = self._check_joint_states_ready()
            roll_vel = joint_data.velocity[0]
            rospy.logdebug("VEL=" + str(roll_vel) + ", ?RANGE=[" + str(v_minus) + ","+str(v_plus)+"]")
            are_close = (roll_vel <= v_plus) and (roll_vel > v_minus)
            if are_close:
                rospy.logdebug("Reached Velocity!")
                end_wait_time = rospy.get_rostime().to_sec()
                break
            rospy.logdebug("Not there yet, keep waiting...")
            rate.sleep()
        delta_time = end_wait_time- start_wait_time
        rospy.logdebug("[Wait Time=" + str(delta_time)+"]")
        return delta_time

It essentially gets a given roll speed and publishes that speed through the ROS publisher self._roll_vel_pub. The last part is also vital because it guarantees that all the actions are executed and aren’t overrun by the next one. This is the method wait_until_roll_is_in_vel. This method will wait until the roll disk wheel reaches the desired speed, with a certain error.

In the same line we have defined the move_joints() function to actuate the robot, we must provide a function (or series of them) to allow the Task Environment to get the sensor values and compose an observation for the RL algorithm.

For this case, we decided to provide two functions:

• get_joints(): returns the status of the joints of the robot
• get_odom(): returns the current odometry.

For the sake of simplicity and robustness, the functions are going to provide the raw message obtained from the topics, without any specific processing of the sensed data. Hence, the Task Environment, when it calls those functions, it will have to handle how to extract from the message the data it requires for the training.

Add the following two functions to the class:

    def get_joints(self):
        return self.joints

    def get_odom(self):
        return self.odom

Finally, add the next import at the top of the Python file:

import numpy

Step 5- Virtual definition of methods that the Task Environment will need to define

These methods are basically virtual methods that will be called by the RobotGazeboEnv and by the MyCubeSingleDiskEnv. However, those methods have to be implemented upstream in the TaskEnvironment. This is required because these methods relate to how the Task is to be learned.

Summarizing, even if we define the methods here, they have to be implemented at the task level.

You can add the following code to your class, exactly as it is below.

    # Methods that the TaskEnvironment will need to define here as virtual
    # because they will be used in RobotGazeboEnv GrandParentClass and defined in the
    # TaskEnvironment.
    # ----------------------------
    def _set_init_pose(self):
        """Sets the Robot in its init pose
        """
        raise NotImplementedError()
    
    def _init_env_variables(self):
        """Inits variables needed to be initialised each time we reset at the start
        of an episode.
        """
        raise NotImplementedError()

    def _compute_reward(self, observations, done):
        """Calculates the reward to give based on the observations given.
        """
        raise NotImplementedError()

    def _set_action(self, action):
        """Applies the given action to the simulation.
        """
        raise NotImplementedError()

    def _get_obs(self):
        raise NotImplementedError()

    def _is_done(self, observations):
        """Checks if episode done based on observations given.
        """
        raise NotImplementedError()

Step 6- Final Result

If you have followed the instructions below, at the end, you should have the following Python code:

my_cube_single_disk_env.py

#! /usr/bin/env python

import numpy
import rospy
from openai_ros import robot_gazebo_env
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from nav_msgs.msg import Odometry



class MyCubeSingleDiskEnv(robot_gazebo_env.RobotGazeboEnv):
    """Superclass for all CubeSingleDisk environments.
    """

    def __init__(self):
        """Initializes a new CubeSingleDisk environment.

        Args:
        """
        # 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(MyCubeSingleDiskEnv, 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()

    # Methods needed by the RobotGazeboEnv
    # ----------------------------
    

    def _check_all_systems_ready(self):
        """
        Checks that all the sensors, publishers and other simulation systems are
        operational.
        """
        self._check_all_sensors_ready()
        return True


    # CubeSingleDiskEnv virtual methods
    # ----------------------------

    def _check_all_sensors_ready(self):
        self._check_joint_states_ready()
        self._check_odom_ready()
        rospy.logdebug("ALL SENSORS READY")

    def _check_joint_states_ready(self):
        self.joints = None
        while self.joints is None and not rospy.is_shutdown():
            try:
                self.joints = rospy.wait_for_message("/moving_cube/joint_states", JointState, timeout=1.0)
                rospy.logdebug("Current moving_cube/joint_states READY=>" + str(self.joints))

            except:
                rospy.logerr("Current moving_cube/joint_states not ready yet, retrying for getting joint_states")
        return self.joints

    def _check_odom_ready(self):
        self.odom = None
        while self.odom is None and not rospy.is_shutdown():
            try:
                self.odom = rospy.wait_for_message("/moving_cube/odom", Odometry, timeout=1.0)
                rospy.logdebug("Current /moving_cube/odom READY=>" + str(self.odom))

            except:
                rospy.logerr("Current /moving_cube/odom not ready yet, retrying for getting odom")

        return self.odom
        
    def _joints_callback(self, data):
        self.joints = data
    
    def _odom_callback(self, data):
        self.odom = data
        
    def _check_publishers_connection(self):
        """
        Checks that all the publishers are working
        :return:
        """
        rate = rospy.Rate(10)  # 10hz
        while self._roll_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown():
            rospy.logdebug("No susbribers to _roll_vel_pub yet so we wait and try again")
            try:
                rate.sleep()
            except rospy.ROSInterruptException:
                # This is to avoid error when world is rested, time when backwards.
                pass
        rospy.logdebug("_roll_vel_pub Publisher Connected")

        rospy.logdebug("All Publishers READY")
    
    # Methods that the TaskEnvironment will need to define here as virtual
    # because they will be used in RobotGazeboEnv GrandParentClass and defined in the
    # TaskEnvironment.
    # ----------------------------
    def _set_init_pose(self):
        """Sets the Robot in its init pose
        """
        raise NotImplementedError()
    
    def _init_env_variables(self):
        """Inits variables needed to be initialised each time we reset at the start
        of an episode.
        """
        raise NotImplementedError()

    def _compute_reward(self, observations, done):
        """Calculates the reward to give based on the observations given.
        """
        raise NotImplementedError()

    def _set_action(self, action):
        """Applies the given action to the simulation.
        """
        raise NotImplementedError()

    def _get_obs(self):
        raise NotImplementedError()

    def _is_done(self, observations):
        """Checks if episode done based on observations given.
        """
        raise NotImplementedError()
        
    # Methods that the TaskEnvironment will need.
    # ----------------------------
    def move_joints(self, roll_speed):
        joint_speed_value = Float64()
        joint_speed_value.data = roll_speed
        rospy.logdebug("Single Disk Roll Velocity>>" + str(joint_speed_value))
        self._roll_vel_pub.publish(joint_speed_value)
        self.wait_until_roll_is_in_vel(joint_speed_value.data)
    
    def wait_until_roll_is_in_vel(self, velocity):
    
        rate = rospy.Rate(10)
        start_wait_time = rospy.get_rostime().to_sec()
        end_wait_time = 0.0
        epsilon = 0.1
        v_plus = velocity + epsilon
        v_minus = velocity - epsilon
        while not rospy.is_shutdown():
            joint_data = self._check_joint_states_ready()
            roll_vel = joint_data.velocity[0]
            rospy.logdebug("VEL=" + str(roll_vel) + ", ?RANGE=[" + str(v_minus) + ","+str(v_plus)+"]")
            are_close = (roll_vel <= v_plus) and (roll_vel > v_minus)
            if are_close:
                rospy.logdebug("Reached Velocity!")
                end_wait_time = rospy.get_rostime().to_sec()
                break
            rospy.logdebug("Not there yet, keep waiting...")
            rate.sleep()
        delta_time = end_wait_time- start_wait_time
        rospy.logdebug("[Wait Time=" + str(delta_time)+"]")
        return delta_time
        

    def get_joints(self):
        return self.joints
    
    def get_odom(self):
        return self.odom

END my_cube_single_disk_env.py

NEXT STEP: How to create the One Disk Moving Cube Learning env for walking on the Y-axis.