jr-robotics / robo-gym-robot-servers

Repository containing Robot Servers ROS packages
https://sites.google.com/view/robo-gym
MIT License
29 stars 22 forks source link

UR10 robot server example doesn't start successfully #9

Closed Shtaiven closed 2 years ago

Shtaiven commented 2 years ago

Running the following code with robo-gym v1.0.0 installed on a host system with python 3.9.5 and robo-gym-robot-servers v1.0.0 installed on an Ubuntu 20.04.3 guest system with python 3.8.10 and ROS Noetic:

import gym
import robo_gym
from robo_gym.wrappers.exception_handling import ExceptionHandling

target_machine_ip = '<guest_ip_address>'

# initialize environment
env = gym.make('EndEffectorPositioningURSim-v0', ip=target_machine_ip, gui=True)
env = ExceptionHandling(env)

num_episodes = 10

for episode in range(num_episodes):
    done = False
    env.reset()
    while not done:
        # random step in the environment
        state, reward, done, info = env.step(env.action_space.sample())

I get the following error on the guest system:

2021-11-12 09:38:18,013 - serverManager - INFO - Tentative 19 of 20
2021-11-12 09:38:18,014 - serverManager - INFO - Waiting 5s before next tentative ...
2021-11-12 09:38:23,017 - serverManager - ERROR - Could not start Robot Server
NoneType: None
2021-11-12 09:38:23,017 - serverManager - ERROR - Failed to add Robot Server
Traceback (most recent call last):
  File "/home/steven/.local/lib/python3.8/site-packages/robo_gym_server_modules/server_manager/server.py", line 119, in StartNewServer
    assert isinstance(rl_server, int)
AssertionError

When running roslaunch ur_robot_server ur_robot_server.launch ur_model:=ur10 gui:=true I get the following output:

... logging to /home/ubuntu/.ros/log/294c2a26-4395-11ec-86ea-0df47a72e456/roslaunch-ubuntu-kvm2-2798.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

Property names must not start with double underscore:__joint_limit_parameters
when instantiating macro: read_model_data (/home/ubuntu/robogym_ws/src/universal_robot/ur_description/urdf/inc/ur_common.xacro)
instantiated from: ur_robot (/home/ubuntu/robogym_ws/src/universal_robot/ur_description/urdf/ur_macro.xacro)
instantiated from: ur_robot_gazebo (/home/ubuntu/robogym_ws/src/robo-gym-robot-servers/ur_robot_server/urdf/ur_macro.xacro)
in file: /home/ubuntu/robogym_ws/src/robo-gym-robot-servers/ur_robot_server/urdf/ur.xacro
RLException: while processing /home/ubuntu/robogym_ws/src/robo-gym-robot-servers/ur_robot_server/launch/inc/load_ur10.launch.xml:
while processing /home/ubuntu/robogym_ws/src/robo-gym-robot-servers/ur_robot_server/launch/inc/load_ur.launch.xml:
Invalid <param> tag: Cannot load command parameter [robot_description]: command [['/opt/ros/noetic/lib/xacro/xacro', '/home/ubuntu/robogym_ws/src/robo-gym-robot-servers/ur_robot_server/urdf/ur.xacro', 'joint_limit_params:=/home/ubuntu/robogym_ws/src/universal_robot/ur_description/config/ur10/joint_limits.yaml', 'kinematics_params:=/home/ubuntu/robogym_ws/src/universal_robot/ur_description/config/ur10/default_kinematics.yaml', 'physical_params:=/home/ubuntu/robogym_ws/src/universal_robot/ur_description/config/ur10/physical_parameters.yaml', 'visual_params:=/home/ubuntu/robogym_ws/src/universal_robot/ur_description/config/ur10/visual_parameters.yaml', 'transmission_hw_interface:=hardware_interface/EffortJointInterface', 'safety_limits:=false', 'safety_pos_margin:=0.15', 'safety_k_position:=20', 'x:=0.0', 'y:=0.0', 'z:=0.1', 'roll:=0.0', 'pitch:=0.0', 'yaw:=0.0', 'camera1_gazebo:=False', 'camera1_link_x:=0.0', 'camera1_link_y:=0.0', 'camera1_link_z:=0.1', 'camera1_link_roll:=0.0', 'camera1_link_pitch:=0.0', 'camera1_link_yaw:=0.0']] returned with code [2]. 

Param xml is <param name="robot_description" command="$(find xacro)/xacro '$(find ur_robot_server)/urdf/ur.xacro'     joint_limit_params:=$(arg joint_limit_params)     kinematics_params:=$(arg kinematics_params)     physical_params:=$(arg physical_params)     visual_params:=$(arg visual_params)     transmission_hw_interface:=$(arg transmission_hw_interface)     safety_limits:=$(arg safety_limits)     safety_pos_margin:=$(arg safety_pos_margin)     safety_k_position:=$(arg safety_k_position)     x:=$(arg x)     y:=$(arg y)     z:=$(arg z)     roll:=$(arg roll)     pitch:=$(arg pitch)     yaw:=$(arg yaw)     camera1_gazebo:=$(arg camera1_gazebo)     camera1_link_x:=$(arg camera1_link_x)     camera1_link_y:=$(arg camera1_link_y)     camera1_link_z:=$(arg camera1_link_z)     camera1_link_roll:=$(arg camera1_link_roll)     camera1_link_pitch:=$(arg camera1_link_pitch)     camera1_link_yaw:=$(arg camera1_link_yaw)"/>
The traceback for the exception was written to the log file
Shtaiven commented 2 years ago

I've been able to solve this issue by replacing all instances of __ with _ in ur_common.xacro

matteolucchi commented 2 years ago

Dear @Shtaiven thank you very much for pointing this out and suggesting a solution! This was caused by a recent update of xacro. It is now fixed with https://github.com/jr-robotics/universal_robot/pull/4

Thank you very much again