Starting Robot Server with Custom Gym Env

[akeaveny]

Hi @matteolucchi,

I've written a ROS bridge for our robot but running into an issue with connecting to the Robot Server.

I've verified that my ROS packages launches ok with rviz using roslaunch uwrt_rl_robot_server uwrt_rl_sim_robot_server.launch . Here is some of my ouput:

[ INFO] [1609972909.100822126, 0.143000000]: [ArmHW] Successfully initialized the base Arm Interface!
[ INFO] [1609972909.101173442, 0.143000000]: [ArmHWSim] Successfully initialized the simulated Arm Interface!
[ INFO] [1609972909.106703446, 0.143000000]: [GazeboArmControlPlugin] Successfully loaded plugin!
[INFO] [1609972909.291014, 0.326000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1609972909.295917, 0.331000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1609972909.299427, 0.334000]: Loading controller: joint_state_controller
[INFO] [1609972909.319850, 0.354000]: Loading controller: arm_velocity_controller
[INFO] [1609972909.340056, 0.375000]: Controller Spawner: Loaded controllers: joint_state_controller, arm_velocity_controller
[INFO] [1609972909.361044, 0.396000]: Started controllers: joint_state_controller, arm_velocity_controller
[INFO] [1609972919.032423, 10.046000]: Initializing robot_server node
[INFO] [1609972919.053032, 10.066000]: uwrt_arm Sim Robot Server started at 50051

However, when I run

class UWRTArmSim(UWRTArmEnv, Simulation):
    cmd = "roslaunch uwrt_rl_robot_server uwrt_rl_sim_robot_server.launch"
    def __init__(self, ip=None, lower_bound_port=None, upper_bound_port=None, gui=False, **kwargs):
        Simulation.__init__(self, self.cmd, ip, lower_bound_port, upper_bound_port, gui, **kwargs)
        UWRTArmEnv.__init__(self, rs_address=self.robot_server_ip, **kwargs)

I've issues with the Simulation wrapper. I've noticed
rl_server = self.stub.StartNewServer(request= server_manager_pb2.RobotServer(cmd= cmd, gui= gui), timeout =240) returns 0 for my custom env. I've checked this against ur10 which outputs Successfully started Robot Server at localhost:55385.

My launch sequence is as follows:

1. (local python 2.7) where you have to run $ start-server-manager
2. (robo-gym conda environment) where you have to run $ python random_agent_sim.py

I can upload my custom scripts if need be!

Cheers, Aidan

[matteolucchi]

Hi @akeaveny, and congratulations on implementing the ROS bridge for your robot!

I have an idea about what could cause this. In our robot servers launch files we have a default roslaunch argument gui which we use to define whether the robot server should be started with graphical interface or not.

The value of the gui argument is appended to the cmd string, if your robot server launch file doesn't have a gui argument the command will break. A quick fix to overcome this issue would be to add the gui argument to your uwrt_rl_sim_robot_server.launch, it is not necessary to use the argument, for testing purposes it is enough to add it. You can add the argument by adding the following line to your launch file:

<arg name="gui" default="false" />

Could you please try this out and let me know if this solves your issue?

Cheers, Matteo

[akeaveny]

Thanks for the feedback @matteolucchi,

I've tried this but still no luck unfortunately. Here's my launch file:

<?xml version="1.0" ?>
<launch>
  <arg name="sim" default="true"/>
  <arg name="real_robot" value="false" />
  <arg name="uwrt_model" value="uwrt_arm" />

  <arg name="gui" default="false" />
  <arg name="gazebo_gui" default="false" />

  <arg name="server_port" default ="50051"/>
  <arg name="action_cycle_rate" default="50" doc="Rate at which new commands are published to the robot controller"/>
  <arg name="max_velocity_scale_factor" default="1.0" doc="Scale factor multplied by maximum joint velocity value"/>

  <!-- Launch uwrt_arm_bringup -->
  <include file="$(find uwrt_arm_bringup)/launch/arm_bringup.launch">
    <arg name="sim"   value="$(arg sim)"/>
  </include>

  <!-- Launch uwrt_arm_bringup -->
  <include file="$(find uwrt_arm_rover_simulation)/launch/empty_world.launch"/>

  <!-- Launch RViz -->
  <node if="$(arg gui)" name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
  args="-d $(find uwrt_rl_robot_server)/rviz/uwrt_sim_rl.rviz" output="screen">
  </node>

  <!-- Launch Command Handler  -->
  <node name="joint_velocity_command_handler" pkg="uwrt_rl_robot_server" type="joint_velocity_command_handler.py" respawn="false" output="screen" >
    <param name="real_robot"        value="$(arg real_robot)"/>
    <param name="action_cycle_rate" value="$(arg action_cycle_rate)"></param>
  </node>

  <!--Launch gRPC server -->
  <node name="robot_server" pkg="uwrt_rl_robot_server" type="robot_server.py" respawn="false" output="screen" >
    <param name="server_port"                 value="$(arg server_port)"></param>
    <param name="real_robot"                  value="$(arg real_robot)"/>
    <param name="uwrt_model"                  value="$(arg uwrt_model)"/>
    <param name="max_velocity_scale_factor"   value="$(arg max_velocity_scale_factor)"></param>
    <param name="action_cycle_rate"           value="$(arg action_cycle_rate)"></param>
  </node>

</launch>

I've attached a gif for when I run python random_agent_sim.py (rviz still spawns with ).

[matteolucchi]

Thank you for the detailed information!

I'll try to describe what I think is happening. As you mentioned, calling rl_server = self.stub.StartNewServer(request= server_manager_pb2.RobotServer(cmd= cmd, gui= gui), timeout =240) returns 0 in your case. This means that the Server Manager failed the tentative to spawn a new Robot Server.

When self.stub.StartNewServer(request= server_manager_pb2.RobotServer(cmd= cmd, gui= gui), timeout =240) is called, a request is sent to the Server Manager. The following method is executed in the Server Manager: https://github.com/jr-robotics/robo-gym-server-modules/blob/be4cc29c772e70399ec5ab2ab99e442de4188270/robo_gym_server_modules/server_manager/server.py#L107-L120

In your case something fails inside the try block of StartNewServer and the ServerManager returns 0.

Now if we take a look at what happens inside this try block you can see that rl_server = self.srv_mngr.add_rl_server(cmd= request.cmd, gui= request.gui) is called.

In the add_rl_server method (here: https://github.com/jr-robotics/robo-gym-server-modules/blob/be4cc29c772e70399ec5ab2ab99e442de4188270/robo_gym_server_modules/server_manager/server.py#L64-L95) the roslaunch line gets executed, and as you can see the robot simulation starts correctly given that you can see the RViz visualization of your robot correctly. To verify that the RobotServer is started correctly a test gRPC Client is created (test_client= Client('localhost:'+repr(grpc_port))) and it is checked whether the get_state Service call returns a list longer than 1. This assert is most probably what fails.

In the case of the UR Robot Server the get_state service is implemented here https://github.com/jr-robotics/robo-gym-robot-servers/blob/bb6732af7ec6db0f55cac7e73733cfda536e9df4/ur_robot_server/scripts/robot_server.py#L12-L16. A first test that you can do is to temporary replace return self.rosbridge.get_state() with return [0.0 , 0.0, 0.0] for instance, and check whether the environment get created correctly.

If this is the case it means that return self.rosbridge.get_state() does not return a list with len>1.

In the case of the UR Robot Server the get_state method is implemented here: https://github.com/jr-robotics/robo-gym-robot-servers/blob/bb6732af7ec6db0f55cac7e73733cfda536e9df4/ur_robot_server/scripts/ros_bridge.py#L95-L135

In the get_state method a robot_server_pb2.State() message is created and filled with the information that you want have available in your environment:

# Create and fill State message
msg = robot_server_pb2.State()
msg.state.extend(target)
msg.state.extend(ur_state)
msg.state.extend(ee_to_base_transform)
msg.state.extend([ur_collision])
msg.success = 1

You should adapt this method to your needs.

I hope I was somehow clear and that this helps you to fix the issue. If anything is not clear and you need further help don't hesitate to ask!

Cheers, Matteo

[akeaveny]

Hi Matteo,

Thanks again for the detailed response! Your instructions were very clear!

I tried returning GetState with return [0.0 , 0.0, 0.0] and also commenting out the assert statement in server.py assert (len(test_client.get_state())> 1) but still no luck..

Here are my scripts for the robot_server and ros_bridge.

Cheers, Aidan

##############################

robot_server.py

##############################

#!/usr/bin/env python2
import grpc
import rospy
from concurrent import futures
from ros_bridge import UWRTRosBridge
from robo_gym_server_modules.robot_server.grpc_msgs.python import robot_server_pb2, robot_server_pb2_grpc

class RobotServerServicer(robot_server_pb2_grpc.RobotServerServicer):
    def __init__(self, real_robot, uwrt_model):
        self.rosbridge = UWRTRosBridge(real_robot= real_robot, uwrt_model= uwrt_model)

    def GetState(self, request, context):
        try:
            # return self.rosbridge.get_state()
            return [0.0 , 0.0, 0.0]
        except:
            return robot_server_pb2.State(success=0)

    def SetState(self, request, context):
        try:
            self.rosbridge.set_state(state_msg = request)
            return robot_server_pb2.Success(success =1)
        except:
            return robot_server_pb2.Success(success =0)

    def SendAction(self, request, context):
        try:
            executed_action = self.rosbridge.publish_env_arm_cmd(request.action)
            return robot_server_pb2.Success(success =1)
        except:
            return robot_server_pb2.Success(success =0)

def serve():

    server_port = rospy.get_param("~server_port")
    real_robot = rospy.get_param("~real_robot")
    uwrt_model = rospy.get_param("~uwrt_model")

    server = grpc.server(futures.ThreadPoolExecutor(max_workers=10))
    robot_server_pb2_grpc.add_RobotServerServicer_to_server(
        RobotServerServicer(real_robot=real_robot, uwrt_model= uwrt_model), server)
    server.add_insecure_port('[::]:' + repr(server_port))
    server.start()

    if real_robot:
        rospy.loginfo(uwrt_model + " Real Robot Server started at " + repr(server_port))
    else:
        rospy.loginfo(uwrt_model + " Sim Robot Server started at " + repr(server_port))

    rospy.spin()

if __name__ == '__main__':

    try:
        rospy.init_node("robot_server")
        rospy.loginfo('Waiting 10s before starting initialization robot_server')
        rospy.sleep(10)
        rospy.loginfo('Initializing robot_server node')
        serve()
    except (KeyboardInterrupt, SystemExit):
        pass

##############################

ros_bridge.py

##############################

#!/usr/bin/env python

import numpy as np
import rospy
import tf
from geometry_msgs.msg import Twist, Pose, Pose2D
from gazebo_msgs.msg import ModelState, ContactsState
from gazebo_msgs.srv import GetModelState, SetModelState, GetLinkState
from gazebo_msgs.srv import SetModelConfiguration, SetModelConfigurationRequest
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
from std_msgs.msg import Float64MultiArray, Header
from std_srvs.srv import Empty
from visualization_msgs.msg import Marker
from nav_msgs.msg import Odometry
import PyKDL
import copy
from threading import Event
import time
from robo_gym_server_modules.robot_server.grpc_msgs.python import robot_server_pb2

class UWRTRosBridge:

    def __init__(self,  real_robot=False, uwrt_model = 'uwrt_arm'):

        # Event is clear while initialization or set_state is going on
        self.reset = Event()
        self.reset.clear()
        self.get_state_event = Event()
        self.get_state_event.set()

        self.real_robot = real_robot

        # joint_trajectory_command_handler publisher
        self.arm_cmd_pub = rospy.Publisher('env_arm_command', Float64MultiArray, queue_size=1)

        # Target RViz Marker publisher
        self.target_key_pub = rospy.Publisher('target_key', Marker, queue_size=10)

        # TF Listener
        self.tf_listener = tf.TransformListener()

        # Joint States
        self.num_joints = 5
        rospy.Subscriber("joint_states", JointState, self.UWRTJointStateCallback)

        self.observation = {
            'key': {
                # TODO: setting fake key location
                "position": [np.random.uniform(0.625, 0.675),
                             np.random.uniform(-0.30, 0.30),
                             np.random.uniform(0.65, 0.675)],
                "orientation": [0, 0, 0, 1],
                "distance_to_target": [0] * 1,
                "orientation_difference": [0] * 4,
            },
            'uwrt_arm': {
                "position": [0] * self.num_joints,
                "velocity": [0] * self.num_joints,
                "effort": [0] * self.num_joints,
                "joint_limit_switches": [0] * self.num_joints,
            },
        }

        # Robot control rate
        self.sleep_time = (1.0/rospy.get_param("~action_cycle_rate")) - 0.01
        self.control_period = rospy.Duration.from_sec(self.sleep_time)

        self.reference_frame = 'world'
        self.ee_frame = 'arm_wrist_link'

    def UWRTJointStateCallback(self, data):
        if self.get_state_event.is_set():
            self.observation['uwrt_arm']['position'] = data.position
            self.observation['uwrt_arm']['velocity'] = data.velocity
            self.observation['uwrt_arm']['effort'] = data.effort

            # TODO: calculate the difference in distance and orientation
            # self.observation['key']['distance_to_target'] = [0]
            # self.observation['key']['orientation_difference'] = [0] * 4

    def get_state(self):
        self.get_state_event.clear()
        ###
        self.get_state_event.set()

        msg = robot_server_pb2.State()
        # TODO: more efficient method to flatten dictionary
        msg.state.extend(self.observation['key']['position'])
        msg.state.extend(self.observation['key']['orientation'])
        msg.state.extend(self.observation['key']['distance_to_target'])
        msg.state.extend(self.observation['key']['orientation_difference'])
        msg.state.extend(self.observation['uwrt_arm']['position'])
        msg.state.extend(self.observation['uwrt_arm']['velocity'])
        msg.state.extend(self.observation['uwrt_arm']['effort'])
        msg.state.extend(self.observation['uwrt_arm']['joint_limit_switches'])

        msg.success = 1

        return msg

    def set_state(self, state_msg):
        # Set environment state
        state = state_msg.state

        # Clear reset Event
        self.reset.clear()

        # Publish Target Marker
        self.publish_target_key()

        # UR Joints Positions
        reset_steps = int(15.0/self.sleep_time)
        for i in range(reset_steps):
            self.publish_env_arm_cmd(state)

        self.reset.set()

        return 1

    def publish_env_arm_cmd(self, vel_cmd):
        msg = Float64MultiArray()
        msg.data = vel_cmd
        self.arm_cmd_pub.publish(msg)
        rospy.sleep(self.control_period)

    def publish_target_key(self):
        KEY = Marker()
        KEY.type = 1 # CUBE
        KEY.action = 0
        KEY.frame_locked = 1
        KEY.pose.position.x = self.observation['key']['position'][0]
        KEY.pose.position.y = self.observation['key']['position'][1]
        KEY.pose.position.z = self.observation['key']['position'][2]
        KEY.pose.orientation.x = self.observation['key']['orientation'][0]
        KEY.pose.orientation.y = self.observation['key']['orientation'][1]
        KEY.pose.orientation.z = self.observation['key']['orientation'][2]
        KEY.pose.orientation.w = self.observation['key']['orientation'][3]
        KEY.scale.x = 0.1
        KEY.scale.y = 0.1
        KEY.scale.z = 0.1
        KEY.id = 0
        KEY.header.stamp = rospy.Time.now()
        KEY.header.frame_id = self.reference_frame
        KEY.color.a = 0.7
        KEY.color.r = 1.0  # red
        KEY.color.g = 0.0
        KEY.color.b = 0.0
        self.target_key_pub.publish(KEY)

[matteolucchi]

Hi,

Sorry I just realized my answer was incomplete. GetState should return a full message, not just a list. Please try replacing return self.rosbridge.get_state() with:

return robot_server_pb2.State(state=[0.0 , 0.0, 0.0] , success=1)

[akeaveny]

Great, it works now! Thanks again for the support!

Aidan

[matteolucchi]

I am glad to hear that! Feel free to reach out if you need further help!

Cheers,

Matteo