search

ADVRHumanoids / XBotControl

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

[XBotInterface] Error in XBotConfig #43

Closed lia2790 closed 3 years ago

lia2790 commented 3 years ago

Problem

I am trying to successfully run a c++ with Cartesio and Xbot. In particular, I am trying to execute my own code (you can find it here) which is basically a wrapper of a c++ code of Davide Antonucci (this one).

When I run my code ( command is roslaunch cartesio_tests_cpp CartesioTestsCpp.launch) I got the following output

liana@liana-MS-7820 ~/catkin_ws $ roslaunch cartesio_tests_cpp CartesioTestsCpp.launch
... logging to /home/liana/.ros/log/aa5fdd8a-b491-11eb-8771-d8cb8a3f0cce/roslaunch-liana-MS-7820-11823.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:40867/

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/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.10
 * /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 [11836]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to aa5fdd8a-b491-11eb-8771-d8cb8a3f0cce
process[rosout-1]: started with pid [11850]
started core service [/rosout]
process[ros_server_node-2]: started with pid [11857]
process[interactive_markers-3]: started with pid [11858]
process[CartesioTestsCpp-4]: started with pid [11859]
[info] Configuring from ROS parameter server
process[rviz-5]: started with pid [11860]
[ WARN] [1620982329.552047421]: 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] [1620982329.561859862]: I am initializing the Node...
[ INFO] [1620982329.566305296]: waitForService: Service [/cartesian/get_task_list] has not been advertised, waiting...
[success] RobotInterface so library found! 
[success] MODEL INTERFACE so library found! 
[ INFO] [1620982329.571450204]: I am loading the Node Inputs...
[ INFO] [1620982329.574875343]: I am initializing CartesIO...
[ WARN] [1620982329.574955799]: Converting unknown joint type of joint 'reference' into a fixed joint
[ WARN] [1620982329.575592674]: 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
[success] ModelInterfaceRBDL initialized successfully!
[info] Model ordered chains: 
virtual_chain
left_leg
right_leg
torso
left_arm
right_arm

[ WARN] [1620982329.583255040]: 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
[ WARN] [1620982329.584828748]: Converting unknown joint type of joint 'reference' into a fixed joint
[success] MODEL INTERFACE so library found! 
[ WARN] [1620982329.585436102]: 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.. 
[ WARN] [1620982329.588923101]: Converting unknown joint type of joint 'reference' into a fixed joint
[ WARN] [1620982329.589657327]: 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
[success] ModelInterfaceRBDL initialized successfully!
[info] Model ordered chains: 
virtual_chain
left_leg
right_leg
torso
left_arm
right_arm

[warning] WARNING in getXBotConfig : XBOT_CONFIG env variable not set.
ERROR in computeAbsolutePath : the input path   is neither an absolute path nor related with the robotology superbuild. Download it!
[error] in FromConfigFile! Can NOT open !
terminate called after throwing an instance of 'YAML::BadFile'
  what():  yaml-cpp: error at line 0, column 0: bad file
[CartesioTestsCpp-4] process has died [pid 11859, exit code -6, cmd /home/liana/catkin_ws/devel/lib/cartesio_tests_cpp/CartesioTestsCpp __name:=CartesioTestsCpp __log:=/home/liana/.ros/log/aa5fdd8a-b491-11eb-8771-d8cb8a3f0cce/CartesioTestsCpp-4.log].
log file: /home/liana/.ros/log/aa5fdd8a-b491-11eb-8771-d8cb8a3f0cce/CartesioTestsCpp-4*.log
[warning] 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
[success] MODEL INTERFACE so library found! 
[ WARN] [1620982330.605220793]: Converting unknown joint type of joint 'reference' into a fixed joint
[ WARN] [1620982330.605869676]: 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
[success] ModelInterfaceRBDL initialized successfully!
[info] Model ordered chains: 
virtual_chain
left_leg
right_leg
torso
left_arm
right_arm

[success] Solver options node found
[success] Successfully added Cartesian task with
   BASE LINK:   world
   DISTAL LINK: com
[success] Successfully added task 'JointLimits' with type 'JointLimits'
[success] Successfully added task 'VelocityLimits' with type 'VelocityLimits'
Created variable 'JointLimits_active' (20 blocks, 10000 elem each)
Created variable 'VelocityLimits_active' (20 blocks, 10000 elem each)
Created variable 'com_task_active' (20 blocks, 10000 elem each)
Created variable 'com_task_pos' (20 blocks, 10000 elem each)
Created variable 'com_task_pos_otg' (20 blocks, 10000 elem each)
Created variable 'com_task_vel' (20 blocks, 10000 elem each)
Created variable 'com_task_rot' (20 blocks, 10000 elem each)
Created variable 'com_task_rot_otg' (20 blocks, 10000 elem each)
Created variable 'com_task_state' (20 blocks, 10000 elem each)
Created variable 'ci_time' (20 blocks, 10000 elem each)
[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 
[success] OpenSotBackEndQPOases so library found! 
[info] Solver Default Options: 

###################   qpOASES  --  QP OPTIONS   ##################

printLevel                     =  PL_NONE

enableRamping                  =  BT_FALSE
enableFarBounds                =  BT_TRUE
enableFlippingBounds           =  BT_FALSE
enableRegularisation           =  BT_FALSE
enableFullLITests              =  BT_FALSE
enableNZCTests                 =  BT_FALSE
enableDriftCorrection          =  0
enableCholeskyRefactorisation  =  0
enableEqualities               =  BT_TRUE

terminationTolerance           =  2.221000e-07
boundTolerance                 =  2.221000e-10
boundRelaxation                =  1.000000e+04
epsNum                         =  -2.221000e-13
epsDen                         =  2.221000e-13
maxPrimalJump                  =  1.000000e+08
maxDualJump                    =  1.000000e+08

initialRamping                 =  5.000000e-01
finalRamping                   =  1.000000e+00
initialFarBounds               =  1.000000e+06
growFarBounds                  =  1.000000e+03
initialStatusBounds            =  ST_INACTIVE
epsFlipping                    =  2.221000e-13
numRegularisationSteps         =  0
epsRegularisation              =  2.221000e-04
numRefinementSteps             =  1
epsIterRef                     =  2.221000e-14
epsLITests                     =  2.221000e-11
epsNZCTests                    =  6.663000e-13

[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] 
[success] Loaded solver 'OpenSot'
[info] Online trajectory generator enabled 
[success] Task::set_otg_vel_limits (1.000000, 1.000000): success
[success] Task::set_otg_acc_limits (10.000000, 10.000000): success
[ INFO] [1620982330.629935131]: waitForService: Service [/cartesian/get_task_list] is now available.
[info] Marker spawner: retrieving task list
[success] Successfully loaded OpenSot
[info] /ros_server_node: started looping @100.0 Hz
Created variable 'q' (20 blocks, 5000 elem each)
Created variable 'qdot' (20 blocks, 5000 elem each)
Created variable 'run_time' (20 blocks, 5000 elem each)
Created variable 'ros_time' (20 blocks, 5000 elem each)
[ INFO] [1620982330.877948464]: Creating marker world -> com

[success] Marker spawner: started spinning...
[rviz-5] process has finished cleanly
log file: /home/liana/.ros/log/aa5fdd8a-b491-11eb-8771-d8cb8a3f0cce/rviz-5*.log
^C[interactive_markers-3] killing on exit
[ros_server_node-2] killing on exit
^CFlushed all data for file '/tmp/ros_executor_log__0_2021_05_14__10_52_09.mat'
^C[info] Dumping data to mat file /tmp/xbot_cartesian_opensot_log_1804289383__2021_05_14__10_52_10.mat
[info] Writing variable tau to mat file...
[info] Writing variable dq to mat file...
[info] Writing variable q to mat file...
^C[info] Writing variable solution_0 to mat file...
[info] Writing variable g_0 to mat file...
^C[info] Writing variable H_0 to mat file...
[info] Writing variable velocity_limits_lowerBound to mat file...
^C[info] Writing variable velocity_limits_upperBound to mat file...
[info] Writing variable joint_limits_upperBound to mat file...
[info] Writing variable CoM_0to1_lambda to mat file...
[info] Writing variable CoM_0to1_c to mat file...
[info] Writing variable joint_limits_lowerBound to mat file...
[info] Writing variable CoM_A to mat file...
[info] Writing variable CoM_pos_actual to mat file...
[info] Writing variable CoM_desiredVelocityRef to mat file...
[info] Writing variable CoM_0to1_W to mat file...
[info] Writing variable l_0 to mat file...
[info] Writing variable CoM_0to1_A to mat file...
[info] Writing variable AGGR_joint_limits+velocity_limits_upperBound to mat file...
[info] Writing variable CoM_b to mat file...
[info] Writing variable CoM_W to mat file...
[info] Writing variable u_0 to mat file...
[info] Writing variable CoM_0to1_b to mat file...
[info] Writing variable CoM_c to mat file...
[info] Writing variable AGGR_joint_limits+velocity_limits_lowerBound to mat file...
[info] Writing variable CoM_lambda to mat file...
[info] Writing variable CoM_position_err to mat file...
[info] Writing variable ddq to mat file...
[info] Writing variable CoM_pos_ref to mat file...
[success] Flushing to /tmp/xbot_cartesian_opensot_log_1804289383__2021_05_14__10_52_10.mat complete!
[success] Cleanly exiting from CartesI/O
Flushed all data for file '/tmp/cartesio_logger__1_2021_05_14__10_52_10.mat'
~LoggerClass
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

where I have some xbotconfig fails. How can I solve it? thank you in advance!

@liesrock @DavideAntonucci

lia2790 commented 3 years ago

Hi guys, this problem I think having fixed using the set_xbot2_config command. I have not this output anymore...