[ROS Projects] OpenAI with Moving Cube Robot in Gazebo Step-by-Step Part2

OpenAI-with-Moving-Cube-Robot-in-Gazebo-Step-by-Step-Part2

Written by Miguel Angel

25/06/2018

 

In this new ROS Project you are going to learn Step-by-Step how to create a moving cube and that it learns to move using OpenAI environment.

This second video is for learning the creation the basics of Reinforcement learning and how to connect to the various systems of the robot to get the state, perform actions and calculate rewards.

ROS Development Studio
OpenAi Course

Moving Cube Git:https://bitbucket.org/theconstructcore/moving_cube/src/master/
Moving Cube Training AI Git: https://bitbucket.org/theconstructcore/moving_cube_training/src/master/

If you didn’t follow up, please check the link below for the last post.

[irp posts=”9744″ name=”[ROS Projects] OpenAI with Moving Cube Robot in Gazebo Step-by-Step Part1″]

 

Step 1. Clone the simulation

In order to make sure we have the same project. Please run the following command

cd ~/simulation_ws/src
git clone https://bitbucket.org/theconstructcore/moving_cube.git

NOTICE: please delete the previous code if you have problems to compile the code

In the /moving_cube/moving_cube_description/urdf/moving_cube.urdf file, please uncomment the following part. We’ll need this part to publish the odom topic.

    <gazebo>
        <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
            <robotNamespace>/moving_cube</robotNamespace>
            <alwaysOn>true</alwaysOn>
            <updateRate>50.0</updateRate>
            <bodyName>cube_body</bodyName>
            <topicName>odom</topicName>
            <gaussianNoise>0.01</gaussianNoise>
            <frameName>world</frameName>
            <xyzOffsets>0 0 0</xyzOffsets>
            <rpyOffsets>0 0 0</rpyOffsets>
        </plugin>
    </gazebo>

Step 2. Create a training package

To train the robot, let’s create a package for training under the catkin_ws/src

cd ~/catkin_ws/src
catkin_create_pkg my_moving_cube_traning_pkg rospy

Then we’ll create a script folder inside the package and put a file called cube_rl_utils.py inside it with the following content

#!/usr/bin/env python

import time
import rospy
import math
import copy
import numpy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point
from tf.transformations import euler_from_quaternion

class CubeRLUtils(object):

    def __init__(self):

        self.check_all_sensors_ready()
        
        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()
        
    
    def check_all_sensors_ready(self):
        self.disk_joints_data = None
        while self.disk_joints_data is None and not rospy.is_shutdown():
            try:
                self.disk_joints_data = rospy.wait_for_message("/moving_cube/joint_states", JointState, timeout=1.0)
                rospy.loginfo("Current moving_cube/joint_states READY=>"+str(self.disk_joints_data))

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

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

            except:
                rospy.logerr("Current /moving_cube/odom not ready yet, retrying for getting odom")
        rospy.loginfo("ALL SENSORS READY")
        
    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.loginfo("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.loginfo("_base_pub Publisher Connected")

        rospy.loginfo("All Publishers READY")
        
    def joints_callback(self, data):
        self.joints = data

    def odom_callback(self, data):
        self.odom = data

    
    # Reinforcement Learning Utility Code
    def move_joints(self, roll_speed):
        
        joint_speed_value = Float64()
        joint_speed_value.data = roll_speed
        rospy.loginfo("Single Disk Roll Velocity>>"+str(joint_speed_value))
        self._roll_vel_pub.publish(joint_speed_value)
        
    def get_cube_state(self):
        
        # We convert from quaternions to euler
        orientation_list = [self.odom.pose.pose.orientation.x,
                            self.odom.pose.pose.orientation.y,
                            self.odom.pose.pose.orientation.z,
                            self.odom.pose.pose.orientation.w]
                            
        roll, pitch, yaw = euler_from_quaternion(orientation_list)
        
        # We get the distance from the origin
        start_position = Point()
        start_position.x = 0.0
        start_position.y = 0.0
        start_position.z = 0.0
        
        distance = self.get_distance_from_point(start_position,
                                                self.odom.pose.pose.position)
        
        cube_state = [
                        round(self.joints.velocity[0],1),
                        round(distance,1),
                        round(roll,1),
                        round(pitch,1),
                        round(yaw,1)
                    ]
        
        return cube_state

    
    def observation_checks(self, cube_state):
        
        
        # MAximum distance to travel permited in meters from origin
        max_distance=2.0
        
        if (cube_state[1] > max_distance): 
            rospy.logerr("Cube Too Far==>"+str(cube_state[1]))
            done = True
        else:
            rospy.loginfo("Cube NOT Too Far==>"+str(cube_state[1]))
            done = False
        
        return done

    def get_distance_from_point(self, pstart, p_end):
        """
        Given a Vector3 Object, get distance from current position
        :param p_end:
        :return:
        """
        a = numpy.array((pstart.x, pstart.y, pstart.z))
        b = numpy.array((p_end.x, p_end.y, p_end.z))

        distance = numpy.linalg.norm(a - b)

        return distance

    def get_reward_for_observations(self, state):

        # We reward it for lower speeds and distance traveled
        
        speed = state[0]
        distance = state[1]

        # Positive Reinforcement
        reward_distance = distance * 10.0
        # Negative Reinforcement for magnitude of speed
        reward_for_efective_movement = -1 * abs(speed) 
        
        reward = reward_distance + reward_for_efective_movement

        rospy.loginfo("Reward_distance="+str(reward_distance))
        rospy.loginfo("Reward_for_efective_movement= "+str(reward_for_efective_movement))
        
        return reward



def cube_rl_systems_test():
    
    rospy.init_node('cube_rl_systems_test_node', anonymous=True, log_level=rospy.INFO)
    cube_rl_utils_object = CubeRLUtils()
    
    rospy.loginfo("Moving to Speed==>80")
    cube_rl_utils_object.move_joints(roll_speed=80.0)
    time.sleep(2)
    rospy.loginfo("Moving to Speed==>-80")
    cube_rl_utils_object.move_joints(roll_speed=-80.0)
    time.sleep(2)
    rospy.loginfo("Moving to Speed==>0.0")
    cube_rl_utils_object.move_joints(roll_speed=0.0)
    time.sleep(2)
    
    cube_state = cube_rl_utils_object.get_cube_state()
    done = cube_rl_utils_object.observation_checks(cube_state)
    reward = cube_rl_utils_object.get_reward_for_observations(cube_state)
    
    rospy.loginfo("Done==>"+str(done))
    rospy.loginfo("Reward==>"+str(reward))
    

if __name__ == "__main__":
    cube_rl_systems_test()

In this post, we’ll focus on the cube_rl_systems_test() function. The function uses the class to move the cube, get the observation, calculate reward and check if it’s done. To run it, you have to run the simulation first. Please go to Simulations->Select launch file-> main.launch

NOTICE: You have to unpause the simulation by clicking the arrow key in the simulation window

Then you can run the following command to run the script

cd ~/catkin_ws/src/my_moving_cube_training_pkg/script
chmod +x cube_rl_utils.py
cd ~/catkin_ws
source devel_setup.bash
rosrun my_moving_cube_training_pkg cube_rl_utils.py

You should see the cube moving around and the reward and the done state is calculated.

 

 

Edit by: Tony Huang

[irp posts=”9976″ name=”ROS Projects OpenAI with Moving Cube Robot in Gazebo Step-by-Step Part3″]

Masterclass 2023 batch2 blog banner

Check Out These Related Posts

129. ros2ai

129. ros2ai

I would like to dedicate this episode to all the ROS Developers who believe that ChatGPT or...

read more

0 Comments

Trackbacks/Pingbacks

  1. [ROS Projects] OpenAI with Moving Cube Robot in Gazebo Step-by-Step Part3 | The Construct - […] https://www.theconstruct.ai/ros-projects-openai-moving-cube-robot-gazebo-step-step-part2/ […]
  2. [ROS Projects] OpenAI with Moving Cube Robot in Gazebo Step-by-Step #Part1 - […] Part 2: Basics of Reinforcement learning and Connect to the various systems of the robot […]

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This