URDF of Nextage Robot wrong

JKBehrens commented 8 years ago

Hello,

a few weeks ago, I was figuring out, that RARM_JOINT5_Link (probably also in the left arm) seem to be 7 mm to short in the URDF model. I was navigating the robot to a pose via rviz and moveit to save the pose using a query to tf. When I then asked the robot to move to the saved pose, it was not going completely there. A few mm were mising. As a workaround, I used different transformations in my code to read and command poses to the end effector.

Can someone please check the URDF model and confirm and maybe correct the robot description? Maybe this issue originates from an inconsistency between the different models used in the different middlewares???

Sorry for being so unspecific. I just wanted to leave a note and hope its obvious to someone. I can give more details, when I get back to the robot.

Thanks! Jan

130s commented 8 years ago

Ok, I'll check the hardware this week. With rulers/tape measure. I'd be great too if you could measure the length of your robot.

As a clarification, what component you're exactly referring to? The white shaft right above the second black "box" in the image below? nxo_vrml_hands_freecad

130s commented 8 years ago

Also, can you provide a code and steps that we can observe the issue you mentioned?

130s commented 8 years ago

FYI @emijah

JKBehrens commented 8 years ago

Thank you for your fast response! I am working this week at a different location. I can give you exact information on next Monday. For now: I moved the gripper to a pose using the interactive markers in RVIZ and read the Position of the EEF via rosrun tf tf_echo /WAIST /RARM_JOINT5_Link Then I asked moveit to move the arm to the saved pose, which resulted in a slightly different pose.

Might it be that the robot state publisher uses the HIRONX URDF while moveit calculates the joint angles with this model? I saw that in HIRONX urdf, RHAND_JOINT0_Link is defined. This is for example missing in NEXTAGE urdf.

Does this already help? I check on Monday with the robot.

130s commented 8 years ago

Ok, I'll write a sample code for it. Meanwhile, can you provide the tf value AND the actual pose value you use? @JKBehrens

Also, if necessary, @emijah can help measuring the actual robot.

emijah commented 8 years ago

I've checked the Nextage Open model against the HiroNX collada file and found that the translation for RARM_JOINT5 and LARM_JOINT5 differs between the two files. The Nextage Open files both vrml and collada define the translation for the final joint as (-0.047, 0.0, -0.090). The HiroNX collada file has the translation down as, (-0.04, 0, -0.09). So there is a difference of 7[mm] between the two. Is this what you are seeing?

hectorherrero commented 8 years ago

Hi Jan,

At Tecnalia (spanish research center) we have a Nextage robot for a few years, we have worked in several projects not only in terms of research conditions but also for industrial environments.

We can reproduce this same issue with also 7mm of difference between the collada file of QNX controller and the URDF model that MoveIt is using for path planning and execution. But no only this, we have created new TFs directly publishing the value of the controller (nextage.getCurrentPosition('RARM_JOINT5','WAIST')) and with this input we have performed both TCP calibration and hand-eye camera calibration obtaining not accurate results, so we strongly believe that the collada file inside of the QNX controller is not accurate enough for working in a normal way. The workaround that we are following is fixing orientations of the TCP in order to alleviate these issues with a set of offsets.

At previous discussions and tickets we concluded that a geometric calibration of the robot could improve these issues, we have not yet begun this process but it is an alternative that does not discarded in short-term.

Best regards,

Héctor

emijah commented 8 years ago

It could be a case of one model being used for one thing, and another model being used for another...

emijah commented 8 years ago

I re-checked the CAD model today. The correct value is 47mm. The collada model for HiroNX is wrong, so please amend.

JKBehrens commented 8 years ago

@emijah Yes, that is what I experience.

In Nextage.urdf of the package nextage_description, I find:

 <link name="RARM_JOINT5_Link">
    <visual>
      <origin xyz="-0.047 0 -0.09" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="package://nextage_description/urdf/meshes/RARM_JOINT5_Link_mesh.dae" scale="1 1 1" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="-0.047 0 -0.09" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="package://nextage_description/urdf/meshes/RARM_JOINT5_Link_mesh.dae" scale="1 1 1" />
      </geometry>
    </collision>
    <inertial>
      <mass value="1.05156" />
      <origin xyz="-0.048476 3.2e-06 -0.0388988" rpy="0 -0 0"/>
      <inertia ixx="0.00194072" ixy="-1.1e-07" ixz="-0.00042482" iyy="0.00209392" iyz="-1.2e-07" izz="0.00035788"/>
    </inertial>
  </link>

while

rosrun tf tf_echo /RARM_JOINT4_Link /RARM_JOINT5_Link

gives me

At time 1462886708.737
- Translation: [-0.040, 0.000, -0.090]
- Rotation: in Quaternion [-0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [-0.000, 0.000, 0.000]
            in RPY (degree) [-0.008, 0.000, 0.000]

I think, it is really inconvenient to have inconsistent models, because this blocks for example teaching poses.

I post the output of the rosbridge terminal here just to be sure.

nxouser@nxconsole:~$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch MODEL_FILE:=/opt/jsk/etc/HIRONX/model/main.wrl ... logging to /home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/roslaunch-nxconsole-27523.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://nxconsole:37976/

SUMMARY

PARAMETERS

/HrpsysSeqStateROSBridge/publish_sensor_transforms: True

/collision_state/comp_name: CollisionDetector0

/controller_configuration: [{'group_name': '...

/diagnostic_aggregator/analyzers/computers/contains: ['HD Temp', 'CPU ...

/diagnostic_aggregator/analyzers/computers/path: Computers

/diagnostic_aggregator/analyzers/computers/type: diagnostic_aggreg...

/diagnostic_aggregator/analyzers/hrpsys/contains: ['hrpEC', 'Emerge...

/diagnostic_aggregator/analyzers/hrpsys/path: Hrpsys

/diagnostic_aggregator/analyzers/hrpsys/type: diagnostic_aggreg...

/diagnostic_aggregator/analyzers/mode/contains: ['Operating Mode'...

/diagnostic_aggregator/analyzers/mode/path: Mode

/diagnostic_aggregator/analyzers/mode/type: diagnostic_aggreg...

/diagnostic_aggregator/analyzers/motor/contains: ['Motor']

/diagnostic_aggregator/analyzers/motor/path: Motor

/diagnostic_aggregator/analyzers/motor/type: diagnostic_aggreg...

/diagnostic_aggregator/base_path:

/diagnostic_aggregator/pub_rate: 1.0

/robot_description: <?xml version="1....

/rosdistro: indigo

/rosversion: 1.11.19

/rtmnameserver_port: 15005

/rtmnameserver_robotname: RobotHardware0

NODES / CollisionDetectorComp (hrpsys/CollisionDetectorComp) DataLoggerServiceROSBridge (hrpsys_ros_bridge/DataLoggerServiceROSBridgeComp) ForwardKinematicsServiceROSBridge (hrpsys_ros_bridge/ForwardKinematicsServiceROSBridgeComp) HrpsysJointTrajectoryBridge (hrpsys_ros_bridge/HrpsysJointTrajectoryBridge) HrpsysSeqStateROSBridge (hrpsys_ros_bridge/HrpsysSeqStateROSBridge) RobotHardwareServiceROSBridge (hrpsys_ros_bridge/RobotHardwareServiceROSBridgeComp) SequencePlayerServiceROSBridge (hrpsys_ros_bridge/SequencePlayerServiceROSBridgeComp) StateHolderServiceROSBridge (hrpsys_ros_bridge/StateHolderServiceROSBridgeComp) collision_state (hrpsys_ros_bridge/collision_state.py) diagnostic_aggregator (diagnostic_aggregator/aggregator_node) hrpsys_profile (hrpsys_ros_bridge/hrpsys_profile.py) hrpsys_py (hironx_ros_bridge/hironx.py) hrpsys_ros_diagnostics (hrpsys_ros_bridge/diagnostics.py) hrpsys_state_publisher (robot_state_publisher/state_publisher) rqt_hironxo (rqt_gui/rqt_gui) rtmlaunch_collision_detector (openrtm_tools/rtmlaunch.py) rtmlaunch_hrpsys_ros_bridge (openrtm_tools/rtmlaunch.py) sensor_ros_bridge_connect (hrpsys_ros_bridge/sensor_ros_bridge_connect.py)

WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtactivate WARNING: unrecognized tag rtactivate WARNING: unrecognized tag rtactivate WARNING: unrecognized tag rtactivate WARNING: unrecognized tag rtactivate WARNING: unrecognized tag rtactivate WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtactivate WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtconnect WARNING: unrecognized tag rtactivate ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found process[hrpsys_py-1]: started with pid [27541] process[HrpsysSeqStateROSBridge-2]: started with pid [27542] process[HrpsysJointTrajectoryBridge-3]: started with pid [27543] process[hrpsys_state_publisher-4]: started with pid [27544] process[hrpsys_ros_diagnostics-5]: started with pid [27549] process[diagnostic_aggregator-6]: started with pid [27553] process[hrpsys_profile-7]: started with pid [27580] process[sensor_ros_bridge_connect-8]: started with pid [27600] process[rtmlaunch_hrpsys_ros_bridge-9]: started with pid [27612] process[SequencePlayerServiceROSBridge-10]: started with pid [27617] process[DataLoggerServiceROSBridge-11]: started with pid [27634] Loading body customizer "/opt/ros/indigo/share/OpenHRP-3.1/customizer/libSampleCustomizer.so" for springJoint process[ForwardKinematicsServiceROSBridge-12]: started with pid [27666] process[StateHolderServiceROSBridge-13]: started with pid [27672] process[RobotHardwareServiceROSBridge-14]: started with pid [27678] duplicated sensor Id is specified(id = 0, name = CAMERA_HEAD_R) duplicated sensor Id is specified(id = 1, name = CAMERA_HEAD_L) [ INFO] [1462884741.081225954]: [HrpsysJointTrajectoryBridge] @Initilize name : HrpsysJointTrajectoryBridge0 done process[CollisionDetectorComp-15]: started with pid [27713] process[rtmlaunch_collision_detector-16]: started with pid [27736] process[collision_state-17]: started with pid [27764] process[rqt_hironxo-18]: started with pid [27777] ;; ;; Could not open /dev/console for writing. ;; open: Permission denied Loading body customizer "/opt/ros/indigo/share/OpenHRP-3.1/customizer/libSampleCustomizer.so" for springJoint [sensor_ros_bridge_connect.py] start configuration ORB with nextage:15005 [rtmlaunch] starting... /home/nxouser/rtmros_nextage_ws/src/rtmros_common/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch [rtmlaunch] RTCTREE_NAMESERVERS nextage:15005 nextage:15005 [rtmlaunch] SIMULATOR_NAME RobotHardware0 prop[collision_pair] ->RARM_JOINT2:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 check collisions between RARM_JOINT2 and LARM_JOINT2 check collisions between RARM_JOINT3 and LARM_JOINT3 check collisions between RARM_JOINT4 and LARM_JOINT4 check collisions between RARM_JOINT5 and LARM_JOINT5 [rtmlaunch] starting... /home/nxouser/rtmros_nextage_ws/src/rtmros_common/hrpsys_ros_bridge/launch/collision_detector.launch [rtmlaunch] RTCTREE_NAMESERVERS nextage:15005 nextage:15005 [rtmlaunch] SIMULATOR_NAME RobotHardware0 [ INFO] [1462884741.515829014]: [HrpsysSeqStateROSBridge] @Initilize name : HrpsysSeqStateROSBridge0 WARNING: Package "ompl" does not follow the version conventions. It should not contain leading zeros (unless the number is 0). Loading body customizer "/opt/ros/indigo/share/OpenHRP-3.1/customizer/libSampleCustomizer.so" for springJoint duplicated sensor Id is specified(id = 0, name = CAMERA_HEAD_R) duplicated sensor Id is specified(id = 1, name = CAMERA_HEAD_L) [ INFO] [1462884741.664371132]: [HrpsysSeqStateROSBridge] Loaded HiroNX [ INFO] [1462884741.666842764]: [HrpsysSeqStateROSBridge] @Initilize name : HrpsysSeqStateROSBridge0 done configuration ORB with nextage:15005 [WARN] [WallTime: 1462884741.705808] CollisionDetector(CollisionDetector0) is not activated, waiting... configuration ORB with nextage:15005 [rtmlaunch] check connection/activation [rtmlaunch] check connection/activation WARNING: Package "ompl" does not follow the version conventions. It should not contain leading zeros (unless the number is 0). [rtmlaunch] Wait for /nextage:15005/HrpsysSeqStateROSBridge0.rtc:rsangle 0 /30 [sensor_ros_bridge_connect.py] wait for RTCmanager : nextage [sensor_ros_bridge_connect.py] wait for RobotHardware0 : <hrpsys.rtm.RTcomponent instance at 0x7feaff97f098> ( timeout 0 < 10) [sensor_ros_bridge_connect.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7feaff97f098> isActive? = True [sensor_ros_bridge_connect.py] simulation_mode : False [sensor_ros_bridge_connect.py] bodyinfo URL = file:///opt/jsk/etc/HIRONX/model/main.wrl [sensor_ros_bridge_connect-8] process has finished cleanly log file: /home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/sensor_ros_bridgeconnect-8.log [WARN] [WallTime: 1462884742.709160] CollisionDetector(CollisionDetector0) is not activated, waiting... [hrpsys.py] wait for RTCmanager : None [hrpsys.py] wait for RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f21673064d0> ( timeout 0 < 10) [hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f21673064d0> isActive? = True [rtmlaunch] Connected from nextage:15005/RobotHardware0.rtc:q [rtmlaunch] to nextage:15005/CollisionDetector0.rtc:qCurrent [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [hrpsys.py] simulation_mode : False [hrpsys.py] Hrpsys controller version info: [hrpsys.py] ms = <hrpsys.rtm.RTCmanager instance at 0x7f216730a2d8> [hrpsys.py] ref = <RTM._objref_Manager instance at 0x7f216730a5a8> [hrpsys.py] version = 315.2.8 [rtmlaunch] Connected from nextage:15005/RobotHardware0.rtc:q [rtmlaunch] to nextage:15005/HrpsysSeqStateROSBridge0.rtc:rsangle [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/RobotHardware0.rtc:tau [rtmlaunch] to nextage:15005/HrpsysSeqStateROSBridge0.rtc:rstorque [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/RobotHardware0.rtc:q [rtmlaunch] to nextage:15005/CollisionDetector0.rtc:qRef [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/StateHolderServiceROSBridge.rtc:StateHolderService [rtmlaunch] to nextage:15005/sh.rtc:StateHolderService [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Activate <- Inactive /nextage:15005/CollisionDetector0.rtc [rtmlaunch] Connected from nextage:15005/sh.rtc:baseTformOut [rtmlaunch] to nextage:15005/HrpsysSeqStateROSBridge0.rtc:baseTform [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [INFO] [WallTime: 1462884743.249350] Connecting to RTM nameserver. host=localhost, port=15005, robotname=RobotHardware0 configuration ORB with localhost:15005 [ERROR] Connection Failed with the Nameserver (hostname=localhost port=15005). Make sure the hostname is correct and the Nameserver is running. CORBA.TRANSIENT(omniORB.TRANSIENT_ConnectFailed, CORBA.COMPLETED_NO) [hrpsys.py] waiting ModelLoader [hrpsys.py] start hrpsys [hrpsys.py] finding RTCManager and RobotHardware [rtmlaunch] Connected from nextage:15005/sh.rtc:qOut [rtmlaunch] to nextage:15005/HrpsysSeqStateROSBridge0.rtc:mcangle [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/HrpsysSeqStateROSBridge0.rtc:SequencePlayerService [rtmlaunch] to nextage:15005/seq.rtc:SequencePlayerService [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/HrpsysJointTrajectoryBridge0.rtc:SequencePlayerService [rtmlaunch] to nextage:15005/seq.rtc:SequencePlayerService [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/DataLoggerServiceROSBridge.rtc:DataLoggerService [rtmlaunch] to nextage:15005/log.rtc:DataLoggerService [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/SequencePlayerServiceROSBridge.rtc:SequencePlayerService [rtmlaunch] to nextage:15005/seq.rtc:SequencePlayerService [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/ForwardKinematicsServiceROSBridge.rtc:ForwardKinematicsService [rtmlaunch] to nextage:15005/fk.rtc:ForwardKinematicsService [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Connected from nextage:15005/RobotHardwareServiceROSBridge.rtc:RobotHardwareService [rtmlaunch] to nextage:15005/RobotHardware0.rtc:RobotHardwareService [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rqt_hironxo-18] process has died [pid 27777, exit code 1, cmd /opt/ros/indigo/lib/rqt_gui/rqt_gui --perspective-file /home/nxouser/rtmros_nextage_ws/src/rtmros_hironx/hironx_ros_bridge/resource/hironx_rqt_dashboard.perspective name:=rqt_hironxo __log:=/home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/rqt_hironxo-18.log]. log file: /home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/rqthironxo-18.log [rtmlaunch] Connected from nextage:15005/RobotHardware0.rtc:servoState [rtmlaunch] to nextage:15005/HrpsysSeqStateROSBridge0.rtc:servoState [rtmlaunch] with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False} [rtmlaunch] Activate <- Inactive /nextage:15005/HrpsysSeqStateROSBridge0.rtc [rtmlaunch] Activate <- Inactive /nextage:15005/HrpsysJointTrajectoryBridge0.rtc [rtmlaunch] Activate <- Inactive /nextage:15005/DataLoggerServiceROSBridge.rtc [rtmlaunch] Activate <- Inactive /nextage:15005/SequencePlayerServiceROSBridge.rtc [rtmlaunch] Activate <- Inactive /nextage:15005/ForwardKinematicsServiceROSBridge.rtc [ INFO] [1462884743.558003080]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 0[Hz](exec 1 Hz, dropped 0) [rtmlaunch] Activate <- Inactive /nextage:15005/StateHolderServiceROSBridge.rtc [rtmlaunch] Activate <- Inactive /nextage:15005/RobotHardwareServiceROSBridge.rtc [ INFO] [1462884743.560940556]: ADD_GROUP: larm (/larm_controller) [ INFO] [1462884743.560971737]: JOINT: LARM_JOINT0 LARM_JOINT1 LARM_JOINT2 LARM_JOINT3 LARM_JOINT4 LARM_JOINT5 [ INFO] [1462884743.599008466]: ADD_GROUP: rarm (/rarm_controller) [ INFO] [1462884743.599032540]: JOINT: RARM_JOINT0 RARM_JOINT1 RARM_JOINT2 RARM_JOINT3 RARM_JOINT4 RARM_JOINT5 [ INFO] [1462884743.616881703]: ADD_GROUP: head (/head_controller) [ INFO] [1462884743.616916376]: JOINT: HEAD_JOINT0 HEAD_JOINT1 [ INFO] [1462884743.636556530]: ADD_GROUP: torso (/torso_controller) [ INFO] [1462884743.636584657]: JOINT: CHEST_JOINT0 [ INFO] [1462884743.657559766]: ADD_GROUP: larm_torso (/larm_torso_controller) [ INFO] [1462884743.657603123]: JOINT: LARM_JOINT0 LARM_JOINT1 LARM_JOINT2 LARM_JOINT3 LARM_JOINT4 LARM_JOINT5 CHEST_JOINT0 [ INFO] [1462884743.678085900]: ADD_GROUP: rarm_torso (/rarm_torso_controller) [ INFO] [1462884743.678122582]: JOINT: RARM_JOINT0 RARM_JOINT1 RARM_JOINT2 RARM_JOINT3 RARM_JOINT4 RARM_JOINT5 CHEST_JOINT0 [INFO] [WallTime: 1462884743.714673] bodyinfo URL = file:///opt/jsk/etc/HIRONX/model/main.wrl [hrpsys.py] wait for RTCmanager : None [hrpsys.py] wait for RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f21673064d0> ( timeout 0 < 10) [hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f21673064d0> isActive? = True [hrpsys.py] simulation_mode : False [hrpsys.py] bodyinfo URL = file:///opt/jsk/etc/HIRONX/model/main.wrl [hrpsys.py] creating components [hrpsys.py] eval : [self.seq, self.seq_svc, self.seq_version] = self.createComp("SequencePlayer","seq") RTC named "seq" already exists. [hrpsys.py] create Comp -> SequencePlayer : <hrpsys.rtm.RTcomponent instance at 0x7f2151ca5878> (315.2.8) [hrpsys.py] create CompSvc -> SequencePlayer Service : <OpenHRP._objref_SequencePlayerService instance at 0x7f21668c5908> [hrpsys.py] eval : [self.sh, self.sh_svc, self.sh_version] = self.createComp("StateHolder","sh") RTC named "sh" already exists. [hrpsys.py] create Comp -> StateHolder : <hrpsys.rtm.RTcomponent instance at 0x7f2151cb87a0> (315.2.8) [ INFO] [1462884744.562363597]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 208[Hz](exec 1774 Hz, dropped 208) [hrpsys.py] create CompSvc -> StateHolder Service : <OpenHRP._objref_StateHolderService instance at 0x7f21668f8710> [hrpsys.py] eval : [self.fk, self.fk_svc, self.fk_version] = self.createComp("ForwardKinematics","fk") RTC named "fk" already exists. [hrpsys.py] create Comp -> ForwardKinematics : <hrpsys.rtm.RTcomponent instance at 0x7f21673155f0> (315.2.8) [hrpsys.py] create CompSvc -> ForwardKinematics Service : <OpenHRP._objref_ForwardKinematicsService instance at 0x7f21673055f0> [hrpsys.py] eval : [self.ic, self.ic_svc, self.ic_version] = self.createComp("ImpedanceController","ic") RTC named "ic" already exists. [hrpsys.py] create Comp -> ImpedanceController : <hrpsys.rtm.RTcomponent instance at 0x7f2167310878> (315.2.8) [hrpsys.py] create CompSvc -> ImpedanceController Service : <OpenHRP._objref_ImpedanceControllerService instance at 0x7f2167305200> [hrpsys.py] eval : [self.el, self.el_svc, self.el_version] = self.createComp("SoftErrorLimiter","el") RTC named "el" already exists. [hrpsys.py] create Comp -> SoftErrorLimiter : <hrpsys.rtm.RTcomponent instance at 0x7f2167309b90> (315.2.8) [hrpsys.py] create CompSvc -> SoftErrorLimiter Service : <OpenHRP._objref_SoftErrorLimiterService instance at 0x7f21672f2b90> [hrpsys.py] eval : [self.sc, self.sc_svc, self.sc_version] = self.createComp("ServoController","sc") RTC named "sc" already exists. [hrpsys.py] create Comp -> ServoController : <hrpsys.rtm.RTcomponent instance at 0x7f21668f74d0> (315.2.8) [hrpsys.py] create CompSvc -> ServoController Service : <OpenHRP._objref_ServoControllerService instance at 0x7f2167318128> [hrpsys.py] eval : [self.log, self.log_svc, self.log_version] = self.createComp("DataLogger","log") RTC named "log" already exists. [hrpsys.py] create Comp -> DataLogger : <hrpsys.rtm.RTcomponent instance at 0x7f21673058c0> (315.2.8) [hrpsys.py] create CompSvc -> DataLogger Service : <OpenHRP._objref_DataLoggerService instance at 0x7f2151df5368> [hrpsys.py] eval : [self.rmfo, self.rmfo_svc, self.rmfo_version] = self.createComp("RemoveForceSensorLinkOffset","rmfo") [hrpsys.py] Fail to createComps 'NoneType' object has no attribute 'ref' [hrpsys.py] connecting components [rtm.py] Connect sh.qOut - ic.qRef [rtm.py] Connect ic.q - el.qRef [rtm.py] Connect el.q - RobotHardware0.qRef [rtm.py] Connect RobotHardware0.servoState - el.servoStateIn [rtm.py] Connect RobotHardware0.q - sh.currentQIn [rtm.py] Connect RobotHardware0.q - fk.q [rtm.py] Connect sh.qOut - fk.qRef [rtm.py] Connect seq.qRef - sh.qIn [rtm.py] Connect seq.tqRef - sh.tqIn [rtm.py] Connect seq.basePos - sh.basePosIn [rtm.py] Connect seq.baseRpy - sh.baseRpyIn [rtm.py] Connect seq.zmpRef - sh.zmpIn [rtm.py] Connect seq.optionalData - sh.optionalDataIn [rtm.py] Connect sh.basePosOut - seq.basePosInit [rtm.py] Connect sh.basePosOut - fk.basePosRef [rtm.py] Connect sh.baseRpyOut - seq.baseRpyInit [rtm.py] Connect sh.baseRpyOut - fk.baseRpyRef [rtm.py] Connect sh.qOut - seq.qInit [rtm.py] Connect sh.zmpOut - seq.zmpRefInit [rtm.py] Connect RobotHardware0.q - ic.qCurrent [rtm.py] Connect RobotHardware0.q - el.qCurrent [hrpsys.py] activating components [hrpsys.py] Fail to find instance (['rmfo', 'RemoveForceSensorLinkOffset']) for getRTCInstanceList seqis already serialized shis already serialized fkis already serialized icis already serialized elis already serialized scis already serialized logis already serialized [hrpsys.py] setup logger [hrpsys.py] setupLogger : q arleady exists in DataLogger [rtm.py] Connect RobotHardware0.q - log.RobotHardware0_q [hrpsys.py] setupLogger : dq arleady exists in DataLogger [rtm.py] Connect RobotHardware0.dq - log.RobotHardware0_dq [ INFO] [1462884745.566755498]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz](exec 1792 Hz, dropped 201) [hrpsys.py] setupLogger : tau arleady exists in DataLogger [rtm.py] Connect RobotHardware0.tau - log.RobotHardware0_tau [hrpsys.py] sensor names for DataLogger [hrpsys.py] setupLogger : qOut arleady exists in DataLogger [rtm.py] Connect sh.qOut - log.sh_qOut [hrpsys.py] setupLogger : tqOut arleady exists in DataLogger [rtm.py] Connect sh.tqOut - log.sh_tqOut [hrpsys.py] setupLogger : basePosOut arleady exists in DataLogger [rtm.py] Connect sh.basePosOut - log.sh_basePosOut [hrpsys.py] setupLogger : baseRpyOut arleady exists in DataLogger [rtm.py] Connect sh.baseRpyOut - log.sh_baseRpyOut [hrpsys.py] setupLogger : zmpOut arleady exists in DataLogger [rtm.py] Connect sh.zmpOut - log.sh_zmpOut [hrpsys.py] setupLogger : q arleady exists in DataLogger [rtm.py] Connect ic.q - log.ic_q [hrpsys.py] setupLogger : q arleady exists in DataLogger [rtm.py] Connect el.q - log.el_q [ INFO] [1462884746.572034064]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz](exec 1793 Hz, dropped 201) [hrpsys.py] setupLogger : emergencySignal arleady exists in DataLogger [rtm.py] Connect RobotHardware0.emergencySignal - log.emergencySignal [hrpsys.py] setupLogger : servoState arleady exists in DataLogger [rtm.py] Connect RobotHardware0.servoState - log.RobotHardware0_servoState [hrpsys.py] setup joint groups [hrpsys.py] initialized successfully [ERROR] [1462884747.118157439]: Robot semantic description not found. Did you forget to define or remap '/robot_description_semantic'? [ INFO] [1462884747.118223487]: Loading robot model 'HiroNX'... [ INFO] [1462884747.118246886]: No root joint specified. Assuming fixed joint [ INFO] [1462884747.572375078]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 200[Hz](exec 1782 Hz, dropped 200) [INFO] [WallTime: 1462884747.638430] Joint names; Torso: ['CHEST_JOINT0'] Head: ['HEAD_JOINT0', 'HEAD_JOINT1'] L-Arm: ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5'] R-Arm: ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'] [WARN] [WallTime: 1462884747.639464] Moveit is not started yet, if you want to use MoveIt! Make sure you've launched MoveGroup (e.g. by launching moveit_planning_execution.launch) [hrpsys_py-1] process has died [pid 27541, exit code -11, cmd /home/nxouser/rtmros_nextage_ws/src/rtmros_hironx/hironx_ros_bridge/scripts/hironx.py --host nextage --port 15005 --modelfile /opt/jsk/etc/HIRONX/model/main.wrl --robot RobotHardware name:=hrpsys_py __log:=/home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/hrpsys_py-1.log]. log file: /home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/hrpsys_py-1*.log [ INFO] [1462884748.572513513]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 200[Hz](exec 1759 Hz, dropped 200) [INFO] [WallTime: 1462884748.728460] check 4 collision status, 0.799878 Hz [ INFO] [1462884749.577146858]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz](exec 1755 Hz, dropped 201) [ INFO] [1462884750.582317638]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz](exec 1766 Hz, dropped 201) [ INFO] [1462884751.582418761]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 200[Hz](exec 1753 Hz, dropped 200) [ INFO] [1462884752.586900478]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz](exec 1761 Hz, dropped 201) [ INFO] [1462884753.587707992]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 200[Hz](exec 1761 Hz, dropped 200) [rtmlaunch] check connection/activation [INFO] [WallTime: 1462884753.748378] check 4 collision status, 0.796818 Hz

I am especially wondering about these Errors:

[rqt_hironxo-18] process has died [pid 27777, exit code 1, cmd /opt/ros/indigo/lib/rqt_gui/rqt_gui --perspective-file /home/nxouser/rtmros_nextage_ws/src/rtmros_hironx/hironx_ros_bridge/resource/hironx_rqt_dashboard.perspective __name:=rqt_hironxo __log:=/home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/rqt_hironxo-18.log]. log file: /home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/rqt_hironxo-18*.log

and

[hrpsys.py] setup joint groups [hrpsys.py] initialized successfully [ERROR] [1462884747.118157439]: Robot semantic description not found. Did you forget to define or remap '/robot_description_semantic'? [ INFO] [1462884747.118223487]: Loading robot model 'HiroNX'... [ INFO] [1462884747.118246886]: No root joint specified. Assuming fixed joint [ INFO] [1462884747.572375078]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 200[Hz](exec 1782 Hz, dropped 200) [INFO] [WallTime: 1462884747.638430] Joint names; Torso: ['CHEST_JOINT0'] Head: ['HEAD_JOINT0', 'HEAD_JOINT1'] L-Arm: ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5'] R-Arm: ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'] [WARN] [WallTime: 1462884747.639464] Moveit is not started yet, if you want to use MoveIt! Make sure you've launched MoveGroup (e.g. by launching moveit_planning_execution.launch) [hrpsys_py-1] process has died [pid 27541, exit code -11, cmd /home/nxouser/rtmros_nextage_ws/src/rtmros_hironx/hironx_ros_bridge/scripts/hironx.py --host nextage --port 15005 --modelfile /opt/jsk/etc/HIRONX/model/main.wrl --robot RobotHardware __name:=hrpsys_py __log:=/home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/hrpsys_py-1.log]. log file: /home/nxouser/.ros/log/06426eec-16ae-11e6-8d85-000bab77d794/hrpsys_py-1*.log

As side note: here, I told that I work on adjusting the PR2 point head action for the Nextage robot. As the rosbridge loads the HIRONX URDF into the /robot_description, I get errors that the root link is not fixed. This was not happening when I used the gazebo simulation. I suppose, this is also an issue with the model of HIRONX.

This is the error: [ WARN] [1462814323.184851794]: The root link WAIST has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.

130s commented 8 years ago

@JKBehrens To address this issue I made pull requests that I can't test without real robot (I do not have access to it immediately). If you could test that'll be great, but if you're not sure then don't hesitate to let me know.

To apply the changes, you need build rtmros_nextage from source like this.

mkdir -p ~/cws_nxo_244/src && cd ~/cws_nxo_244/src && catkin_init_workspace
git clone https://github.com/130s/rtmros_hironx.git && cd rtmros_hironx && git checkout impr/collada_realrobot
cd ~/cws_nxo_244/src
git clone https://github.com/130s/rtmros_nextage.git && cd rtmros_nextage && git checkout fix/real/wrong_wrl
cd ~/cws_nxo_244 && catkin_make && source devel/setup.bash

JKBehrens commented 8 years ago

@130s I built it from source and started the rosbridge, but it is not coming up. CollisionDetector is not found and startup of moveit fails. Is it still correct to start the rosbridge by

roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch MODEL_FILE:=/opt/jsk/etc/HIRONX/model/main.wrl

??

Rosbridge output:

nxouser@nxconsole:~/cws_nxo_244$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch MODEL_FILE:=/opt/jsk/etc/HIRONX/model/main.wrl
... logging to /home/nxouser/.ros/log/58b4004a-1759-11e6-a8dd-000bab77d794/roslaunch-nxconsole-9295.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://nxconsole:33069/

SUMMARY
========

PARAMETERS
 * /HrpsysSeqStateROSBridge/publish_sensor_transforms: True
 * /collision_state/comp_name: CollisionDetector0
 * /controller_configuration: [{'group_name': '...
 * /diagnostic_aggregator/analyzers/computers/contains: ['HD Temp', 'CPU ...
 * /diagnostic_aggregator/analyzers/computers/path: Computers
 * /diagnostic_aggregator/analyzers/computers/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/hrpsys/contains: ['hrpEC', 'Emerge...
 * /diagnostic_aggregator/analyzers/hrpsys/path: Hrpsys
 * /diagnostic_aggregator/analyzers/hrpsys/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/mode/contains: ['Operating Mode'...
 * /diagnostic_aggregator/analyzers/mode/path: Mode
 * /diagnostic_aggregator/analyzers/mode/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/motor/contains: ['Motor']
 * /diagnostic_aggregator/analyzers/motor/path: Motor
 * /diagnostic_aggregator/analyzers/motor/type: diagnostic_aggreg...
 * /diagnostic_aggregator/base_path: 
 * /diagnostic_aggregator/pub_rate: 1.0
 * /robot_description: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.19
 * /rtmnameserver_port: 15005
 * /rtmnameserver_robotname: RobotHardware0

NODES
  /
    CollisionDetectorComp (hrpsys/CollisionDetectorComp)
    DataLoggerServiceROSBridge (hrpsys_ros_bridge/DataLoggerServiceROSBridgeComp)
    ForwardKinematicsServiceROSBridge (hrpsys_ros_bridge/ForwardKinematicsServiceROSBridgeComp)
    HrpsysJointTrajectoryBridge (hrpsys_ros_bridge/HrpsysJointTrajectoryBridge)
    HrpsysSeqStateROSBridge (hrpsys_ros_bridge/HrpsysSeqStateROSBridge)
    RobotHardwareServiceROSBridge (hrpsys_ros_bridge/RobotHardwareServiceROSBridgeComp)
    SequencePlayerServiceROSBridge (hrpsys_ros_bridge/SequencePlayerServiceROSBridgeComp)
    StateHolderServiceROSBridge (hrpsys_ros_bridge/StateHolderServiceROSBridgeComp)
    collision_state (hrpsys_ros_bridge/collision_state.py)
    diagnostic_aggregator (diagnostic_aggregator/aggregator_node)
    hrpsys_profile (hrpsys_ros_bridge/hrpsys_profile.py)
    hrpsys_py (hironx_ros_bridge/hironx.py)
    hrpsys_ros_diagnostics (hrpsys_ros_bridge/diagnostics.py)
    hrpsys_state_publisher (robot_state_publisher/state_publisher)
    rqt_hironxo (rqt_gui/rqt_gui)
    rtmlaunch_collision_detector (openrtm_tools/rtmlaunch.py)
    rtmlaunch_hrpsys_ros_bridge (openrtm_tools/rtmlaunch.py)
    sensor_ros_bridge_connect (hrpsys_ros_bridge/sensor_ros_bridge_connect.py)

WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtactivate
WARNING: unrecognized tag rtactivate
WARNING: unrecognized tag rtactivate
WARNING: unrecognized tag rtactivate
WARNING: unrecognized tag rtactivate
WARNING: unrecognized tag rtactivate
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtactivate
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtconnect
WARNING: unrecognized tag rtactivate
auto-starting new master
process[master]: started with pid [9307]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 58b4004a-1759-11e6-a8dd-000bab77d794
process[rosout-1]: started with pid [9320]
started core service [/rosout]
process[hrpsys_py-2]: started with pid [9337]
process[HrpsysSeqStateROSBridge-3]: started with pid [9338]
process[HrpsysJointTrajectoryBridge-4]: started with pid [9339]
process[hrpsys_state_publisher-5]: started with pid [9340]
process[hrpsys_ros_diagnostics-6]: started with pid [9348]
process[diagnostic_aggregator-7]: started with pid [9353]
process[hrpsys_profile-8]: started with pid [9382]
process[sensor_ros_bridge_connect-9]: started with pid [9394]
process[rtmlaunch_hrpsys_ros_bridge-10]: started with pid [9396]
process[SequencePlayerServiceROSBridge-11]: started with pid [9404]
process[DataLoggerServiceROSBridge-12]: started with pid [9407]
process[ForwardKinematicsServiceROSBridge-13]: started with pid [9416]
process[StateHolderServiceROSBridge-14]: started with pid [9435]
Loading body customizer "/opt/ros/indigo/share/OpenHRP-3.1/customizer/libSampleCustomizer.so" for springJoint
process[RobotHardwareServiceROSBridge-15]: started with pid [9472]
process[CollisionDetectorComp-16]: started with pid [9495]
duplicated sensor Id is specified(id = 0, name = CAMERA_HEAD_R)
duplicated sensor Id is specified(id = 1, name = CAMERA_HEAD_L)
[ INFO] [1462958319.635363098]: [HrpsysJointTrajectoryBridge] @Initilize name : HrpsysJointTrajectoryBridge0 done
process[rtmlaunch_collision_detector-17]: started with pid [9508]
process[collision_state-18]: started with pid [9536]
process[rqt_hironxo-19]: started with pid [9537]
;;
;; Could not open /dev/console for writing.
;;
open: Permission denied
ModelLoaderException : /opt/jsk/etc/NEXTAGE/model/main.wrl cannot be found.
failed to load model[file:///opt/jsk/etc/NEXTAGE/model/main.wrl]
terminate called after throwing an instance of 'CORBA::BAD_PARAM'
/opt/ros/indigo/share/hrpsys_ros_bridge/scripts/diagnostics.py:89: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
  pub = rospy.Publisher('diagnostics', DiagnosticArray)
[ERROR] [1462958319.840246048]: Target Node visual1/node_joint0_axis0 NOT found!!!

[sensor_ros_bridge_connect.py]  start
configuration ORB with nextage:15005
[rtmlaunch] starting...  /opt/ros/indigo/share/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
[rtmlaunch] RTCTREE_NAMESERVERS nextage:15005 nextage:15005
[rtmlaunch] SIMULATOR_NAME RobotHardware0
/opt/ros/indigo/share/hrpsys_ros_bridge/scripts/hrpsys_profile.py:88: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
  pub = rospy.Publisher('diagnostics', DiagnosticArray)
[CollisionDetectorComp-16] process has died [pid 9495, exit code -6, cmd /opt/ros/indigo/lib/hrpsys/CollisionDetectorComp -o corba.nameservers:nextage:15005 -o naming.formats:%n.rtc -o exec_cxt.periodic.type:PeriodicExecutionContext -o exec_cxt.periodic.rate:2000 -o logger.file_name:/tmp/collision_detector%p.log -o example.CollisionDetector.config_file:/home/nxouser/cws_nxo_244/src/rtmros_nextage/nextage_ros_bridge/conf/realrobot_fixedpath.conf __name:=CollisionDetectorComp __log:=/home/nxouser/.ros/log/58b4004a-1759-11e6-a8dd-000bab77d794/CollisionDetectorComp-16.log].
log file: /home/nxouser/.ros/log/58b4004a-1759-11e6-a8dd-000bab77d794/CollisionDetectorComp-16*.log
[rtmlaunch] starting...  /opt/ros/indigo/share/hrpsys_ros_bridge/launch/collision_detector.launch
[rtmlaunch] RTCTREE_NAMESERVERS nextage:15005 nextage:15005
[rtmlaunch] SIMULATOR_NAME RobotHardware0
[ INFO] [1462958320.112218546]: [HrpsysSeqStateROSBridge] @Initilize name : HrpsysSeqStateROSBridge0
WARNING: Package "ompl" does not follow the version conventions. It should not contain leading zeros (unless the number is 0).
Loading body customizer "/opt/ros/indigo/share/OpenHRP-3.1/customizer/libSampleCustomizer.so" for springJoint
configuration ORB with nextage:15005
duplicated sensor Id is specified(id = 0, name = CAMERA_HEAD_R)
duplicated sensor Id is specified(id = 1, name = CAMERA_HEAD_L)
/opt/ros/indigo/share/hrpsys_ros_bridge/scripts/collision_state.py:181: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
  pub_diagnostics = rospy.Publisher('diagnostics', DiagnosticArray)
/opt/ros/indigo/share/hrpsys_ros_bridge/scripts/collision_state.py:182: SyntaxWarning: The publisher should be created with an explicit keyword argument 'queue_size'. Please see http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers for more information.
  pub_collision = rospy.Publisher('collision_detector_marker_array', MarkerArray)
configuration ORB with nextage:15005
[ INFO] [1462958320.248542976]: [HrpsysSeqStateROSBridge] Loaded HiroNX
[ INFO] [1462958320.251199425]: [HrpsysSeqStateROSBridge] @Initilize name : HrpsysSeqStateROSBridge0 done
[WARN] [WallTime: 1462958320.253952] Could not found CollisionDetector(CollisionDetector0), waiting...
[rtmlaunch] check connection/activation
WARNING: Package "ompl" does not follow the version conventions. It should not contain leading zeros (unless the number is 0).
[rtmlaunch] check connection/activation
[sensor_ros_bridge_connect.py] wait for RTCmanager : nextage
[sensor_ros_bridge_connect.py] wait for RobotHardware0 : <hrpsys.rtm.RTcomponent instance at 0x7fdcedf9f7e8> ( timeout 0 < 10)
[sensor_ros_bridge_connect.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7fdcedf9f7e8> isActive? = True 
[rtmlaunch] Wait for  /nextage:15005/HrpsysSeqStateROSBridge0.rtc:rsangle   0 /30
[sensor_ros_bridge_connect.py] simulation_mode : False
[sensor_ros_bridge_connect.py]   bodyinfo URL = file:///opt/jsk/etc/HIRONX/model/main.wrl
[sensor_ros_bridge_connect-9] process has finished cleanly
log file: /home/nxouser/.ros/log/58b4004a-1759-11e6-a8dd-000bab77d794/sensor_ros_bridge_connect-9*.log
[hrpsys.py] wait for RTCmanager : None
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   0 /30
[WARN] [WallTime: 1462958321.256404] Could not found CollisionDetector(CollisionDetector0), waiting...
[hrpsys.py] wait for RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f2062dcb638> ( timeout 0 < 10)
[hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f2062dcb638> isActive? = True 
[hrpsys.py] simulation_mode : False
[hrpsys.py]  Hrpsys controller version info: 
[hrpsys.py]    ms =  <hrpsys.rtm.RTCmanager instance at 0x7f2062dce440>
[hrpsys.py]    ref =  <RTM._objref_Manager instance at 0x7f2062dce710>
[INFO] [WallTime: 1462958321.765429] Connecting to RTM nameserver. host=localhost, port=15005, robotname=RobotHardware0
configuration ORB with localhost:15005
[ERROR] Connection Failed with the Nameserver (hostname=localhost port=15005).
Make sure the hostname is correct and the Nameserver is running.
CORBA.TRANSIENT(omniORB.TRANSIENT_ConnectFailed, CORBA.COMPLETED_NO)
[hrpsys.py]    version  =  315.2.8
[rqt_hironxo-19] process has died [pid 9537, exit code 1, cmd /opt/ros/indigo/lib/rqt_gui/rqt_gui --perspective-file /home/nxouser/cws_nxo_244/src/rtmros_hironx/hironx_ros_bridge/resource/hironx_rqt_dashboard.perspective __name:=rqt_hironxo __log:=/home/nxouser/.ros/log/58b4004a-1759-11e6-a8dd-000bab77d794/rqt_hironxo-19.log].
log file: /home/nxouser/.ros/log/58b4004a-1759-11e6-a8dd-000bab77d794/rqt_hironxo-19*.log
[hrpsys.py] waiting ModelLoader
[hrpsys.py] start hrpsys
[hrpsys.py] finding RTCManager and RobotHardware
[WARN] [WallTime: 1462958322.258694] Could not found CollisionDetector(CollisionDetector0), waiting...
[rtmlaunch] Connected from nextage:15005/RobotHardware0.rtc:q
[rtmlaunch]             to nextage:15005/HrpsysSeqStateROSBridge0.rtc:rsangle
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/RobotHardware0.rtc:tau
[rtmlaunch]             to nextage:15005/HrpsysSeqStateROSBridge0.rtc:rstorque
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/StateHolderServiceROSBridge.rtc:StateHolderService
[rtmlaunch]             to nextage:15005/sh.rtc:StateHolderService
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/sh.rtc:baseTformOut
[rtmlaunch]             to nextage:15005/HrpsysSeqStateROSBridge0.rtc:baseTform
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/sh.rtc:qOut
[rtmlaunch]             to nextage:15005/HrpsysSeqStateROSBridge0.rtc:mcangle
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/HrpsysSeqStateROSBridge0.rtc:SequencePlayerService
[rtmlaunch]             to nextage:15005/seq.rtc:SequencePlayerService
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/HrpsysJointTrajectoryBridge0.rtc:SequencePlayerService
[rtmlaunch]             to nextage:15005/seq.rtc:SequencePlayerService
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/DataLoggerServiceROSBridge.rtc:DataLoggerService
[rtmlaunch]             to nextage:15005/log.rtc:DataLoggerService
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/SequencePlayerServiceROSBridge.rtc:SequencePlayerService
[rtmlaunch]             to nextage:15005/seq.rtc:SequencePlayerService
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/ForwardKinematicsServiceROSBridge.rtc:ForwardKinematicsService
[rtmlaunch]             to nextage:15005/fk.rtc:ForwardKinematicsService
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/RobotHardwareServiceROSBridge.rtc:RobotHardwareService
[rtmlaunch]             to nextage:15005/RobotHardware0.rtc:RobotHardwareService
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Connected from nextage:15005/RobotHardware0.rtc:servoState
[rtmlaunch]             to nextage:15005/HrpsysSeqStateROSBridge0.rtc:servoState
[rtmlaunch]           with {'id': '', 'properties': {'dataport.publisher.push_policy': 'all', 'dataport.subscription_type': 'new'}, 'name': None, 'verbose': False}
[rtmlaunch] Activate <- Inactive /nextage:15005/HrpsysSeqStateROSBridge0.rtc
[rtmlaunch] Activate <- Inactive /nextage:15005/HrpsysJointTrajectoryBridge0.rtc
[rtmlaunch] Activate <- Inactive /nextage:15005/DataLoggerServiceROSBridge.rtc
[rtmlaunch] Activate <- Inactive /nextage:15005/SequencePlayerServiceROSBridge.rtc
[rtmlaunch] Activate <- Inactive /nextage:15005/ForwardKinematicsServiceROSBridge.rtc
[ INFO] [1462958322.737495522]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 0[Hz] (exec 1 Hz, dropped 0)
[rtmlaunch] Activate <- Inactive /nextage:15005/StateHolderServiceROSBridge.rtc
[rtmlaunch] Activate <- Inactive /nextage:15005/RobotHardwareServiceROSBridge.rtc
[ INFO] [1462958322.738261802]: ADD_GROUP: larm (/larm_controller)
[ INFO] [1462958322.738290051]:     JOINT: LARM_JOINT0 LARM_JOINT1 LARM_JOINT2 LARM_JOINT3 LARM_JOINT4 LARM_JOINT5
[ INFO] [1462958322.779341709]: ADD_GROUP: rarm (/rarm_controller)
[ INFO] [1462958322.779369832]:     JOINT: RARM_JOINT0 RARM_JOINT1 RARM_JOINT2 RARM_JOINT3 RARM_JOINT4 RARM_JOINT5
[ INFO] [1462958322.798754398]: ADD_GROUP: head (/head_controller)
[ INFO] [1462958322.798776139]:     JOINT: HEAD_JOINT0 HEAD_JOINT1
[ INFO] [1462958322.818228894]: ADD_GROUP: torso (/torso_controller)
[ INFO] [1462958322.818249314]:     JOINT: CHEST_JOINT0
[ INFO] [1462958322.839070272]: ADD_GROUP: larm_torso (/larm_torso_controller)
[ INFO] [1462958322.839097371]:     JOINT: LARM_JOINT0 LARM_JOINT1 LARM_JOINT2 LARM_JOINT3 LARM_JOINT4 LARM_JOINT5 CHEST_JOINT0
[ INFO] [1462958322.859151814]: ADD_GROUP: rarm_torso (/rarm_torso_controller)
[ INFO] [1462958322.859174860]:     JOINT: RARM_JOINT0 RARM_JOINT1 RARM_JOINT2 RARM_JOINT3 RARM_JOINT4 RARM_JOINT5 CHEST_JOINT0
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   1 /30
[hrpsys.py] wait for RTCmanager : None
[hrpsys.py] wait for RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f2062dcb638> ( timeout 0 < 10)
[hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f2062dcb638> isActive? = True 
[WARN] [WallTime: 1462958323.262670] Could not found CollisionDetector(CollisionDetector0), waiting...
[hrpsys.py] simulation_mode : False
[hrpsys.py]   bodyinfo URL = file:///opt/jsk/etc/HIRONX/model/main.wrl
[hrpsys.py] creating components
[hrpsys.py]   eval : [self.seq, self.seq_svc, self.seq_version] = self.createComp("SequencePlayer","seq")
RTC named "seq" already exists.
[hrpsys.py] create Comp -> SequencePlayer : <hrpsys.rtm.RTcomponent instance at 0x7f2062dd47a0> (315.2.8)
[hrpsys.py] create CompSvc -> SequencePlayer Service : <OpenHRP._objref_SequencePlayerService instance at 0x7f204d8dcb48>
[hrpsys.py]   eval : [self.sh, self.sh_svc, self.sh_version] = self.createComp("StateHolder","sh")
RTC named "sh" already exists.
[hrpsys.py] create Comp -> StateHolder : <hrpsys.rtm.RTcomponent instance at 0x7f2062dabef0> (315.2.8)
[hrpsys.py] create CompSvc -> StateHolder Service : <OpenHRP._objref_StateHolderService instance at 0x7f204d8543f8>
[hrpsys.py]   eval : [self.fk, self.fk_svc, self.fk_version] = self.createComp("ForwardKinematics","fk")
RTC named "fk" already exists.
[hrpsys.py] create Comp -> ForwardKinematics : <hrpsys.rtm.RTcomponent instance at 0x7f2062dce908> (315.2.8)
[hrpsys.py] create CompSvc -> ForwardKinematics Service : <OpenHRP._objref_ForwardKinematicsService instance at 0x7f204d7eb6c8>
[hrpsys.py]   eval : [self.ic, self.ic_svc, self.ic_version] = self.createComp("ImpedanceController","ic")
RTC named "ic" already exists.
[hrpsys.py] create Comp -> ImpedanceController : <hrpsys.rtm.RTcomponent instance at 0x7f204d896998> (315.2.8)
[hrpsys.py] create CompSvc -> ImpedanceController Service : <OpenHRP._objref_ImpedanceControllerService instance at 0x7f204d7e5f38>
[hrpsys.py]   eval : [self.el, self.el_svc, self.el_version] = self.createComp("SoftErrorLimiter","el")
RTC named "el" already exists.
[hrpsys.py] create Comp -> SoftErrorLimiter : <hrpsys.rtm.RTcomponent instance at 0x7f2062dd47e8> (315.2.8)
[hrpsys.py] create CompSvc -> SoftErrorLimiter Service : <OpenHRP._objref_SoftErrorLimiterService instance at 0x7f204d7f6b48>
[hrpsys.py]   eval : [self.sc, self.sc_svc, self.sc_version] = self.createComp("ServoController","sc")
RTC named "sc" already exists.
[hrpsys.py] create Comp -> ServoController : <hrpsys.rtm.RTcomponent instance at 0x7f204d7fb2d8> (315.2.8)
[hrpsys.py] create CompSvc -> ServoController Service : <OpenHRP._objref_ServoControllerService instance at 0x7f204d81a050>
[hrpsys.py]   eval : [self.log, self.log_svc, self.log_version] = self.createComp("DataLogger","log")
RTC named "log" already exists.
[hrpsys.py] create Comp -> DataLogger : <hrpsys.rtm.RTcomponent instance at 0x7f204d813128> (315.2.8)
[ INFO] [1462958323.738917155]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 208[Hz] (exec 1788 Hz, dropped 208)
[hrpsys.py] create CompSvc -> DataLogger Service : <OpenHRP._objref_DataLoggerService instance at 0x7f204d85e7a0>
[hrpsys.py]   eval : [self.rmfo, self.rmfo_svc, self.rmfo_version] = self.createComp("RemoveForceSensorLinkOffset","rmfo")
[hrpsys.py] Fail to createComps 'NoneType' object has no attribute 'ref'
[hrpsys.py] connecting components
[rtm.py]    Connect sh.qOut - ic.qRef
[rtm.py]    Connect ic.q - el.qRef
[rtm.py]    Connect el.q - RobotHardware0.qRef
[rtm.py]    Connect RobotHardware0.servoState - el.servoStateIn
[rtm.py]    Connect RobotHardware0.q - sh.currentQIn
[rtm.py]    Connect RobotHardware0.q - fk.q
[rtm.py]    Connect sh.qOut - fk.qRef
[rtm.py]    Connect seq.qRef - sh.qIn
[rtm.py]    Connect seq.tqRef - sh.tqIn
[rtm.py]    Connect seq.basePos - sh.basePosIn
[rtm.py]    Connect seq.baseRpy - sh.baseRpyIn
[rtm.py]    Connect seq.zmpRef - sh.zmpIn
[rtm.py]    Connect seq.optionalData - sh.optionalDataIn
[rtm.py]    Connect sh.basePosOut - seq.basePosInit
[rtm.py]    Connect sh.basePosOut - fk.basePosRef
[rtm.py]    Connect sh.baseRpyOut - seq.baseRpyInit
[rtm.py]    Connect sh.baseRpyOut - fk.baseRpyRef
[rtm.py]    Connect sh.qOut - seq.qInit
[rtm.py]    Connect sh.zmpOut - seq.zmpRefInit
[rtm.py]    Connect RobotHardware0.q - ic.qCurrent
[rtm.py]    Connect RobotHardware0.q - el.qCurrent
[WARN] [WallTime: 1462958324.264551] Could not found CollisionDetector(CollisionDetector0), waiting...
[hrpsys.py] activating components
[hrpsys.py] Fail to find instance (['rmfo', 'RemoveForceSensorLinkOffset']) for getRTCInstanceList
seqis already serialized
shis already serialized
fkis already serialized
icis already serialized
elis already serialized
scis already serialized
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   2 /30
logis already serialized
[hrpsys.py] setup logger
[hrpsys.py]   setupLogger : q arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.q - log.RobotHardware0_q
[hrpsys.py]   setupLogger : dq arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.dq - log.RobotHardware0_dq
[ INFO] [1462958324.740893933]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 200[Hz] (exec 1793 Hz, dropped 200)
[hrpsys.py]   setupLogger : tau arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.tau - log.RobotHardware0_tau
[hrpsys.py] sensor names for DataLogger
[hrpsys.py]   setupLogger : qOut arleady exists in DataLogger
[rtm.py]    Connect sh.qOut - log.sh_qOut
[WARN] [WallTime: 1462958325.268108] Could not found CollisionDetector(CollisionDetector0), waiting...
[hrpsys.py]   setupLogger : tqOut arleady exists in DataLogger
[rtm.py]    Connect sh.tqOut - log.sh_tqOut
[hrpsys.py]   setupLogger : basePosOut arleady exists in DataLogger
[rtm.py]    Connect sh.basePosOut - log.sh_basePosOut
[hrpsys.py]   setupLogger : baseRpyOut arleady exists in DataLogger
[rtm.py]    Connect sh.baseRpyOut - log.sh_baseRpyOut
[ INFO] [1462958325.744256731]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1786 Hz, dropped 201)
[hrpsys.py]   setupLogger : zmpOut arleady exists in DataLogger
[rtm.py]    Connect sh.zmpOut - log.sh_zmpOut
[hrpsys.py]   setupLogger : q arleady exists in DataLogger
[rtm.py]    Connect ic.q - log.ic_q
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   3 /30
[hrpsys.py]   setupLogger : q arleady exists in DataLogger
[rtm.py]    Connect el.q - log.el_q
[hrpsys.py]   setupLogger : emergencySignal arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.emergencySignal - log.emergencySignal
[WARN] [WallTime: 1462958326.270300] Could not found CollisionDetector(CollisionDetector0), waiting...
[hrpsys.py]   setupLogger : servoState arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.servoState - log.RobotHardware0_servoState
[hrpsys.py] setup joint groups
[hrpsys.py] initialized successfully
[ WARN] [1462958326.666126310]: COLLADA warning: The DOM was unable to create an element named copyright at line 10. Probably a schema violation.

[ERROR] [1462958326.729907735]: Target Node visual1/node_joint0_axis0 NOT found!!!

[ WARN] [1462958326.732925085]: could not find binding for axis: kmodel1/jointsid1000/axis0, axis0

[ WARN] [1462958326.735328409]: could not find binding for axis: kmodel1/jointsid1000/axis0, axis0

[ INFO] [1462958326.749766192]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1794 Hz, dropped 201)
[ WARN] [1462958326.801411792]: could not find openrave joint WAIST!

[ERROR] [1462958326.805462970]: Robot semantic description not found. Did you forget to define or remap '/robot_description_semantic'?
[ INFO] [1462958326.805521295]: Loading robot model 'HiroNX'...
[ INFO] [1462958326.805553115]: No root joint specified. Assuming fixed joint
[WARN] [WallTime: 1462958327.271933] Could not found CollisionDetector(CollisionDetector0), waiting...
[INFO] [WallTime: 1462958327.280145] Joint names; Torso: ['CHEST_JOINT0']
    Head: ['HEAD_JOINT0', 'HEAD_JOINT1']
    L-Arm: ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']
    R-Arm: ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
[WARN] [WallTime: 1462958327.281033] Moveit is not started yet, if you want to use MoveIt!
Make sure you've launched MoveGroup (e.g. by launching moveit_planning_execution.launch)
[hrpsys_py-2] process has finished cleanly
log file: /home/nxouser/.ros/log/58b4004a-1759-11e6-a8dd-000bab77d794/hrpsys_py-2*.log
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   4 /30
[ INFO] [1462958327.750806214]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 200[Hz] (exec 1774 Hz, dropped 200)
[WARN] [WallTime: 1462958328.274363] Could not found CollisionDetector(CollisionDetector0), waiting...
[ INFO] [1462958328.753798995]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1769 Hz, dropped 201)
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   5 /30
[WARN] [WallTime: 1462958329.276107] Could not found CollisionDetector(CollisionDetector0), waiting...
[ INFO] [1462958329.753916426]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 200[Hz] (exec 1759 Hz, dropped 200)
[ERROR] [WallTime: 1462958330.276465] Could not found CollisionDetector, exiting...
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   6 /30
[collision_state-18] process has finished cleanly
log file: /home/nxouser/.ros/log/58b4004a-1759-11e6-a8dd-000bab77d794/collision_state-18*.log
[ INFO] [1462958330.760497632]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1769 Hz, dropped 201)
[ INFO] [1462958331.764760083]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1769 Hz, dropped 201)
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   7 /30
[ INFO] [1462958332.769944846]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1770 Hz, dropped 201)
[rtmlaunch] check connection/activation
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   8 /30
[ INFO] [1462958333.774766764]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1794 Hz, dropped 201)
[rtmlaunch] Wait for  /nextage:15005/CollisionDetector0.rtc:qCurrent   9 /30
[ INFO] [1462958334.779621655]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1792 Hz, dropped 201)
[ INFO] [1462958335.783957581]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz] (exec 1781 Hz, dropped 201)
^C[rtmlaunch_collision_detector-17] killing on exit
[RobotHardwareServiceROSBridge-15] killing on exit
[StateHolderServiceROSBridge-14] killing on exit
[ForwardKinematicsServiceROSBridge-13] killing on exit
[rtmlaunch] Catch signal 'SIGINT', exitting...
[DataLoggerServiceROSBridge-12] killing on exit
[SequencePlayerServiceROSBridge-11] killing on exit
[rtmlaunch_hrpsys_ros_bridge-10] killing on exit
[hrpsys_profile-8] killing on exit
[diagnostic_aggregator-7] killing on exit
[hrpsys_ros_diagnostics-6] killing on exit
[rtmlaunch] Catch signal 'SIGINT', exitting...
[hrpsys_state_publisher-5] killing on exit
[HrpsysJointTrajectoryBridge-4] killing on exit
[ INFO] [1462958336.179694341]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (larm
[ INFO] [1462958336.194349068]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (rarm
[ INFO] [1462958336.201351281]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (head
[ INFO] [1462958336.206884934]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (torso
[ INFO] [1462958336.212006127]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (larm_torso
[ INFO] [1462958336.216803186]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (rarm_torso
[HrpsysSeqStateROSBridge-3] killing on exit
[ INFO] [1462958336.280211841]: [HrpsysSeqStateROSBridge] @onFinalize : HrpsysSeqStateROSBridge0
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Moveit output:

nxouser@nxconsole:~/cws_nxo_244$ roslaunch nextage_moveit_config moveit_planning_execution.launch 
... logging to /home/nxouser/.ros/log/1d54b4d0-175a-11e6-954d-000bab77d794/roslaunch-nxconsole-12858.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://nxconsole:40144/

SUMMARY
========

PARAMETERS
 * /move_group/allow_trajectory_execution: True
 * /move_group/botharms/default_planner_config: RRTConnectkConfig...
 * /move_group/botharms/longest_valid_segment_fraction: 0.05
 * /move_group/botharms/planner_configs: ['SBLkConfigDefau...
 * /move_group/botharms/projection_evaluator: joints(LARM_JOINT...
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'default': True...
 * /move_group/controller_manager_name: pr2_controller_ma...
 * /move_group/controller_manager_ns: pr2_controller_ma...
 * /move_group/head/default_planner_config: RRTConnectkConfig...
 * /move_group/head/kinematics_solver: kdl_kinematics_pl...
 * /move_group/head/kinematics_solver_attempts: 3
 * /move_group/head/kinematics_solver_search_resolution: 0.005
 * /move_group/head/kinematics_solver_timeout: 0.005
 * /move_group/head/longest_valid_segment_fraction: 0.05
 * /move_group/head/planner_configs: ['SBLkConfigDefau...
 * /move_group/head/projection_evaluator: joints(HEAD_JOINT...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/left_arm/default_planner_config: RRTConnectkConfig...
 * /move_group/left_arm/kinematics_solver: kdl_kinematics_pl...
 * /move_group/left_arm/kinematics_solver_attempts: 3
 * /move_group/left_arm/kinematics_solver_search_resolution: 0.005
 * /move_group/left_arm/kinematics_solver_timeout: 0.005
 * /move_group/left_arm/longest_valid_segment_fraction: 0.05
 * /move_group/left_arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/left_arm/projection_evaluator: joints(LARM_JOINT...
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: pr2_moveit_contro...
 * /move_group/moveit_manage_controllers: True
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT
 * /move_group/planning_plugin: ompl_interface/OM...
 * /move_group/planning_scene_monitor/publish_geometry_updates: True
 * /move_group/planning_scene_monitor/publish_planning_scene: True
 * /move_group/planning_scene_monitor/publish_state_updates: True
 * /move_group/planning_scene_monitor/publish_transforms_updates: True
 * /move_group/request_adapters: default_planner_r...
 * /move_group/right_arm/default_planner_config: RRTConnectkConfig...
 * /move_group/right_arm/kinematics_solver: kdl_kinematics_pl...
 * /move_group/right_arm/kinematics_solver_attempts: 3
 * /move_group/right_arm/kinematics_solver_search_resolution: 0.005
 * /move_group/right_arm/kinematics_solver_timeout: 0.005
 * /move_group/right_arm/longest_valid_segment_fraction: 0.05
 * /move_group/right_arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/right_arm/projection_evaluator: joints(RARM_JOINT...
 * /move_group/start_state_max_bounds_error: 0.1
 * /move_group/torso/default_planner_config: RRTConnectkConfig...
 * /move_group/torso/kinematics_solver: kdl_kinematics_pl...
 * /move_group/torso/kinematics_solver_attempts: 3
 * /move_group/torso/kinematics_solver_search_resolution: 0.005
 * /move_group/torso/kinematics_solver_timeout: 0.005
 * /move_group/torso/longest_valid_segment_fraction: 0.05
 * /move_group/torso/planner_configs: ['SBLkConfigDefau...
 * /move_group/torso/projection_evaluator: joints(CHEST_JOINT0)
 * /move_group/upperbody/default_planner_config: RRTConnectkConfig...
 * /move_group/upperbody/longest_valid_segment_fraction: 0.05
 * /move_group/upperbody/planner_configs: ['SBLkConfigDefau...
 * /move_group/upperbody/projection_evaluator: joints(LARM_JOINT...
 * /move_group/use_controller_manager: False
 * /robot_description: <?xml version="1....
 * /robot_description_planning/joint_limits/LARM_JOINT0/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT0/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT0/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT0/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT1/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT1/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT1/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT2/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT2/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT2/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT3/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT3/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT3/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT4/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT4/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT4/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT5/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT5/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT5/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT0/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT0/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT0/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT0/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT1/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT1/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT1/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT2/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT2/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT2/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT3/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT3/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT3/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT4/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT4/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT4/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT5/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT5/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT5/max_velocity: 1.0
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.19
 * /rviz_nxconsole_12858_6887182820923041230/head/kinematics_solver: kdl_kinematics_pl...
 * /rviz_nxconsole_12858_6887182820923041230/head/kinematics_solver_attempts: 3
 * /rviz_nxconsole_12858_6887182820923041230/head/kinematics_solver_search_resolution: 0.005
 * /rviz_nxconsole_12858_6887182820923041230/head/kinematics_solver_timeout: 0.005
 * /rviz_nxconsole_12858_6887182820923041230/left_arm/kinematics_solver: kdl_kinematics_pl...
 * /rviz_nxconsole_12858_6887182820923041230/left_arm/kinematics_solver_attempts: 3
 * /rviz_nxconsole_12858_6887182820923041230/left_arm/kinematics_solver_search_resolution: 0.005
 * /rviz_nxconsole_12858_6887182820923041230/left_arm/kinematics_solver_timeout: 0.005
 * /rviz_nxconsole_12858_6887182820923041230/right_arm/kinematics_solver: kdl_kinematics_pl...
 * /rviz_nxconsole_12858_6887182820923041230/right_arm/kinematics_solver_attempts: 3
 * /rviz_nxconsole_12858_6887182820923041230/right_arm/kinematics_solver_search_resolution: 0.005
 * /rviz_nxconsole_12858_6887182820923041230/right_arm/kinematics_solver_timeout: 0.005
 * /rviz_nxconsole_12858_6887182820923041230/torso/kinematics_solver: kdl_kinematics_pl...
 * /rviz_nxconsole_12858_6887182820923041230/torso/kinematics_solver_attempts: 3
 * /rviz_nxconsole_12858_6887182820923041230/torso/kinematics_solver_search_resolution: 0.005
 * /rviz_nxconsole_12858_6887182820923041230/torso/kinematics_solver_timeout: 0.005

NODES
  /
    move_group (moveit_ros_move_group/move_group)
    rviz_nxconsole_12858_6887182820923041230 (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[move_group-1]: started with pid [12876]
process[rviz_nxconsole_12858_6887182820923041230-2]: started with pid [12877]
[ INFO] [1462958735.671540628]: rviz version 1.11.14
[ INFO] [1462958735.671585696]: compiled against Qt version 4.8.6
[ INFO] [1462958735.671596248]: compiled against OGRE version 1.8.1 (Byatis)
[ INFO] [1462958735.703858469]: Loading robot model 'NextageOpen'...
[ INFO] [1462958735.703913801]: No root joint specified. Assuming fixed joint
[ INFO] [1462958735.938347103]: Stereo is NOT SUPPORTED
[ INFO] [1462958735.938454940]: OpenGl version: 3 (GLSL 1.3).
[ INFO] [1462958736.124796030]: Loading robot model 'NextageOpen'...
[ INFO] [1462958736.124840034]: No root joint specified. Assuming fixed joint
[ INFO] [1462958736.374075205]: Loading robot model 'NextageOpen'...
[ INFO] [1462958736.374105507]: No root joint specified. Assuming fixed joint
[ INFO] [1462958736.622462648]: Loading robot model 'NextageOpen'...
[ INFO] [1462958736.622507294]: No root joint specified. Assuming fixed joint
[ INFO] [1462958736.869005570]: Loading robot model 'NextageOpen'...
[ INFO] [1462958736.869040459]: No root joint specified. Assuming fixed joint
[ERROR] [1462958737.104018599]: Group 'torso' is not a chain
[ERROR] [1462958737.104081376]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'torso'
[ERROR] [1462958737.104279977]: Kinematics solver could not be instantiated for joint group torso.
[ INFO] [1462958737.171480246]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1462958737.174094222]: MoveGroup debug mode is OFF
Starting context monitors...
[ INFO] [1462958737.174126678]: Starting scene monitor
[ INFO] [1462958737.176517290]: Listening to '/planning_scene'
[ INFO] [1462958737.176543065]: Starting world geometry monitor
[ INFO] [1462958737.178850381]: Listening to '/collision_object' using message notifier with target frame '/world '
[ INFO] [1462958737.181421463]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1462958737.181843828]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1462958737.229166596]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1462958737.313913008]: Initializing OMPL interface using ROS parameters
[ INFO] [1462958737.363797802]: Using planning interface 'OMPL'
[ INFO] [1462958737.431162422]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1462958737.431677519]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1462958737.432121151]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1462958737.432537143]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1462958737.432946468]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1462958737.433402957]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1462958737.433444968]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1462958737.433465470]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1462958737.433488599]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1462958737.433503071]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1462958737.433517149]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1462958737.514356726]: Trajectory execution is managing controllers
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteService'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1462958737.664808821]: 

********************************************************
* MoveGroup using: 
*     - CartesianPathService
*     - ExecutePathService
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1462958737.664890150]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1462958737.664909417]: MoveGroup context initialization complete

All is well! Everyone is happy! You can start planning now!

[rviz_nxconsole_12858_6887182820923041230-2] process has died [pid 12877, exit code -11, cmd /opt/ros/indigo/lib/rviz/rviz --fixed-frame WAIST -d /home/nxouser/cws_nxo_244/src/rtmros_hironx/hironx_moveit_config/launch/moveit_viewnatto.rviz __name:=rviz_nxconsole_12858_6887182820923041230 __log:=/home/nxouser/.ros/log/1d54b4d0-175a-11e6-954d-000bab77d794/rviz_nxconsole_12858_6887182820923041230-2.log].
log file: /home/nxouser/.ros/log/1d54b4d0-175a-11e6-954d-000bab77d794/rviz_nxconsole_12858_6887182820923041230-2*.log

emijah commented 8 years ago

I issued a pull request for the HiroNX collada model. However, I need someone to generate the other models from the collada file. The pull request is, start-jsk/rtmros_hironx#461

130s commented 8 years ago

@JKBehrens My bad, with the change I proposed, correct command for your robot should have been:

roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch MODEL_FILE:=/opt/jsk/etc/HIRONX/model/main.wrl CONF_FILE_COLLISIONDETECT:=`rospack find hironx_ros_bridge`/conf/hironx_realrobot_fixedpath.conf

(I know the command is getting longer...we will need to come up with a solution for that but for now please bear with it ;)

NOTE for future reader: the aforementioned command is only for the NEXTAGE Open robot that does not contain /opt/jsk/etc/NEXTAGE folder internal to the ControlBox/QNX. If your robot does contain the folder then you don't need CONF_FILE_COLLISIONDETECT arg.

JKBehrens commented 8 years ago

@130s Now, the Rosbridge is coming up. Furthermore, from rosrun tf tf_echo /RARM_JOINT4_Link /RARM_JOINT5_Link I get

- Translation: [-0.047, 0.000, -0.090]
- Rotation: in Quaternion [-0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [-0.000, 0.000, 0.000]
            in RPY (degree) [-0.008, 0.000, 0.000]

, which is now consistent with the URDF file. Thank you all!!!

Can I now rely on the equivalence of the models or did you just fix the pointed differences? Would it be useful to update the software and files on the QNX?

130s commented 8 years ago

Glad that the changes worked for you @JKBehrens !

Can I now rely on the equivalence of the models or did you just fix the pointed differences? Would it be useful to update the software and files on the QNX?

So for NEXTAGE Open robot (which you are using), there's no need to worry for incorrect model files as far as we know -- the root cause of this ticketed issue is that the software of NEXTAGE Open was referring to the model of Hironx robot (former version of NEXTAGE) where there seems to be a slight geometric difference (patch https://github.com/start-jsk/rtmros_hironx/pull/461).

I don't think there's a need for you (and all NEXTAGE Open users) to update the files on QNX, in terms of this issue.

If there's still anythiing unclear etc. feel free to reopen.

130s commented 8 years ago

BTW, the changes made for this ticket will soon be available via DEB binary with the following versions (or greater):

rtmros_hironx 1.1.13
rtmros_nextage 0.7.7

tork-a / rtmros_nextage

URDF of Nextage Robot wrong #244

SUMMARY