diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index a1e5b88f..594a5e3e 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -530,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) diff --git a/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py b/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py index d01fd43f..b94e2bf8 100755 --- a/hrpsys_ros_bridge_tutorials/scripts/add-obj-to-hrpproject.py +++ b/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 : .xml .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 : .xml .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] diff --git a/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py b/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py index f66c1448..9298238a 100755 --- a/hrpsys_ros_bridge_tutorials/scripts/publish_end_effector_tf.py +++ b/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() diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 3bf0d267..16816e77 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/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 diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py index 96a19b26..e31b3d70 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/samplerobot_hrpsys_config.py +++ b/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): diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index a64c7eb8..4114a66f 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -144,7 +144,7 @@ def setStAbcParametersSTARO (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]] stp.eefm_zmp_delay_time_const=[0.055, 0.055] #stp.eefm_cogvel_cutoff_freq = 3.18 #stp.eefm_cogvel_cutoff_freq = 6.18 # servooff+walk @@ -280,7 +280,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_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]] stp.eefm_cogvel_cutoff_freq = 4.0 stp.eefm_k1=[-1.48412,-1.48412] stp.eefm_k2=[-0.486727,-0.486727] @@ -385,7 +385,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_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]] stp.eefm_cogvel_cutoff_freq = 4.0 # for only leg stp.eefm_k1=[-1.36334,-1.36334] @@ -472,7 +472,7 @@ def setStAbcParametersURATALEG (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]] stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.25 stp.eefm_k1=[-1.38077, -1.38077] @@ -481,7 +481,7 @@ def setStAbcParametersURATALEG (self): self.st_svc.setParameter(stp) def setStAbcParametersYSTLEG (self): - print "Not implemented yet" + print("Not implemented yet") def setStAbcParametersCHIDORI (self): # abc setting @@ -545,7 +545,7 @@ def setStAbcParametersCHIDORI (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]] # stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.0 stp.eefm_k1=[-1.48412,-1.48412] @@ -624,7 +624,7 @@ def setStAbcParametersTQLEG0 (self): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [rleg_vertices, lleg_vertices]] stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.25 stp.eefm_k1=[-1.38077, -1.38077] @@ -688,7 +688,7 @@ def setStAbcParametersTABLIS(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]] # stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.0 # calculated by calculate-eefm-st-state-feedback-default-gain-from-robot *chidori* @@ -903,4 +903,4 @@ def loadForceMomentOffsetFile (self): elif self.ROBOT_NAME == "JAXON_RED": self.rmfo_svc.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_JAXON_RED") else: - print "No force moment offset file" + print("No force moment offset file")