search

ADVRHumanoids / XBotControl

XBotControl framework: XBotCore + OpenSoT + CartesI/O
33 stars 3 forks source link

[XBOT 2] Unable to run Robot Interface , not communication with xbotcore #45

Closed lia2790 closed 3 years ago

lia2790 commented 3 years ago

Problem

I wanted to run my example "CartesioTestsCpp" of my package CartesioTests through the following command

roslaunch cartesio_tests_cpp CartesioTestsCpp.launch

after having set xbot2 with the following command

set_xbot2_config /home/liana/catkin_ws/src/CartesioTests/CartesioTestsCpp/configs/xbot/coman_config/coman_example.yaml

The output obtained is the following

liana@liana-MS-7820 ~/catkin_ws/src/CartesioTests (main)$ roslaunch cartesio_tests_cpp CartesioTestsCpp.launch 
... logging to /home/liana/.ros/log/27ad6200-b94f-11eb-bdf7-d8cb8a3f0cce/roslaunch-liana-MS-7820-15771.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.

started roslaunch server http://liana-MS-7820:33247/

SUMMARY
========

PARAMETERS
 * /cartesian/problem_description: <...>
 * /interactive_markers/tf_prefix: ci
 * /robot_description: <?xml version="1....
 * /robot_description_semantic: <?xml version="1....
 * /robot_is_floating: True
 * /robot_model_type: RBDL
 * /robot_srdf_path: /home/liana/catki...
 * /robot_task_path: /home/liana/catki...
 * /robot_urdf_path: /home/liana/catki...
 * /ros_server_node/enable_otg: True
 * /ros_server_node/is_model_floating_base: True
 * /ros_server_node/model_type: RBDL
 * /ros_server_node/rate: 100.0
 * /ros_server_node/solver: OpenSot
 * /ros_server_node/tf_prefix: ci
 * /ros_server_node/use_xbot_config: False
 * /rosdistro: melodic
 * /rosversion: 1.14.11
 * /solver_type: OpenSot
 * /target_time: 10.0
 * /task_name: com_task

NODES
  /
    CartesioTestsCpp (cartesio_tests_cpp/CartesioTestsCpp)
    interactive_markers (cartesian_interface/marker_spawner)
    ros_server_node (cartesian_interface/ros_server_node)
    rviz (rviz/rviz)

WARNING: [/opt/xbot/share/cartesian_interface/launch/cartesio.launch] unknown <node> attribute 'rosmon-stop-timeout'
auto-starting new master
process[master]: started with pid [15784]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 27ad6200-b94f-11eb-bdf7-d8cb8a3f0cce
process[rosout-1]: started with pid [15800]
started core service [/rosout]
process[ros_server_node-2]: started with pid [15813]
process[interactive_markers-3]: started with pid [15818]
process[CartesioTestsCpp-4]: started with pid [15819]
process[rviz-5]: started with pid [15820]
[ INFO] [1621503519.073385295]: waitForService: Service [/cartesian/get_task_list] has not been advertised, waiting...
[info] Configuring from ROS parameter server
[ INFO] [1621503519.078348504]: I am initializing the Node...
[ INFO] [1621503519.080899639]: I am loading the Node Inputs...
[ WARN] [1621503519.086319517]: Group 'imu_sensors' is empty.
[info] Generated joint id map as:
 joint_map:
  1: LAnkLat
  2: LAnkSag
  3: LElbj
  4: LForearmPlate
  5: LHipLat
  6: LHipSag
  7: LHipYaw
  8: LKneeSag
  9: LShLat
  10: LShSag
  11: LShYaw
  12: LWrj1
  13: LWrj2
  14: RAnkLat
  15: RAnkSag
  16: RElbj
  17: RForearmPlate
  18: RHipLat
  19: RHipSag
  20: RHipYaw
  21: RKneeSag
  22: RShLat
  23: RShSag
  24: RShYaw
  25: RWrj1
  26: RWrj2
  27: WaistLat
  28: WaistSag
  29: WaistYaw
  30: l_leg_ft_joint
  31: r_leg_ft_joint
  32: l_arm_ft_joint
  33: r_arm_ft_joint
[ INFO] [1621503519.088924373]: I am initializing CartesIO...
[ WARN] [1621503519.093112910]: Group 'imu_sensors' is empty.
[info] Generated joint id map as:
 joint_map:
  1: LAnkLat
  2: LAnkSag
  3: LElbj
  4: LForearmPlate
  5: LHipLat
  6: LHipSag
  7: LHipYaw
  8: LKneeSag
  9: LShLat
  10: LShSag
  11: LShYaw
  12: LWrj1
  13: LWrj2
  14: RAnkLat
  15: RAnkSag
  16: RElbj
  17: RForearmPlate
  18: RHipLat
  19: RHipSag
  20: RHipYaw
  21: RKneeSag
  22: RShLat
  23: RShSag
  24: RShYaw
  25: RWrj1
  26: RWrj2
  27: WaistLat
  28: WaistSag
  29: WaistYaw
  30: l_leg_ft_joint
  31: r_leg_ft_joint
  32: l_arm_ft_joint
  33: r_arm_ft_joint
[ok  ] MODEL INTERFACE so library found! 
[ WARN] [1621503519.099468175]: Group 'imu_sensors' is empty.
[info] Found FT l_arm_ft on chain left_arm
[info] Found FT r_arm_ft on chain right_arm
[info] Found FT l_leg_ft on chain left_leg
[info] Found FT r_leg_ft on chain right_leg
[info] Floating base model: TRUE
[ok  ] RobotInterface so library found! 
[info] Floating base link name: base_link
[info] Joint name: LHipSag RBDL ID: 6
[info] Joint name: LHipLat RBDL ID: 7
[info] Joint name: LHipYaw RBDL ID: 8
[info] Joint name: LKneeSag RBDL ID: 9
[info] Joint name: LAnkLat RBDL ID: 10
[info] Joint name: LAnkSag RBDL ID: 11
[info] Joint name: RHipSag RBDL ID: 12
[info] Joint name: RHipLat RBDL ID: 13
[info] Joint name: RHipYaw RBDL ID: 14
[info] Joint name: RKneeSag RBDL ID: 15
[info] Joint name: RAnkLat RBDL ID: 16
[info] Joint name: RAnkSag RBDL ID: 17
[info] Joint name: WaistLat RBDL ID: 18
[info] Joint name: WaistSag RBDL ID: 19
[info] Joint name: WaistYaw RBDL ID: 20
[info] Joint name: LShSag RBDL ID: 21
[info] Joint name: LShLat RBDL ID: 22
[info] Joint name: LShYaw RBDL ID: 23
[info] Joint name: LElbj RBDL ID: 24
[info] Joint name: LForearmPlate RBDL ID: 25
[info] Joint name: LWrj1 RBDL ID: 26
[info] Joint name: LWrj2 RBDL ID: 27
[info] Joint name: RShSag RBDL ID: 28
[info] Joint name: RShLat RBDL ID: 29
[info] Joint name: RShYaw RBDL ID: 30
[info] Joint name: RElbj RBDL ID: 31
[info] Joint name: RForearmPlate RBDL ID: 32
[info] Joint name: RWrj1 RBDL ID: 33
[info] Joint name: RWrj2 RBDL ID: 34
[info] Floating base origin offset: 0 0 0
[ok  ] ModelInterfaceRBDL initialized successfully!
[info] Model ordered chains: 
virtual_chain
left_leg
right_leg
torso
left_arm
right_arm

[ok  ] MODEL INTERFACE so library found! 
ERROR in computeAbsolutePath : the input path  robots/iit-coman-ros-pkg/coman_urdf/urdf/coman.urdf is neither an absolute path nor related with the robotology superbuild. Download it!
terminate called after throwing an instance of 'std::runtime_error'
  what():  shell failed interpreting 'robots/iit-coman-ros-pkg/coman_urdf/urdf/coman.urdf'
[ WARN] [1621503519.109861330]: Group 'imu_sensors' is empty.
[info] Found FT l_arm_ft on chain left_arm
[info] Found FT r_arm_ft on chain right_arm
[info] Found FT l_leg_ft on chain left_leg
[info] Found FT r_leg_ft on chain right_leg
[info] Floating base model: TRUE
[info] Floating base link name: base_link
[info] Joint name: LHipSag RBDL ID: 6
[info] Joint name: LHipLat RBDL ID: 7
[info] Joint name: LHipYaw RBDL ID: 8
[info] Joint name: LKneeSag RBDL ID: 9
[info] Joint name: LAnkLat RBDL ID: 10
[info] Joint name: LAnkSag RBDL ID: 11
[info] Joint name: RHipSag RBDL ID: 12
[info] Joint name: RHipLat RBDL ID: 13
[info] Joint name: RHipYaw RBDL ID: 14
[info] Joint name: RKneeSag RBDL ID: 15
[info] Joint name: RAnkLat RBDL ID: 16
[info] Joint name: RAnkSag RBDL ID: 17
[info] Joint name: WaistLat RBDL ID: 18
[info] Joint name: WaistSag RBDL ID: 19
[info] Joint name: WaistYaw RBDL ID: 20
[info] Joint name: LShSag RBDL ID: 21
[info] Joint name: LShLat RBDL ID: 22
[info] Joint name: LShYaw RBDL ID: 23
[info] Joint name: LElbj RBDL ID: 24
[info] Joint name: LForearmPlate RBDL ID: 25
[info] Joint name: LWrj1 RBDL ID: 26
[info] Joint name: LWrj2 RBDL ID: 27
[info] Joint name: RShSag RBDL ID: 28
[info] Joint name: RShLat RBDL ID: 29
[info] Joint name: RShYaw RBDL ID: 30
[info] Joint name: RElbj RBDL ID: 31
[info] Joint name: RForearmPlate RBDL ID: 32
[info] Joint name: RWrj1 RBDL ID: 33
[info] Joint name: RWrj2 RBDL ID: 34
[info] Floating base origin offset: 0 0 0
[ok  ] ModelInterfaceRBDL initialized successfully!
[info] Model ordered chains: 
virtual_chain
left_leg
right_leg
torso
left_arm
right_arm

[ WARN] [1621503519.119853516]: Group 'imu_sensors' is empty.
[info] Found FT l_arm_ft on chain left_arm
[info] Found FT r_arm_ft on chain right_arm
[info] Found FT l_leg_ft on chain left_leg
[info] Found FT r_leg_ft on chain right_leg
[info] Constructing ROS implementation of RobotInterface.. 
[CartesioTestsCpp-4] process has died [pid 15819, exit code -6, cmd /home/liana/catkin_ws/devel/lib/cartesio_tests_cpp/CartesioTestsCpp __name:=CartesioTestsCpp __log:=/home/liana/.ros/log/27ad6200-b94f-11eb-bdf7-d8cb8a3f0cce/CartesioTestsCpp-4.log].
log file: /home/liana/.ros/log/27ad6200-b94f-11eb-bdf7-d8cb8a3f0cce/CartesioTestsCpp-4*.log
[warn] Unable to communicate with robot (exception thrown: 'Unable to connect to XBotCore'), running in visual mode
[info] Running in visual mode: no commands sent to robot
[ok  ] MODEL INTERFACE so library found! 
[ WARN] [1621503520.133407953]: Group 'imu_sensors' is empty.
[info] Found FT l_arm_ft on chain left_arm
[info] Found FT r_arm_ft on chain right_arm
[info] Found FT l_leg_ft on chain left_leg
[info] Found FT r_leg_ft on chain right_leg
[info] Floating base model: TRUE
[info] Floating base link name: base_link
[info] Joint name: LHipSag RBDL ID: 6
[info] Joint name: LHipLat RBDL ID: 7
[info] Joint name: LHipYaw RBDL ID: 8
[info] Joint name: LKneeSag RBDL ID: 9
[info] Joint name: LAnkLat RBDL ID: 10
[info] Joint name: LAnkSag RBDL ID: 11
[info] Joint name: RHipSag RBDL ID: 12
[info] Joint name: RHipLat RBDL ID: 13
[info] Joint name: RHipYaw RBDL ID: 14
[info] Joint name: RKneeSag RBDL ID: 15
[info] Joint name: RAnkLat RBDL ID: 16
[info] Joint name: RAnkSag RBDL ID: 17
[info] Joint name: WaistLat RBDL ID: 18
[info] Joint name: WaistSag RBDL ID: 19
[info] Joint name: WaistYaw RBDL ID: 20
[info] Joint name: LShSag RBDL ID: 21
[info] Joint name: LShLat RBDL ID: 22
[info] Joint name: LShYaw RBDL ID: 23
[info] Joint name: LElbj RBDL ID: 24
[info] Joint name: LForearmPlate RBDL ID: 25
[info] Joint name: LWrj1 RBDL ID: 26
[info] Joint name: LWrj2 RBDL ID: 27
[info] Joint name: RShSag RBDL ID: 28
[info] Joint name: RShLat RBDL ID: 29
[info] Joint name: RShYaw RBDL ID: 30
[info] Joint name: RElbj RBDL ID: 31
[info] Joint name: RForearmPlate RBDL ID: 32
[info] Joint name: RWrj1 RBDL ID: 33
[info] Joint name: RWrj2 RBDL ID: 34
[info] Floating base origin offset: 0 0 0
[ok  ] ModelInterfaceRBDL initialized successfully!
[info] Model ordered chains: 
virtual_chain
left_leg
right_leg
torso
left_arm
right_arm

[ok  ] Solver options node found
[ok  ] Successfully added Cartesian task with
   BASE LINK:   world
   DISTAL LINK: com
[ok  ] Successfully added task 'JointLimits' with type 'JointLimits'
[ok  ] Successfully added task 'VelocityLimits' with type 'VelocityLimits'
[info] OpenSot: using back-end 'qpoases'
[info] OpenSot: regularization value is 1.0e+09
[info] PilerHelper: expanding to 35 x 1 
[info] PilerHelper: expanding to 35 x 1 
[info] #USING BACK-END @LEVEL 0: qpOASES
[info] PilerHelper: expanding to 35 x 1 
[info] PilerHelper: expanding to 35 x 1 
[ok  ] OpenSotBackEndQPOases so library found! 

[info] PROBLEM 0 ID: CoM_0to1 
[info] CONSTRAINTS ID:  
[info]     # OF CONSTRAINTS: 0 
[info] BOUNDS ID: AGGR_joint_limits+velocity_limits 
[info]     # OF BOUNDS: 35 
[info] # OF VARIABLES: 35 
[info] eps Regularisation factor: 2.221000e-04 
[info] qpOASES # OF CONSTRAINTS: 0
[info] qpOASES # OF VARIABLES: 35
[info] 
[ok  ] Loaded solver 'OpenSot'
[info] Online trajectory generator enabled 
[ok  ] Task::set_otg_vel_limits (1.000000, 1.000000): success
[ok  ] Task::set_otg_acc_limits (10.000000, 10.000000): success
[ INFO] [1621503520.171156254]: waitForService: Service [/cartesian/get_task_list] is now available.
[info] Marker spawner: retrieving task list
[ok  ] Successfully loaded OpenSot
[info] /ros_server_node: started looping @100.0 Hz
[ INFO] [1621503520.472815279]: Creating marker world -> com

[ok  ] Marker spawner: started spinning...

Then, I think the point is here:

...
[ WARN] [1621503519.119853516]: Group 'imu_sensors' is empty.
[info] Found FT l_arm_ft on chain left_arm
[info] Found FT r_arm_ft on chain right_arm
[info] Found FT l_leg_ft on chain left_leg
[info] Found FT r_leg_ft on chain right_leg
[info] Constructing ROS implementation of RobotInterface.. 
[CartesioTestsCpp-4] process has died [pid 15819, exit code -6, cmd /home/liana/catkin_ws/devel/lib/cartesio_tests_cpp/CartesioTestsCpp __name:=CartesioTestsCpp __log:=/home/liana/.ros/log/27ad6200-b94f-11eb-bdf7-d8cb8a3f0cce/CartesioTestsCpp-4.log].
log file: /home/liana/.ros/log/27ad6200-b94f-11eb-bdf7-d8cb8a3f0cce/CartesioTestsCpp-4*.log
[warn] Unable to communicate with robot (exception thrown: 'Unable to connect to XBotCore'), running in visual mode
[info] Running in visual mode: no commands sent to robot
[ok  ] MODEL INTERFACE so library found! 
[ WARN] [1621503520.133407953]: Group 'imu_sensors' is empty.
[info] Found FT l_arm_ft on chain left_arm
[info] Found FT r_arm_ft on chain right_arm
...

how could I fix it? @liesrock

I saw here that I could run

XBotCore -D

but I do not have the command in my machine... could you help me on this? thank you in advance

lia2790 commented 3 years ago

Then, after talking with Luca It was clear that the above issue it was related with the communication with xbot core which in version 2.0 such a command has been changed into 

xbot2-core