[nextage_tutorials] Add current setup of jsk Nextage Open.

hironx_tutorial/config/hironxjsk_image_transport_plugins_params.yaml

@@ -51,6 +51,19 @@ head_camera:
           - 'image_transport/theora'
         compressedDepth:
           png_level: 5
+    hw_registered:
+      image_rect:
+        disable_pub_plugins:
+          - 'image_transport/compressed'
+          - 'image_transport/theora'
+        compressedDepth:
+          png_level: 5
+      image_rect_raw:
+        disable_pub_plugins:
+          - 'image_transport/compressed'
+          - 'image_transport/theora'
+        compressedDepth:
+          png_level: 5
   ir:
     image:
       disable_pub_plugins:

hironx_tutorial/launch/hironxjsk_real.launch

@@ -5,6 +5,7 @@
   <!-- Start Camera -->
   <include file="$(find openni2_launch)/launch/openni2.launch">
     <arg name="camera" value="head_camera" />
+    <arg name="depth_registration" value="true" />
     <arg name="publish_tf" value="false" />
   </include>
   <!-- Set image_transport_plugins parameters (png_level and disable_pub_plugins) -->

hrpsys_ros_bridge_tutorials/CMakeLists.txt

@@ -444,6 +444,14 @@ compile_rbrain_model_for_closed_robots(TQLEG0 TQLEG0 TQLEG0
   --conf-file-option "seq_optional_data_dim: 4"
   )
 
+# A0B
+compile_rbrain_model_for_closed_robots(A0B A0B A0B
+  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
+  --conf-file-option "end_effectors: rarm,RARM_JOINT5,WAIST,0.25375,0.0,0.0,0.0,0.0,0.0,0.0,"
+  --conf-dt-option "0.002"
+  --simulation-timestep-option "0.002"
+  )
+
 if(EXISTS ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl)
   compile_openhrp_model(
     ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl
@@ -502,6 +510,7 @@ generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(URATALEG
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(YSTLEG YSTLEG "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(CHIDORI CHIDORI "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TQLEG0 TQLEG0 "--use-robot-hrpsys-config")
+generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(A0B A0B "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TABLIS TABLIS "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files(
   "$(find hrpsys_ros_bridge_tutorials)/models/TESTMDOFARM.wrl"
@@ -521,8 +530,10 @@ generate_default_launch_eusinterface_files("$(find openhrp3)/share/OpenHRP-3.1/s
 find_package(xacro)
 if(EXISTS ${xacro_SOURCE_PREFIX}/xacro.py)
   set(xacro_exe ${xacro_SOURCE_PREFIX}/xacro.py)
-else()
+elseif(EXISTS ${xacro_PREFIX}/share/xacro/xacro.py)
   set(xacro_exe ${xacro_PREFIX}/share/xacro/xacro.py)
+else()
+  set(xacro_exe ${xacro_PREFIX}/bin/xacro) # xacro.py is deprecated in Noetic
 endif()
 
 find_package(euscollada)
@@ -532,6 +543,14 @@ else ()
   set(euscollada_PACKAGE_PATH ${euscollada_PREFIX}/share/euscollada)
 endif()
 
+# enable to execute with correct python version
+# cf. https://github.com/tork-a/openrtm_aist_python-release/pull/5
+find_package(PythonInterp QUIET)
+if(NOT PYTHONINTERP_FOUND)
+  find_package(Python REQUIRED)
+  set(PYTHON_EXECUTABLE ${Python_EXECUTABLE})
+endif()
+
 macro (generate_hand_attached_hrp2_model _robot_name)
   set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
   set(_in_urdf_file "${_model_dir}/${_robot_name}.urdf")
@@ -540,7 +559,7 @@ macro (generate_hand_attached_hrp2_model _robot_name)
   string(TOLOWER ${_robot_name} _srobot_name)
   message("generate hand_attached_hrp2_model for ${_robot_name}")
   add_custom_command(OUTPUT ${_out_urdf_file}
-      COMMAND ${_script_file}
+      COMMAND ${PYTHON_EXECUTABLE} ${_script_file}
       LARM_LINK6 RARM_LINK6 ${_in_urdf_file} ${_out_urdf_file}
       DEPENDS ${_in_urdf_file} ${_script_file} ${_srobot_name}_${PROJECT_NAME}_compile_urdf)
 endmacro()
@@ -566,7 +585,7 @@ macro (attach_sensor_and_endeffector_to_hrp2jsk_urdf
   set(_out_urdf_file "${_model_dir}/${_out_file}")
   set(_script_file ${euscollada_PACKAGE_PATH}/scripts/add_sensor_to_collada.py)
   add_custom_command(OUTPUT ${_out_urdf_file}
-    COMMAND ${_script_file}
+    COMMAND ${PYTHON_EXECUTABLE} ${_script_file}
     ${_in_urdf_file} -O ${_out_urdf_file} -C ${_in_yaml_file}
     DEPENDS ${_in_urdf_file} ${_in_yaml_file} ${_script_file})
   list(APPEND compile_urdf_robots ${_out_urdf_file})

hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l

@@ -0,0 +1,15 @@
+(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l")
+(require :a0b "package://hrpsys_ros_bridge_tutorials/models/a0b.l")
+
+(defclass a0b-interface
+  :super rtm-ros-robot-interface
+  :slots ())
+(defmethod a0b-interface
+  (:init (&rest args &key ((:controller-timeout ct) nil))
+         (send-super* :init :robot a0b-robot :controller-timeout ct args)))
+
+(defun a0b-init (&rest args)
+  (if (not (boundp '*ri*))
+      (setq *ri* (instance* a0b-interface :init args)))
+  (if (not (boundp '*a0b*))
+      (setq *a0b* (instance a0b-robot :init))))

hrpsys_ros_bridge_tutorials/models/a0b.yaml

@@ -0,0 +1,25 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+  - RARM_JOINT0   : rarm-joint0
+  - RARM_JOINT1   : rarm-joint1
+  - RARM_JOINT2   : rarm-joint2
+  - RARM_JOINT3   : rarm-joint3
+  - RARM_JOINT4   : rarm-joint4
+  - RARM_JOINT5   : rarm-joint5
+
+##
+## end-coords
+##
+rarm-end-coords:
+  translate : [0.25375, 0, 0]
+
+##
+## reset-pose
+##
+angle-vector:
+  zero-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+  reset-pose : [60.0, -90.0, 90.0, -30.0, -90.0, 90.0]
+  reverse-reset-pose : [60.0, 90.0, -90.0, -30.0, 90.0, -90.0]

hrpsys_ros_bridge_tutorials/models/gen_hand_attached_staro_model.sh

@@ -10,7 +10,18 @@ INPUT_FILE=$2
 EUSCOLLADA_PATH=$3
 BODY_FILE=${INPUT_FILE//.urdf/_body.urdf}
 
+# enable to execute with correct python version
+# cf. https://github.com/tork-a/openrtm_aist_python-release/pull/5
+if command -v python3 &>/dev/null; then
+    PYTHON_EXECUTABLE=$(command -v python3)
+elif command -v python &>/dev/null; then
+    PYTHON_EXECUTABLE=$(command -v python)
+else
+    echo "Cannot find python executable"
+    exit 1
+fi
+
 tmp1=`mktemp`
 tmp2=`mktemp`
-${EUSCOLLADA_PATH}/scripts/remove_sensor_from_urdf.py LARM_LINK7 $INPUT_FILE $tmp1
-${EUSCOLLADA_PATH}/scripts/remove_sensor_from_urdf.py RARM_LINK7 $tmp1 $BODY_FILE
+${PYTHON_EXECUTABLE} ${EUSCOLLADA_PATH}/scripts/remove_sensor_from_urdf.py LARM_LINK7 $INPUT_FILE $tmp1
+${PYTHON_EXECUTABLE} ${EUSCOLLADA_PATH}/scripts/remove_sensor_from_urdf.py RARM_LINK7 $tmp1 $BODY_FILE

hrpsys_ros_bridge_tutorials/models/gen_sensor_attached_staro_model.sh

@@ -9,11 +9,22 @@ input_urdf=$3
 output_urdf=$4
 yaml_file=$5
 
+# enable to execute with correct python version
+# cf. https://github.com/tork-a/openrtm_aist_python-release/pull/5
+if command -v python3 &>/dev/null; then
+    PYTHON_EXECUTABLE=$(command -v python3)
+elif command -v python &>/dev/null; then
+    PYTHON_EXECUTABLE=$(command -v python)
+else
+    echo "Cannot find python executable"
+    exit 1
+fi
+
 # generating model with sensors
 function add_sensor_to_tmp_urdf()
 {
     tmp_model=`mktemp`
-    ${eusurdf_path}/scripts/add_sensor_to_urdf.py $@ $tmp_model
+    ${PYTHON_EXECUTABLE} ${eusurdf_path}/scripts/add_sensor_to_urdf.py $@ $tmp_model
     if [ $? == 0 ]; then
         echo $tmp_model
     else
@@ -24,7 +35,7 @@ function add_sensor_to_tmp_urdf()
 function add_eef_to_tmp_urdf()
 {
     tmp_model=`mktemp`
-    ${eusurdf_path}/scripts/add_eef_to_urdf.py $@ $tmp_model
+    ${PYTHON_EXECUTABLE} ${eusurdf_path}/scripts/add_eef_to_urdf.py $@ $tmp_model
     if [ $? == 0 ]; then
         echo $tmp_model
     else
@@ -59,7 +70,7 @@ function generate_staro_model()
 #     # tmp_model11=$(add_sensor_to_tmp_urdf 0 0.2 -0.2 0 0 1.57 LARM_LINK6 LARM_cb_jig $tmp_model10)
 #     # tmp_model12=$(add_sensor_to_tmp_urdf 0 -0.2 -0.2 0 0 1.57 RARM_LINK6 RARM_cb_jig $tmp_model11)
 #     cp $tmp_model4 $2
-    ${eusurdf_path}/scripts/add_sensor_to_collada.py $1 -O $2 -C $3
+    ${PYTHON_EXECUTABLE} ${eusurdf_path}/scripts/add_sensor_to_collada.py $1 -O $2 -C $3
 }
 
 generate_staro_model $input_urdf $output_urdf $yaml_file

hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py

@@ -22,7 +22,7 @@ def fixed_writexml(self, writer, indent="", addindent="", newl=""):
     writer.write(indent+"<" + self.tagName)
 
     attrs = self._get_attributes()
-    a_names = attrs.keys()
+    a_names = list(attrs.keys())
     a_names.sort()
 
     for a_name in a_names:
@@ -118,9 +118,9 @@ def add_object_to_projectfile(objname, url, robot=False, add_collision=True, tra
     argc = len(argvs)
 
     if argc < 4:
-        print '### usage   : <in>.xml <out>.xml objname,url,is_robot_q,add_collision_q,0,0,0,0,0,0,0'
-        print '### example : sample_in.xml sample_out.xml  table,url_of_table,True,True,0.0,0.0,1.0,0.0,0.0,1.0,90.0'
-        print '###                             string,string      ,bool,bool,trans<3>   ,rotation<4>'
+        print('### usage   : <in>.xml <out>.xml objname,url,is_robot_q,add_collision_q,0,0,0,0,0,0,0')
+        print('### example : sample_in.xml sample_out.xml  table,url_of_table,True,True,0.0,0.0,1.0,0.0,0.0,1.0,90.0')
+        print('###                             string,string      ,bool,bool,trans<3>   ,rotation<4>')
         exit(0)
 
     infile = argvs[1]

hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py

@@ -13,7 +13,7 @@ def publishEndEffectorOne(eef_name, eef_info, stamp):
                      stamp, eef_name, eef_info['parent'])
 
 def publishEndEffectorAll():
-    for limb_name, eef_info in g_eef_info_list.items():
+    for limb_name, eef_info in list(g_eef_info_list.items()):
         publishEndEffectorOne(limb_name+"_end_coords", eef_info, rospy.Time.now())
 
 if __name__ == "__main__":
@@ -33,7 +33,7 @@ def publishEndEffectorAll():
         parent_link_name_next = parent_link_name
         while True:
             parent_link_name = parent_link_name_next
-            if robot_model.child_map.has_key(parent_link_name):
+            if parent_link_name in robot_model.child_map:
                 parent_link_name_next = robot_model.child_map[parent_link_name][0][1]
                 if not (parent_link_name_next.startswith(limb.upper()) or parent_link_name_next.startswith(limb.lower())):
                     break
@@ -43,16 +43,16 @@ def publishEndEffectorAll():
         param_name = '~'+limb+'-end-coords'
         if rospy.has_param(param_name):
             g_eef_info_list[limb] = rospy.get_param(param_name)
-            if not g_eef_info_list[limb].has_key('translate'):
+            if 'translate' not in g_eef_info_list[limb]:
                 g_eef_info_list[limb]['translate'] = [0, 0, 0, 0]
-            if not g_eef_info_list[limb].has_key('rotate'):
+            if 'rotate' not in g_eef_info_list[limb]:
                 g_eef_info_list[limb]['rotate'] = [0, 0, 0, 0]
-            if not g_eef_info_list[limb].has_key('parent'):
+            if 'parent' not in g_eef_info_list[limb]:
                 g_eef_info_list[limb]['parent'] = parent_link_name
     # print for debug
-    print 'eef_infos param list:'
-    for limb, eef_info in g_eef_info_list.items():
-        print '%s: %s' % (limb, eef_info)
+    print('eef_infos param list:')
+    for limb, eef_info in list(g_eef_info_list.items()):
+        print('%s: %s' % (limb, eef_info))
 
     while not rospy.is_shutdown():
         publishEndEffectorAll()

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/a0b_hrpsys_config.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from urata_hrpsys_config import *
+
+if __name__ == '__main__':
+    hcf = URATAHrpsysConfigurator("A0B")
+    if len(sys.argv) > 2 :
+        hcf.init(sys.argv[1], sys.argv[2])
+    elif len(sys.argv) > 1 :
+        hcf.init(sys.argv[1])
+    else :
+        hcf.init()

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py

@@ -14,7 +14,7 @@ def getRTCList (self):
         return self.getRTCListUnstable()
     def init (self, robotname="Robot", url=""):
         HrpsysConfigurator.init(self, robotname, url)
-        print "initialize rtc parameters"
+        print("initialize rtc parameters")
         self.setStAbcParameters()
         self.loadForceMomentOffsetFile()
 
@@ -122,7 +122,7 @@ def setStAbcParametershrp2017c(self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         #   tpcc st params
         stp.k_tpcc_p=[2.0, 2.0]
         stp.k_tpcc_x=[5.0, 5.0]
@@ -219,7 +219,7 @@ def setStAbcParametershrp2016c (self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         #   tpcc st params
         stp.k_tpcc_p=[2.0, 2.0]
         stp.k_tpcc_x=[5.0, 5.0]
@@ -316,7 +316,7 @@ def setStAbcParametershrp2007c (self):
                          OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
         rarm_vertices = rleg_vertices
         larm_vertices = lleg_vertices
-        stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices])
+        stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]]
         #   tpcc st params
         stp.k_tpcc_p=[2.0, 2.0]
         stp.k_tpcc_x=[5.0, 5.0]
@@ -380,7 +380,7 @@ def loadForceMomentOffsetFile (self):
         elif self.ROBOT_NAME == "HRP2JSK":
             self.rmfo_svc.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_thumb_60deg_HRP2JSK")
         else:
-            print "No force moment offset file"
+            print("No force moment offset file")
 
     def __init__(self, robotname=""):
         self.ROBOT_NAME = robotname

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py

@@ -12,7 +12,7 @@ def getRTCList (self):
         return self.getRTCListUnstable()
     def init (self, robotname="SampleRobot", url=""):
         HrpsysConfigurator.init(self, robotname, url)
-        print "initialize rtc parameters"
+        print("initialize rtc parameters")
         self.setStAbcParameters()
 
     def defJointGroups (self):