From 1c88646655bbe1ae6d0351bd2e4bc9cabb3d4fbe Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:24:23 +0900 Subject: [PATCH 01/26] add setup.py, add catkin_package_setup(), use sysconfig.get_config_var instead of sys.veresion_info --- CMakeLists.txt | 3 ++- setup.py | 9 +++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) create mode 100644 setup.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 59f2933b317..70f6c5f7988 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -169,7 +169,7 @@ if(USE_HRPSYSUTIL AND APPLE AND NOT PCL_FOUND) endif() execute_process( - COMMAND python -c "from distutils import sysconfig; print sysconfig.get_config_var(\"VERSION\")" + COMMAND python -c "import sys; print(sys.version_info[0])" OUTPUT_VARIABLE PYTHON_VERSION RESULT_VARIABLE PYTHON_VERSION_SUCCESS OUTPUT_STRIP_TRAILING_WHITESPACE) @@ -225,6 +225,7 @@ endif() string(REGEX MATCH "catkin" need_catkin "$ENV{_}") if(need_catkin OR "${CATKIN_BUILD_BINARY_PACKAGE}") find_package(catkin) + catkin_python_setup() if(catkin_FOUND) catkin_package_xml() if(NOT hrpsys_VERSION VERSION_EQUAL CPACK_PACKAGE_VERSION) diff --git a/setup.py b/setup.py new file mode 100644 index 00000000000..7b4e92adff0 --- /dev/null +++ b/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['hrpsys'], + package_dir={'': 'src'}, + ) +setup(**d) + From 08fe7213ec3a4f5216bbad5f6837abc5aec9838b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:30:53 +0900 Subject: [PATCH 02/26] mv python -> src/hrpsys for setup.py --- {python => src/hrpsys}/CMakeLists.txt | 0 {python => src/hrpsys}/__init__.py | 0 {python => src/hrpsys}/hrpsys_config.py | 0 {python => src/hrpsys}/hrpsyspy | 0 {python => src/hrpsys}/rtm.py | 0 {python => src/hrpsys}/waitInput.py | 0 6 files changed, 0 insertions(+), 0 deletions(-) rename {python => src/hrpsys}/CMakeLists.txt (100%) rename {python => src/hrpsys}/__init__.py (100%) rename {python => src/hrpsys}/hrpsys_config.py (100%) rename {python => src/hrpsys}/hrpsyspy (100%) rename {python => src/hrpsys}/rtm.py (100%) rename {python => src/hrpsys}/waitInput.py (100%) diff --git a/python/CMakeLists.txt b/src/hrpsys/CMakeLists.txt similarity index 100% rename from python/CMakeLists.txt rename to src/hrpsys/CMakeLists.txt diff --git a/python/__init__.py b/src/hrpsys/__init__.py similarity index 100% rename from python/__init__.py rename to src/hrpsys/__init__.py diff --git a/python/hrpsys_config.py b/src/hrpsys/hrpsys_config.py similarity index 100% rename from python/hrpsys_config.py rename to src/hrpsys/hrpsys_config.py diff --git a/python/hrpsyspy b/src/hrpsys/hrpsyspy similarity index 100% rename from python/hrpsyspy rename to src/hrpsys/hrpsyspy diff --git a/python/rtm.py b/src/hrpsys/rtm.py similarity index 100% rename from python/rtm.py rename to src/hrpsys/rtm.py diff --git a/python/waitInput.py b/src/hrpsys/waitInput.py similarity index 100% rename from python/waitInput.py rename to src/hrpsys/waitInput.py From 733433b2617ba660161e3840719a6f803c47882a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:46:03 +0900 Subject: [PATCH 03/26] CMakeLists.txt: rename add_subdirectory(python) -> add_subdirectory(src/hrpsys) --- CMakeLists.txt | 2 +- doc/CMakeLists.txt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 70f6c5f7988..ed670dfa773 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -204,7 +204,7 @@ message(STATUS "compile iob with -DROBOT_IOB_VERSION=${ROBOT_IOB_VERSION}") if(COMPILE_JAVA_STUFF) add_subdirectory(jython) endif() -add_subdirectory(python) +add_subdirectory(src/hrpsys) # for doc/html/index.html add_subdirectory(idl) add_subdirectory(lib) add_subdirectory(ec) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 989740266d1..868940aebda 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -1,6 +1,6 @@ set(input_files package.h utilities.h - ../python/hrpsys_config.py - ../python/rtm.py + ../src/hrpsys/hrpsys_config.py + ../src/hrpsys/rtm.py ../rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.txt ../rtc/CameraImageViewer/CameraImageViewer.txt ../rtc/CameraImageSaver/CameraImageSaver.txt From 4d88f4e4dddaab5bcd237ebf448fb6ec400b53ee Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:24:52 +0900 Subject: [PATCH 04/26] doc/CMakeLists.txt: avoid target named doc --- doc/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 868940aebda..82d88203cd9 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -70,8 +70,8 @@ add_custom_command( DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile ${input_files} ) -add_custom_target(doc ALL DEPENDS ${output_dir}/index.html) -add_custom_target(doc ALL DEPENDS ${output_dir_xml}/index.xml) +add_custom_target(doc_index_html ALL DEPENDS ${output_dir}/index.html) +add_custom_target(doc_index_xml ALL DEPENDS ${output_dir_xml}/index.xml) install(DIRECTORY ${output_dir} DESTINATION share/doc/hrpsys/base) install(DIRECTORY ${output_dir_xml} DESTINATION share/doc/hrpsys/base) From d50a65a9ea94c5a0a2454edcd46b721dd27d1fe9 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:29:19 +0900 Subject: [PATCH 05/26] apply 2to3 -w -f print --- jython/poseEditor.py | 2 +- jython/rtm.py | 40 ++--- launch/samplerobot-terrain-walk.py | 8 +- rtc/OccupancyGridMap3D/sample/sample.py | 8 +- sample/HRP4C/HRP4C.py | 8 +- sample/PA10/PA10.py | 8 +- .../sample4legrobot_auto_balancer.py | 12 +- .../sample4legrobot_stabilizer.py | 8 +- .../SampleRobot/samplerobot_auto_balancer.py | 156 +++++++++--------- .../SampleRobot/samplerobot_carry_object.py | 40 ++--- .../samplerobot_collision_detector.py | 50 +++--- sample/SampleRobot/samplerobot_data_logger.py | 14 +- .../samplerobot_emergency_stopper.py | 64 +++---- .../samplerobot_impedance_controller.py | 48 +++--- .../SampleRobot/samplerobot_kalman_filter.py | 14 +- .../samplerobot_reference_force_updater.py | 58 +++---- .../samplerobot_remove_force_offset.py | 48 +++--- .../samplerobot_sequence_player.py | 80 ++++----- .../samplerobot_soft_error_limiter.py | 42 ++--- sample/SampleRobot/samplerobot_stabilizer.py | 92 +++++------ .../SampleRobot/samplerobot_terrain_walk.py | 12 +- .../samplerobot_virtual_force_sensor.py | 16 +- sample/SampleRobot/samplerobot_walk.py | 2 +- .../samplespecialjointrobot_auto_balancer.py | 18 +- sample/simTest1.py | 2 +- sample/simTest2.py | 4 +- sample/vcTest.py | 2 +- sample/visionTest.py | 2 +- src/hrpsys/rtm.py | 78 ++++----- test/test-hostname.py | 10 +- test/test-jointangle.py | 4 +- test/test-pkgconfig.py | 4 +- test/test-robot-hardware.py | 4 +- test/test-samplerobot.py | 2 +- 34 files changed, 480 insertions(+), 480 deletions(-) diff --git a/jython/poseEditor.py b/jython/poseEditor.py index aed83af1c44..3e82094f8d0 100644 --- a/jython/poseEditor.py +++ b/jython/poseEditor.py @@ -65,7 +65,7 @@ def __init__(self, url, seq, sh): for li in self.bodyInfo.links(): if li.jointId >= 0: self.dof += 1 - print "dof=",self.dof + print("dof=",self.dof) self.setSize(550, 800) self.seq = seq self.sh = sh diff --git a/jython/rtm.py b/jython/rtm.py index f51e3744684..7edb1111bcf 100644 --- a/jython/rtm.py +++ b/jython/rtm.py @@ -93,7 +93,7 @@ def getProperty(self, name): cfg = self.ref.get_configuration() cfgsets = cfg.get_configuration_sets() if len(cfgsets) == 0: - print "configuration set is not found" + print("configuration set is not found") return None cfgset = cfgsets[0] for d in cfgset.configuration_data: @@ -108,11 +108,11 @@ def properties(self): cfg = self.ref.get_configuration() cfgsets = cfg.get_configuration_sets() if len(cfgsets) == 0: - print "configuration set is not found" + print("configuration set is not found") return cfgset = cfgsets[0] for d in cfgset.configuration_data: - print d.name,":",d.value.extract_string() + print(d.name,":",d.value.extract_string()) ## @@ -204,7 +204,7 @@ def load(self, basename): try: self.ref.load_module(path, initfunc) except: - print "failed to load",path + print("failed to load",path) ## # \brief create an instance of RT component @@ -216,7 +216,7 @@ def create(self, module,name=None): if name != None: rtc = findRTC(name) if rtc != None: - print 'RTC named "',name,'" already exists.' + print('RTC named "',name,'" already exists.') return rtc args = module if name != None: @@ -277,7 +277,7 @@ def initCORBA(): nshost = System.getProperty("NS_OPT").split(':')[2] if nshost == "localhost" or nshost == "127.0.0.1": nshost = socket.gethostname() - print 'nshost =',nshost + print('nshost =',nshost) orb = ORB.init(args, props) nameserver = orb.resolve_initial_references("NameService"); @@ -330,7 +330,7 @@ def findRTCmanager(hostname=None, rnc=None): obj = findObject("manager","mgr",cxt) return RTCmanager(ManagerHelper.narrow(obj)) except: - print "exception in findRTCmanager("+hostname+")" + print("exception in findRTCmanager("+hostname+")") ## # \brief get RT component @@ -381,9 +381,9 @@ def serializeComponents(rtcs, stopEC=True): if ec.add_component(rtc.ref) == ReturnCode_t.RTC_OK: rtc.ec = ec else: - print 'error in add_component()' + print('error in add_component()') else: - print 'already serialized' + print('already serialized') ## # \brief check two ports are connected or not @@ -435,10 +435,10 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength inPs = [inPs] for inP in inPs: if isConnected(outP, inP) == True: - print outP.get_port_profile().name,'and',inP.get_port_profile().name,'are already connected' + print(outP.get_port_profile().name,'and',inP.get_port_profile().name,'are already connected') continue if dataTypeOfPort(outP) != dataTypeOfPort(inP): - print outP.get_port_profile().name,'and',inP.get_port_profile().name,'have different data types' + print(outP.get_port_profile().name,'and',inP.get_port_profile().name,'have different data types') continue con_prof = ConnectorProfile() @@ -480,11 +480,11 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength con_prof_holder = ConnectorProfileHolder() con_prof_holder.value = con_prof if inP.connect(con_prof_holder) != ReturnCode_t.RTC_OK: - print "failed to connect(",outP.get_port_profile().name,'<->',inP.get_port_profile().name,")" + print("failed to connect(",outP.get_port_profile().name,'<->',inP.get_port_profile().name,")") continue # confirm connection if isConnected(outP, inP) == False: - print "connet() returned RTC_OK, but not connected" + print("connet() returned RTC_OK, but not connected") ## # \brief convert data into CDR format @@ -551,7 +551,7 @@ def writeDataPort(port, data, tm=1.0): con_prof_holder = ConnectorProfileHolder() con_prof_holder.value = con_prof if port.connect(con_prof_holder) != ReturnCode_t.RTC_OK: - print "failed to connect" + print("failed to connect") return None for p in con_prof_holder.value.properties: if p.name == 'dataport.corba_cdr.inport_ior': @@ -560,7 +560,7 @@ def writeDataPort(port, data, tm=1.0): inport = InPortCdrHelper.narrow(obj) cdr = data2cdr(data) if inport.put(cdr) != PortStatus.PORT_OK: - print "failed to put" + print("failed to put") time.sleep(tm) port.disconnect(con_prof_holder.value.connector_id) return None @@ -604,7 +604,7 @@ def readDataPort(port, timeout = 1.0): con_prof_holder = ConnectorProfileHolder() con_prof_holder.value = con_prof if port.connect(con_prof_holder) != ReturnCode_t.RTC_OK: - print "failed to connect" + print("failed to connect") return None for p in con_prof_holder.value.properties: #print p.name @@ -646,7 +646,7 @@ def findService(rtc, port_name, type_name, instance_name): else: p = rtc.port(port_name) if p == None: - print "can't find a port named",port_name + print("can't find a port named",port_name) return None else: port_prof = [p.get_port_profile()] @@ -660,7 +660,7 @@ def findService(rtc, port_name, type_name, instance_name): if aif.instance_name == instance_name and (type_name == "" or aif.type_name == type_name): port = pp.port_ref if port == None: - print "can't find a service named",instance_name + print("can't find a service named",instance_name) return None #print port con_prof = ConnectorProfile() @@ -684,7 +684,7 @@ def setConfiguration(rtc, nvlist): cfg = rtc.get_configuration() cfgsets = cfg.get_configuration_sets() if len(cfgsets) == 0: - print "configuration set is not found" + print("configuration set is not found") return cfgset = cfgsets[0] for nv in nvlist: @@ -698,7 +698,7 @@ def setConfiguration(rtc, nvlist): found = True break; if not found: - print "no such property(",name,")" + print("no such property(",name,")") cfg.activate_configuration_set('default') ## diff --git a/launch/samplerobot-terrain-walk.py b/launch/samplerobot-terrain-walk.py index b01ff8c22ef..ae168e1157f 100755 --- a/launch/samplerobot-terrain-walk.py +++ b/launch/samplerobot-terrain-walk.py @@ -28,8 +28,8 @@ samplerobot_terrain_walk.demo() import sys if len(sys.argv) != 2: - print "Usage:" - print " ",sys.argv[0]," --StairUp or --StairDown or --SlopeUpDown" + print("Usage:") + print(" ",sys.argv[0]," --StairUp or --StairDown or --SlopeUpDown") else: if sys.argv[1] == "--StairUp": samplerobot_terrain_walk.demoStairUp() @@ -38,8 +38,8 @@ elif sys.argv[1] == "--SlopeUpDown": samplerobot_terrain_walk.demoSlopeUpDown() else: - print "Usage:" - print " ",sys.argv[0]," --StairUp or --StairDown or --SlopeUpDown" + print("Usage:") + print(" ",sys.argv[0]," --StairUp or --StairDown or --SlopeUpDown") ## IGNORE ME: this code used for rostest diff --git a/rtc/OccupancyGridMap3D/sample/sample.py b/rtc/OccupancyGridMap3D/sample/sample.py index 6e594a3275e..900e199c01b 100644 --- a/rtc/OccupancyGridMap3D/sample/sample.py +++ b/rtc/OccupancyGridMap3D/sample/sample.py @@ -16,12 +16,12 @@ aabb.size.w = 4.0 aabb.size.h = 0.1 map = ogm_svc.getOGMap3D(aabb) -print "resolution = ",map.resolution -print "number of voxels = ",map.nx,"x",map.ny,"x",map.nz +print("resolution = ",map.resolution) +print("number of voxels = ",map.nx,"x",map.ny,"x",map.nz) for x in range(map.nx): for y in range(map.ny): for z in range(map.nz): - print map.cells[x*map.ny*map.nz+y*map.nz+z], - print + print(map.cells[x*map.ny*map.nz+y*map.nz+z], end=' ') + print() diff --git a/sample/HRP4C/HRP4C.py b/sample/HRP4C/HRP4C.py index 2a5fbc9237a..8e2996eaff0 100644 --- a/sample/HRP4C/HRP4C.py +++ b/sample/HRP4C/HRP4C.py @@ -29,15 +29,15 @@ def init(): ms = rtm.findRTCmanager() - print "creating components" + print("creating components") createComps() - print "connecting components" + print("connecting components") connectComps() - print "activating components" + print("activating components") activateComps() - print "initialized successfully" + print("initialized successfully") def loadPattern(basename, tm=1.0): seq_svc.loadPattern(basename, tm) diff --git a/sample/PA10/PA10.py b/sample/PA10/PA10.py index df44b4b9baf..66d7242bf75 100644 --- a/sample/PA10/PA10.py +++ b/sample/PA10/PA10.py @@ -58,15 +58,15 @@ def init(): ms = rtm.findRTCmanager() - print "creating components" + print("creating components") createComps() - print "connecting components" + print("connecting components") connectComps() - print "activating components" + print("activating components") activateComps() - print "initialized successfully" + print("initialized successfully") def goInitial(tm=3.0): seq_svc.setJointAngles([0]*9, tm) diff --git a/sample/Sample4LegRobot/sample4legrobot_auto_balancer.py b/sample/Sample4LegRobot/sample4legrobot_auto_balancer.py index a5acc69583e..ee80be86e0f 100755 --- a/sample/Sample4LegRobot/sample4legrobot_auto_balancer.py +++ b/sample/Sample4LegRobot/sample4legrobot_auto_balancer.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -42,10 +42,10 @@ def init (): hcf.abc_svc.setGaitGeneratorParam(ggp) hcf.startAutoBalancer(limbs=['rleg','lleg','rarm','larm']) hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) def demoGaitGeneratorSetFootSteps(print_str="1. setFootSteps"): - print >> sys.stderr, print_str + print(print_str, file=sys.stderr) hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.00,-0.19,0], [1,0,0,0], "rleg"), OpenHRP.AutoBalancerService.Footstep([0.7+0.00,+0.19,0], [1,0,0,0], "larm")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.15,+0.19,0], [1,0,0,0], "lleg"), @@ -74,7 +74,7 @@ def demoGaitGeneratorSetFootStepsCycloidDelay(): demoGaitGeneratorSetFootSteps("3. setFootSteps with Cycloiddelay orbit"); def demoGaitGeneratorSetFootStepsCrawl(print_str="4. setFootSteps in Crawl"): - print >> sys.stderr, print_str + print(print_str, file=sys.stderr) hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.00,-0.19,0], [1,0,0,0], "rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.7+0.01,+0.19,0], [1,0,0,0], "larm")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.01,+0.19,0], [1,0,0,0], "lleg")]), @@ -83,7 +83,7 @@ def demoGaitGeneratorSetFootStepsCrawl(print_str="4. setFootSteps in Crawl"): hcf.abc_svc.waitFootSteps() def demoGoPosQuadruped(gait_type=OpenHRP.AutoBalancerService.TROT, print_str="5. go pos in trot"): - print >> sys.stderr, print_str + print(print_str, file=sys.stderr) # set gait type abcp=hcf.abc_svc.getAutoBalancerParam()[1] abcp.default_gait_type = gait_type @@ -97,7 +97,7 @@ def demoGoPosQuadruped(gait_type=OpenHRP.AutoBalancerService.TROT, print_str="5. hcf.abc_svc.waitFootSteps() def demoGoVelocityQuadruped(gait_type=OpenHRP.AutoBalancerService.TROT, print_str="7. go velocity in trot"): - print >> sys.stderr, print_str + print(print_str, file=sys.stderr) # set gait type abcp=hcf.abc_svc.getAutoBalancerParam()[1] abcp.default_gait_type = gait_type diff --git a/sample/Sample4LegRobot/sample4legrobot_stabilizer.py b/sample/Sample4LegRobot/sample4legrobot_stabilizer.py index 54e3642a465..b6cdd2b56b3 100755 --- a/sample/Sample4LegRobot/sample4legrobot_stabilizer.py +++ b/sample/Sample4LegRobot/sample4legrobot_stabilizer.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -36,10 +36,10 @@ def init (): hcf.abc_svc.setGaitGeneratorParam(ggp) hcf.startAutoBalancer(['rleg', 'lleg', 'rarm', 'larm']) hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) def demoSetParameterAndStartST(): - print >> sys.stderr, "1. setParameter" + print("1. setParameter", file=sys.stderr) stp_org = hcf.st_svc.getParameter() # for tpcc stp_org.k_tpcc_p=[0.2, 0.2] @@ -79,7 +79,7 @@ def demoSetParameterAndStartST(): hcf.startStabilizer () def demoSetFootStepsWithST(): - print >> sys.stderr,"2. setFootSteps" + print("2. setFootSteps", file=sys.stderr) hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.00,-0.19,0], [1,0,0,0], "rleg"), OpenHRP.AutoBalancerService.Footstep([0.7+0.00,+0.19,0], [1,0,0,0], "larm")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.15,+0.19,0], [1,0,0,0], "lleg"), diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index 606ded99173..560ac1a4edc 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -40,7 +40,7 @@ def init (): hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.waitInterpolation() hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) def testPoseList(pose_list, initial_pose): for pose in pose_list: @@ -64,7 +64,7 @@ def checkActualBaseAttitude(ref_rpy = None, thre=0.1): # degree if ref_rpy == None: ref_rpy = checkParameterFromLog("baseRpyOut", rtc_name="sh", save_log=False) ret = abs(math.degrees(act_rpy[0]-ref_rpy[0])) < thre and abs(math.degrees(act_rpy[1]-ref_rpy[1])) < thre - print >> sys.stderr, " ret = ", ret, ", actual base rpy = (", act_rpy, "), ", "reference base rpy = (", ref_rpy, ")" + print(" ret = ", ret, ", actual base rpy = (", act_rpy, "), ", "reference base rpy = (", ref_rpy, ")", file=sys.stderr) assert (ret) return ret @@ -97,8 +97,8 @@ def checkGoPosParam (goalx, goaly, goalth, prev_dst_foot_midcoords): # Check diff [difx, dify, difth] = calcDiffFootMidCoords(prev_dst_foot_midcoords) ret = (abs(difx-goalx) < 5e-5 and abs(dify-goaly) < 5e-5 and abs(difth-goalth) < 1e-2) - print >> sys.stderr, " Check goPosParam (diff = ", (difx-goalx), "[m], ", (dify-goaly), "[m], ", (difth-goalth), "[deg])" - print >> sys.stderr, " => ", ret + print(" Check goPosParam (diff = ", (difx-goalx), "[m], ", (dify-goaly), "[m], ", (difth-goalth), "[deg])", file=sys.stderr) + print(" => ", ret, file=sys.stderr) assert(ret) return ret @@ -132,11 +132,11 @@ def checkTooLargeABCCogAcc (acc_thre = 5.0): # [m/s^2] max_cogx_acc = max(map(lambda x : abs(x[0]), ddcog_list)) max_cogy_acc = max(map(lambda x : abs(x[1]), ddcog_list)) ret = (max_cogx_acc < acc_thre) and (max_cogy_acc < acc_thre) - print >> sys.stderr, " Check acc x = ", max_cogx_acc, ", y = ", max_cogy_acc, ", thre = ", acc_thre, "[m/s^2], ret = ", ret + print(" Check acc x = ", max_cogx_acc, ", y = ", max_cogy_acc, ", thre = ", acc_thre, "[m/s^2], ret = ", ret, file=sys.stderr) assert(ret) def demoAutoBalancerFixFeet (): - print >> sys.stderr, "1. AutoBalancer mode by fixing feet" + print("1. AutoBalancer mode by fixing feet", file=sys.stderr) hcf.startAutoBalancer(["rleg", "lleg"]); hcf.seq_svc.setJointAngles(arm_front_pose, 1.0) hcf.waitInterpolation() @@ -144,10 +144,10 @@ def demoAutoBalancerFixFeet (): hcf.waitInterpolation() hcf.stopAutoBalancer(); checkActualBaseAttitude() - print >> sys.stderr, " Start and Stop AutoBalancer by fixing feet=>OK" + print(" Start and Stop AutoBalancer by fixing feet=>OK", file=sys.stderr) def demoAutoBalancerFixFeetHands (): - print >> sys.stderr, "2. AutoBalancer mode by fixing hands and feet" + print("2. AutoBalancer mode by fixing hands and feet", file=sys.stderr) hcf.startAutoBalancer() hcf.seq_svc.setJointAngles(arm_front_pose, 1.0) hcf.waitInterpolation() @@ -155,39 +155,39 @@ def demoAutoBalancerFixFeetHands (): hcf.waitInterpolation() hcf.stopAutoBalancer(); checkActualBaseAttitude() - print >> sys.stderr, " Start and Stop AutoBalancer by fixing hands and feet=>OK" + print(" Start and Stop AutoBalancer by fixing hands and feet=>OK", file=sys.stderr) def demoAutoBalancerGetParam(): - print >> sys.stderr, "3. getAutoBalancerParam" + print("3. getAutoBalancerParam", file=sys.stderr) ret = hcf.abc_svc.getAutoBalancerParam() if ret[0]: - print >> sys.stderr, " getAutoBalancerParam() => OK" + print(" getAutoBalancerParam() => OK", file=sys.stderr) def demoAutoBalancerSetParam(): - print >> sys.stderr, "4. setAutoBalancerParam" + print("4. setAutoBalancerParam", file=sys.stderr) abcp=hcf.abc_svc.getAutoBalancerParam()[1] abcp.default_zmp_offsets = [[0.1,0,0], [0.1,0,0], [0,0,0], [0,0,0]] hcf.abc_svc.setAutoBalancerParam(abcp) - print >> sys.stderr, " default_zmp_offsets setting check in start and stop" + print(" default_zmp_offsets setting check in start and stop", file=sys.stderr) hcf.startAutoBalancer(["rleg", "lleg"]); hcf.stopAutoBalancer(); ret=hcf.abc_svc.getAutoBalancerParam() flag = (ret[0] and numpy.allclose(ret[1].default_zmp_offsets, abcp.default_zmp_offsets, 1e-6)) if flag: - print >> sys.stderr, " setAutoBalancerParam() => OK" + print(" setAutoBalancerParam() => OK", file=sys.stderr) assert (flag), (ret[0], ret[1].default_zmp_offsets, abcp.default_zmp_offsets) abcp.default_zmp_offsets = [[0,0,0], [0,0,0], [0,0,0], [0,0,0]] hcf.abc_svc.setAutoBalancerParam(abcp) def demoAutoBalancerTestPoses(): - print >> sys.stderr, "5. change base height, base rot x, base rot y, and upper body while AutoBalancer mode" + print("5. change base height, base rot x, base rot y, and upper body while AutoBalancer mode", file=sys.stderr) hcf.startAutoBalancer(["rleg", "lleg"]); testPoseList(pose_list, initial_pose) hcf.stopAutoBalancer(); checkActualBaseAttitude() def demoAutoBalancerStartStopCheck(): - print >> sys.stderr, "6. start stop check" + print("6. start stop check", file=sys.stderr) abcp=hcf.abc_svc.getAutoBalancerParam()[1] abcp.default_zmp_offsets = [[-0.05,0.05,0], [-0.05,0.05,0], [0,0,0], [0,0,0]] hcf.abc_svc.setAutoBalancerParam(abcp) @@ -206,7 +206,7 @@ def demoAutoBalancerStartStopCheck(): checkActualBaseAttitude() def demoAutoBalancerBalanceAgainstHandForce(): - print >> sys.stderr, "7. balance against hand force" + print("7. balance against hand force", file=sys.stderr) hcf.startAutoBalancer(["rleg", "lleg"]); hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, @@ -222,16 +222,16 @@ def demoAutoBalancerBalanceAgainstHandForce(): checkActualBaseAttitude() def demoAutoBalancerBalanceWithArms(): - print >> sys.stderr, "8. balance with arms" + print("8. balance with arms", file=sys.stderr) hcf.seq_svc.setJointAngles(four_legs_mode_pose, 1.0) hcf.waitInterpolation() abcp=hcf.abc_svc.getAutoBalancerParam()[1] abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm'] hcf.abc_svc.setAutoBalancerParam(abcp) hcf.startAutoBalancer(); - print >> sys.stderr, " startAutoBalancer with arms" + print(" startAutoBalancer with arms", file=sys.stderr) hcf.stopAutoBalancer(); - print >> sys.stderr, " stopAutoBalancer" + print(" stopAutoBalancer", file=sys.stderr) abcp.leg_names = ['rleg', 'lleg'] hcf.abc_svc.setAutoBalancerParam(abcp) checkActualBaseAttitude() @@ -239,7 +239,7 @@ def demoAutoBalancerBalanceWithArms(): hcf.waitInterpolation() def demoGaitGeneratorBaseTformCheck (): - print >> sys.stderr, "0. baseTform check" + print("0. baseTform check", file=sys.stderr) # Set parameter orig_ggp = hcf.abc_svc.getGaitGeneratorParam()[1] orig_abcp = hcf.abc_svc.getAutoBalancerParam()[1] @@ -291,12 +291,12 @@ def demoGaitGeneratorBaseTformCheck (): # Check values (currently pos x,y only 1[mm]) startABC_OK = all(map (lambda x,y : abs(x-y)<1*1e-3, btf1[0:3], btf2[0:3])) stopABC_OK = all(map (lambda x,y : abs(x-y)<1*1e-3, btf3[0:3], btf4[0:3])) - print >> sys.stderr, " before startABC = ", btf1[0:3], ", after startABC = ", btf2[0:3], ", diff = ", startABC_OK - print >> sys.stderr, " before stopABC = ", btf3[0:3], ", after stopABC = ", btf4[0:3], ", diff = ", stopABC_OK + print(" before startABC = ", btf1[0:3], ", after startABC = ", btf2[0:3], ", diff = ", startABC_OK, file=sys.stderr) + print(" before stopABC = ", btf3[0:3], ", after stopABC = ", btf4[0:3], ", diff = ", stopABC_OK, file=sys.stderr) assert(startABC_OK and stopABC_OK) def demoGaitGeneratorGoPos(): - print >> sys.stderr, "1. goPos" + print("1. goPos", file=sys.stderr) hcf.startAutoBalancer(); # initialize dst_foot_midcoords hcf.abc_svc.goPos(0,0,0) @@ -314,17 +314,17 @@ def demoGaitGeneratorGoPos(): hcf.abc_svc.waitFootSteps() checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords) checkActualBaseAttitude() - print >> sys.stderr, " goPos()=>OK" + print(" goPos()=>OK", file=sys.stderr) def demoGaitGeneratorGoVelocity(): - print >> sys.stderr, "2. goVelocity and goStop" - print >> sys.stderr, " goVelocity few steps" + print("2. goVelocity and goStop", file=sys.stderr) + print(" goVelocity few steps", file=sys.stderr) hcf.abc_svc.goVelocity(-0.1, -0.05, -20) time.sleep(1) hcf.abc_svc.goStop() checkActualBaseAttitude() - print >> sys.stderr, " goVelocity few steps=>OK" - print >> sys.stderr, " Check discontinuity of COG by checking too large COG acc." + print(" goVelocity few steps=>OK", file=sys.stderr) + print(" Check discontinuity of COG by checking too large COG acc.", file=sys.stderr) hcf.setMaxLogLength(10000) hcf.clearLog() hcf.abc_svc.goVelocity(0,0,0) # One step overwrite @@ -333,7 +333,7 @@ def demoGaitGeneratorGoVelocity(): checkTooLargeABCCogAcc() def demoGaitGeneratorSetFootSteps(): - print >> sys.stderr, "3. setFootSteps" + print("3. setFootSteps", file=sys.stderr) hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() @@ -343,10 +343,10 @@ def demoGaitGeneratorSetFootSteps(): OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3,0.09,0], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() checkActualBaseAttitude() - print >> sys.stderr, " setFootSteps()=>OK" + print(" setFootSteps()=>OK", file=sys.stderr) def demoGaitGeneratorChangePoseWhileWalking(): - print >> sys.stderr, "4. Change base height, base rot x, base rot y, and upper body while walking" + print("4. Change base height, base rot x, base rot y, and upper body while walking", file=sys.stderr) hcf.abc_svc.waitFootSteps() hcf.abc_svc.goVelocity(0,0,0) testPoseList(pose_list, initial_pose) @@ -354,13 +354,13 @@ def demoGaitGeneratorChangePoseWhileWalking(): checkActualBaseAttitude() def demoGaitGeneratorGetParam(): - print >> sys.stderr, "5. getGaitGeneratorParam" + print("5. getGaitGeneratorParam", file=sys.stderr) ret = hcf.abc_svc.getGaitGeneratorParam() if ret[0]: - print >> sys.stderr, " getGaitGeneratorParam() => OK" + print(" getGaitGeneratorParam() => OK", file=sys.stderr) def demoGaitGeneratorSetParam(): - print >> sys.stderr, "6. setGaitGeneratorParam" + print("6. setGaitGeneratorParam", file=sys.stderr) ggp_org = hcf.abc_svc.getGaitGeneratorParam()[1] ggp = hcf.abc_svc.getGaitGeneratorParam()[1] ggp.default_step_time = 0.9 @@ -371,13 +371,13 @@ def demoGaitGeneratorSetParam(): hcf.abc_svc.setGaitGeneratorParam(ggp) ret = hcf.abc_svc.getGaitGeneratorParam() if ret[0] and ret[1].default_step_time == ggp.default_step_time and ret[1].default_step_height == ggp.default_step_height and ret[1].default_double_support_ratio == ggp.default_double_support_ratio and ret[1].default_orbit_type == ggp.default_orbit_type: - print >> sys.stderr, " setGaitGeneratorParam() => OK" + print(" setGaitGeneratorParam() => OK", file=sys.stderr) hcf.abc_svc.goPos(0.2,0,0) hcf.abc_svc.waitFootSteps() hcf.abc_svc.setGaitGeneratorParam(ggp_org) # revert parameter def demoGaitGeneratorNonDefaultStrideStop(): - print >> sys.stderr, "7. non-default stride" + print("7. non-default stride", file=sys.stderr) hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.15,0.09,0], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() @@ -385,10 +385,10 @@ def demoGaitGeneratorNonDefaultStrideStop(): OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() checkActualBaseAttitude() - print >> sys.stderr, " Non default Stride()=>OK" + print(" Non default Stride()=>OK", file=sys.stderr) def demoGaitGeneratorToeHeelContact(): - print >> sys.stderr, "8. Use toe heel contact" + print("8. Use toe heel contact", file=sys.stderr) ggp=hcf.abc_svc.getGaitGeneratorParam()[1]; ggp.toe_pos_offset_x = 1e-3*182.0; ggp.heel_pos_offset_x = 1e-3*-72.0; @@ -403,18 +403,18 @@ def demoGaitGeneratorToeHeelContact(): ggp.heel_angle = 0; hcf.abc_svc.setGaitGeneratorParam(ggp); checkActualBaseAttitude() - print >> sys.stderr, " Toe heel contact=>OK" + print(" Toe heel contact=>OK", file=sys.stderr) def demoGaitGeneratorStopStartSyncCheck(): - print >> sys.stderr, "9. Stop and start auto balancer sync check2" - print >> sys.stderr, " Check 9-1 Sync after setFootSteps" + print("9. Stop and start auto balancer sync check2", file=sys.stderr) + print(" Check 9-1 Sync after setFootSteps", file=sys.stderr) hcf.startAutoBalancer(); hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")])]); hcf.abc_svc.waitFootSteps(); hcf.stopAutoBalancer(); - print >> sys.stderr, " Sync after setFootSteps => OK" - print >> sys.stderr, " Check 9-2 Sync from setJointAngles at the beginning" + print(" Sync after setFootSteps => OK", file=sys.stderr) + print(" Check 9-2 Sync from setJointAngles at the beginning", file=sys.stderr) open_stride_pose = [0.00026722677758058496, -0.3170503560247552, -0.0002054613599000865, 0.8240549352035262, -0.5061434785447525, -8.67443660992421e-05, 0.3112899999999996, -0.15948099999999998, -0.11539900000000003, -0.6362769999999993, 0.0, 0.0, 0.0, 0.00023087433689200683, -0.4751295978345554, -0.00021953834197007937, 0.8048588066686679, -0.3288687069275527, -8.676469399681631e-05, 0.3112899999999996, 0.15948099999999998, 0.11539900000000003, -0.6362769999999993, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] hcf.seq_svc.setJointAngles(open_stride_pose, 2.0); hcf.waitInterpolation(); @@ -423,43 +423,43 @@ def demoGaitGeneratorStopStartSyncCheck(): OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")])]); hcf.abc_svc.waitFootSteps(); hcf.stopAutoBalancer(); - print >> sys.stderr, " Sync from setJointAngle at the beginning => OK" - print >> sys.stderr, " Check 9-3 Sync from setJointAngles" + print(" Sync from setJointAngle at the beginning => OK", file=sys.stderr) + print(" Check 9-3 Sync from setJointAngles", file=sys.stderr) hcf.startAutoBalancer(); hcf.seq_svc.setJointAngles(initial_pose, 2.0); hcf.waitInterpolation(); hcf.stopAutoBalancer(); - print >> sys.stderr, " Sync from setJointAngle => OK" + print(" Sync from setJointAngle => OK", file=sys.stderr) def demoGaitGeneratorEmergencyStop(): - print >> sys.stderr, "10. Emergency stop" + print("10. Emergency stop", file=sys.stderr) hcf.startAutoBalancer(); hcf.abc_svc.goPos(0,0,90); - print >> sys.stderr, " Start goPos and wait for 4 steps" + print(" Start goPos and wait for 4 steps", file=sys.stderr) for idx in range(4): # Wait for 4 steps including initial double support phase # Wait for 1 steps hcf.seq_svc.setJointAngles(initial_pose, hcf.abc_svc.getGaitGeneratorParam()[1].default_step_time); hcf.waitInterpolation(); - print >> sys.stderr, " Emergency stoping" + print(" Emergency stoping", file=sys.stderr) hcf.abc_svc.emergencyStop(); - print >> sys.stderr, " Align foot steps" + print(" Align foot steps", file=sys.stderr) hcf.abc_svc.goPos(0,0,0); checkActualBaseAttitude() def demoGaitGeneratorGetRemainingSteps(): - print >> sys.stderr, "11. Get remaining foot steps" + print("11. Get remaining foot steps", file=sys.stderr) hcf.abc_svc.goPos(0.3,0.1,15); fslist=hcf.abc_svc.getRemainingFootstepSequence()[1] while fslist != []: fslist=hcf.abc_svc.getRemainingFootstepSequence()[1] - print >> sys.stderr, " Remaining footstep ", len(fslist) + print(" Remaining footstep ", len(fslist), file=sys.stderr) # Wait for 1 step hcf.seq_svc.setJointAngles(initial_pose, hcf.abc_svc.getGaitGeneratorParam()[1].default_step_time); hcf.waitInterpolation(); checkActualBaseAttitude() def demoGaitGeneratorChangeStepParam(): - print >> sys.stderr, "12. Change step param with setFootSteps" + print("12. Change step param with setFootSteps", file=sys.stderr) ggp_org=hcf.abc_svc.getGaitGeneratorParam()[1]; # dummy setting ggp=hcf.abc_svc.getGaitGeneratorParam()[1]; @@ -497,7 +497,7 @@ def demoGaitGeneratorChangeStepParam(): checkActualBaseAttitude() def demoGaitGeneratorOverwriteFootsteps(overwrite_offset_idx = 1): - print >> sys.stderr, "13. Overwrite footsteps during walking." + print("13. Overwrite footsteps during walking.", file=sys.stderr) hcf.startAutoBalancer() demoGaitGeneratorOverwriteFootstepsBase("x", overwrite_offset_idx, True) # Overwrite by X direction foot steps hcf.seq_svc.setJointAngles(initial_pose, 1.0*overwrite_offset_idx) @@ -517,15 +517,15 @@ def demoGaitGeneratorOverwriteFootstepsBase(axis, overwrite_offset_idx = 1, init OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3, 0.09,0], [1,0,0,0], "lleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.4,-0.09,0], [1,0,0,0], "rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.4, 0.09,0], [1,0,0,0], "lleg")])]); - print >> sys.stderr, " Overwrite footsteps ", overwrite_offset_idx + print(" Overwrite footsteps ", overwrite_offset_idx, file=sys.stderr) # Get remaining footstep [remain_fs, current_fs_idx]=hcf.abc_svc.getRemainingFootstepSequence()[1:] #print >> sys.stderr, remain_fs - print >> sys.stderr, " Remaining legs = ", map(lambda fs : fs.leg, remain_fs) - print >> sys.stderr, " Remaining idx = ", map(lambda idx : current_fs_idx+idx, range(len(remain_fs))) + print(" Remaining legs = ", map(lambda fs : fs.leg, remain_fs), file=sys.stderr) + print(" Remaining idx = ", map(lambda idx : current_fs_idx+idx, range(len(remain_fs))), file=sys.stderr) # Footstep index to be overwritten overwrite_fs_idx = current_fs_idx + overwrite_offset_idx - print >> sys.stderr, " Overwrite index = ",overwrite_fs_idx, ", leg = ", remain_fs[overwrite_offset_idx].leg + print(" Overwrite index = ",overwrite_fs_idx, ", leg = ", remain_fs[overwrite_offset_idx].leg, file=sys.stderr) # Calc new footsteps import numpy support_fs = remain_fs[overwrite_offset_idx-1] # support fs before overwritten fs @@ -547,7 +547,7 @@ def demoGaitGeneratorOverwriteFootstepsBase(axis, overwrite_offset_idx = 1, init hcf.abc_svc.setFootSteps(new_fs, overwrite_fs_idx); def demoGaitGeneratorFixHand(): - print >> sys.stderr, "14. Fix arm walking" + print("14. Fix arm walking", file=sys.stderr) hcf.stopAutoBalancer() hcf.startAutoBalancer() # Set pose @@ -557,7 +557,7 @@ def demoGaitGeneratorFixHand(): dualarm_push_pose=[-3.998549e-05,-0.710564,-0.000264,1.41027,-0.680767,-2.335251e-05,-0.541944,-0.091558,0.122667,-1.02616,-1.71287,-0.056837,1.5708,-3.996804e-05,-0.710511,-0.000264,1.41016,-0.680706,-2.333505e-05,-0.542,0.091393,-0.122578,-1.02608,1.71267,-0.05677,-1.5708,0.006809,0.000101,-0.000163] hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0) hcf.waitInterpolation() - print >> sys.stderr, " Walk without fixing arm" + print(" Walk without fixing arm", file=sys.stderr) abcp=hcf.abc_svc.getAutoBalancerParam()[1] abcp.is_hand_fix_mode=False hcf.abc_svc.setAutoBalancerParam(abcp) @@ -567,7 +567,7 @@ def demoGaitGeneratorFixHand(): hcf.abc_svc.waitFootSteps() hcf.abc_svc.goPos(0,0,30) hcf.abc_svc.waitFootSteps() - print >> sys.stderr, " Walk with fixing arm" + print(" Walk with fixing arm", file=sys.stderr) abcp=hcf.abc_svc.getAutoBalancerParam()[1] abcp.is_hand_fix_mode=True hcf.abc_svc.setAutoBalancerParam(abcp) @@ -584,10 +584,10 @@ def demoGaitGeneratorFixHand(): ref_rpy = checkParameterFromLog("baseRpyOut", rtc_name="abc") hcf.stopAutoBalancer() checkActualBaseAttitude(ref_rpy) - print >> sys.stderr, " Fix hand=>OK" + print(" Fix hand=>OK", file=sys.stderr) def demoGaitGeneratorOverwriteCurrentFootstep(): - print >> sys.stderr, "15. Overwrite current footstep" + print("15. Overwrite current footstep", file=sys.stderr) hcf.seq_svc.setJointAngles(initial_pose, 1.0) hcf.waitInterpolation() hcf.startAutoBalancer() @@ -606,14 +606,14 @@ def demoGaitGeneratorOverwriteCurrentFootstep(): hcf.seq_svc.setJointAngles(initial_pose, 2.0);hcf.waitInterpolation() # wait 2 step using dummy waitInterpolation hcf.abc_svc.goStop() checkActualBaseAttitude() - print >> sys.stderr, " Overwrite current footstep=>OK" + print(" Overwrite current footstep=>OK", file=sys.stderr) # reset params hcf.abc_svc.setGaitGeneratorParam(orig_ggp) def demoGaitGeneratorGoPosOverwrite(): - print >> sys.stderr, "16. goPos overwriting" + print("16. goPos overwriting", file=sys.stderr) hcf.startAutoBalancer(); - print >> sys.stderr, " Overwrite goPos by goPos" + print(" Overwrite goPos by goPos", file=sys.stderr) # Initialize dst_foot_midcoords hcf.abc_svc.goPos(0,0.001,0); hcf.abc_svc.waitFootSteps(); @@ -624,7 +624,7 @@ def demoGaitGeneratorGoPosOverwrite(): hcf.abc_svc.goPos(goalx,goaly,goalth) # overwrite gopos hcf.abc_svc.waitFootSteps() checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords) - print >> sys.stderr, " Overwrite setFootSteps by goPos" + print(" Overwrite setFootSteps by goPos", file=sys.stderr) prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]), @@ -638,7 +638,7 @@ def demoGaitGeneratorGoPosOverwrite(): checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords) def demoGaitGeneratorGrasplessManipMode(): - print >> sys.stderr, "17. Graspless manip mode" + print("17. Graspless manip mode", file=sys.stderr) hcf.startAutoBalancer(); # Initialize and pose define dualarm_push_pose=[-3.998549e-05,-0.710564,-0.000264,1.41027,-0.680767,-2.335251e-05,-0.541944,-0.091558,0.122667,-1.02616,-1.71287,-0.056837,1.5708,-3.996804e-05,-0.710511,-0.000264,1.41016,-0.680706,-2.333505e-05,-0.542,0.091393,-0.122578,-1.02608,1.71267,-0.05677,-1.5708,0.006809,0.000101,-0.000163] @@ -691,10 +691,10 @@ def demoGaitGeneratorGrasplessManipMode(): else: tmpret = abs(diff[0]) < 1e-3 and abs(diff[1]) < 1e-3 and abs(diff[2]) < 1.0 ret = ret and tmpret - print >> sys.stderr, " ret = ", tmpret, ", diff = ", diff + print(" ret = ", tmpret, ", diff = ", diff, file=sys.stderr) # Finishing if ret: - print >> sys.stderr, " total is OK" + print(" total is OK", file=sys.stderr) assert(ret) hcf.co_svc.enableCollisionDetection() hcf.seq_svc.setJointAngles(dualarm_push_pose, 0.5) @@ -702,7 +702,7 @@ def demoGaitGeneratorGrasplessManipMode(): hcf.abc_svc.setAutoBalancerParam(org_abcp) def demoGaitGeneratorSetFootStepsWithArms(): - print >> sys.stderr, "18. Trot Walking" + print("18. Trot Walking", file=sys.stderr) hcf.stopAutoBalancer() hcf.seq_svc.setJointAngles(four_legs_mode_pose, 1.0) hcf.waitInterpolation() @@ -725,7 +725,7 @@ def demoGaitGeneratorSetFootStepsWithArms(): OpenHRP.AutoBalancerService.Footstep([0.23,-0.21,0.86], [1,0,0,0], "rarm")])]) hcf.abc_svc.waitFootSteps() checkActualBaseAttitude() - print >> sys.stderr, " setFootSteps()=>OK" + print(" setFootSteps()=>OK", file=sys.stderr) # reset params hcf.stopAutoBalancer() hcf.abc_svc.setAutoBalancerParam(orig_abcp) @@ -733,7 +733,7 @@ def demoGaitGeneratorSetFootStepsWithArms(): hcf.startAutoBalancer() def demoGaitGeneratorChangeStrideLimitationType(): - print >> sys.stderr, "19. Change stride limitation type to CIRCLE" + print("19. Change stride limitation type to CIRCLE", file=sys.stderr) hcf.startAutoBalancer(); # initialize dst_foot_midcoords hcf.abc_svc.goPos(0,0,0) @@ -758,12 +758,12 @@ def demoGaitGeneratorChangeStrideLimitationType(): hcf.abc_svc.waitFootSteps() checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords) checkActualBaseAttitude() - print >> sys.stderr, " Change stride limitation type to CIRCLE=>OK" + print(" Change stride limitation type to CIRCLE=>OK", file=sys.stderr) # reset params hcf.abc_svc.setGaitGeneratorParam(orig_ggp) def demoStandingPosResetting(): - print >> sys.stderr, "demoStandingPosResetting" + print("demoStandingPosResetting", file=sys.stderr) hcf.abc_svc.goPos(0,0,math.degrees(-1*checkParameterFromLog("WAIST")[5])); # Rot yaw hcf.abc_svc.waitFootSteps() hcf.abc_svc.goPos(0,-1*checkParameterFromLog("WAIST")[1],0); # Pos Y @@ -806,7 +806,7 @@ def demo(): demoGaitGeneratorGrasplessManipMode() demoGaitGeneratorSetFootStepsWithArms() demoGaitGeneratorChangeStrideLimitationType() - print >> sys.stderr, "All samplerobot_auto_balancer.py demo time ", (time.time()-start_time), "[s]" + print("All samplerobot_auto_balancer.py demo time ", (time.time()-start_time), "[s]", file=sys.stderr) if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_carry_object.py b/sample/SampleRobot/samplerobot_carry_object.py index 752763089ad..8e52cba2e74 100755 --- a/sample/SampleRobot/samplerobot_carry_object.py +++ b/sample/SampleRobot/samplerobot_carry_object.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -95,50 +95,50 @@ def init (): hcf.rmfo_svc.loadForceMomentOffsetParams(HRPSYS_DIR+'/share/hrpsys/samples/SampleRobot/ForceSensorOffset_SampleRobot.txt') def demoDualarmCarryup (is_walk=True, auto_detecion = True): - print >> sys.stderr, "1. Dual-arm carry-up demo." - print >> sys.stderr, " Reaching" + print("1. Dual-arm carry-up demo.", file=sys.stderr) + print(" Reaching", file=sys.stderr) hcf.seq_svc.setJointAngles(dualarm_via_pose, 2.0) hcf.waitInterpolation() hcf.seq_svc.setJointAngles(dualarm_reach_pose, 2.0) hcf.waitInterpolation() - print >> sys.stderr, " Increase operational force" + print(" Increase operational force", file=sys.stderr) if auto_detecion: objectTurnaroundDetection() else: hcf.seq_svc.setWrenches([0]*6+[0]*6+[0,0,-9.8*2.5,0,0,0]*2, 2.0) # 2.5[kg]*2 = 5.0[kg] hcf.waitInterpolation() - print >> sys.stderr, " Lift up & down" + print(" Lift up & down", file=sys.stderr) hcf.seq_svc.setJointAngles(dualarm_liftup_pose, 2.0) hcf.waitInterpolation() if is_walk: demoWalk() hcf.seq_svc.setJointAngles(dualarm_reach_pose, 2.0) hcf.waitInterpolation() - print >> sys.stderr, " Reset operational force" + print(" Reset operational force", file=sys.stderr) hcf.seq_svc.setWrenches([0]*24, 2.0) hcf.waitInterpolation() - print >> sys.stderr, " Releasing" + print(" Releasing", file=sys.stderr) hcf.seq_svc.setJointAngles(dualarm_via_pose, 2.0) hcf.waitInterpolation() hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.waitInterpolation() def demoSinglearmCarryup (is_walk=True, auto_detecion = True): - print >> sys.stderr, "2. Single-arm carry-up demo." - print >> sys.stderr, " Reaching" + print("2. Single-arm carry-up demo.", file=sys.stderr) + print(" Reaching", file=sys.stderr) hcf.abc_svc.goPos(0.02,0.15,0) hcf.abc_svc.waitFootSteps(); hcf.seq_svc.setJointAngles(singlearm_via_pose, 2.0) hcf.waitInterpolation() hcf.seq_svc.setJointAngles(singlearm_reach_pose, 2.0) hcf.waitInterpolation() - print >> sys.stderr, " Increase operational force" + print(" Increase operational force", file=sys.stderr) if auto_detecion: objectTurnaroundDetection(limbs=['rarm']) else: hcf.seq_svc.setWrenches([0]*6+[0]*6+[0]*6+[0,0,-9.8*5.0,0,0,0], 2.0) hcf.waitInterpolation() - print >> sys.stderr, " Lift up & down" + print(" Lift up & down", file=sys.stderr) hcf.seq_svc.setJointAngles(singlearm_liftup_pose, 2.0) hcf.waitInterpolation() if is_walk: @@ -147,10 +147,10 @@ def demoSinglearmCarryup (is_walk=True, auto_detecion = True): demoWalk() hcf.seq_svc.setJointAngles(singlearm_reach_pose, 2.0) hcf.waitInterpolation() - print >> sys.stderr, " Reset operational force" + print(" Reset operational force", file=sys.stderr) hcf.seq_svc.setWrenches([0]*24, 2.0) hcf.waitInterpolation() - print >> sys.stderr, " Releasing" + print(" Releasing", file=sys.stderr) hcf.seq_svc.setJointAngles(singlearm_via_pose, 2.0) hcf.waitInterpolation() hcf.seq_svc.setJointAngles(initial_pose, 2.0) @@ -174,11 +174,11 @@ def objectTurnaroundDetection(max_time = 4.0, max_ref_force = 9.8*6.0, limbs=["r tmpflg = hcf.octd_svc.checkObjectContactTurnaroundDetection() #print rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data, rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data [ret, fv, mv, total, fric_w] = hcf.octd_svc.getObjectForcesMoments() - print " flag = ", tmpflg, ", forces = ", fv, ", moments = ", mv, ", total = ", total, ", fric_w = ", fric_w + print(" flag = ", tmpflg, ", forces = ", fv, ", moments = ", mv, ", total = ", total, ", fric_w = ", fric_w) flg = (tmpflg == OpenHRP.ObjectContactTurnaroundDetectorService.MODE_DETECTOR_IDLE) or (tmpflg == OpenHRP.ObjectContactTurnaroundDetectorService.MODE_STARTED) time.sleep(0.5) [ret, fv, mv, total, fric_w] = hcf.octd_svc.getObjectForcesMoments() - print " flag = ", tmpflg, ", forces = ", fv, ", moments = ", mv, ", total = ", total, ", fric_w = ", fric_w + print(" flag = ", tmpflg, ", forces = ", fv, ", moments = ", mv, ", total = ", total, ", fric_w = ", fric_w) if limbs==['rarm']: hcf.seq_svc.setWrenches([0]*18+fv[0]+mv[0], 2.0) else: @@ -196,13 +196,13 @@ def demoWalk (): hcf.abc_svc.waitFootSteps(); def demoDualarmPush (auto_detecion = True): - print >> sys.stderr, "3. Dual-arm push demo." - print >> sys.stderr, " Move to" + print("3. Dual-arm push demo.", file=sys.stderr) + print(" Move to", file=sys.stderr) hcf.abc_svc.goPos(-0.45,0,0); hcf.abc_svc.waitFootSteps(); hcf.abc_svc.goPos(0,0,(math.degrees(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.orientation.y-rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y))); hcf.abc_svc.waitFootSteps(); - print >> sys.stderr, " Reaching" + print(" Reaching", file=sys.stderr) #hcf.abc_svc.goPos(0.25, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0); hcf.abc_svc.goPos(0.1, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0); hcf.abc_svc.waitFootSteps(); @@ -210,13 +210,13 @@ def demoDualarmPush (auto_detecion = True): hcf.waitInterpolation() hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0) hcf.waitInterpolation() - print >> sys.stderr, " Increase operational force" + print(" Increase operational force", file=sys.stderr) if auto_detecion: objectTurnaroundDetection(axis=[-1,0,0],max_ref_force=100, max_time=2) else: hcf.seq_svc.setWrenches([0]*6+[0]*6+[-40,0,0,0,0,0]*2, 2.0) hcf.waitInterpolation() - print >> sys.stderr, " Push forward" + print(" Push forward", file=sys.stderr) abcp=hcf.abc_svc.getAutoBalancerParam()[1] abcp.is_hand_fix_mode = True hcf.abc_svc.setAutoBalancerParam(abcp) diff --git a/sample/SampleRobot/samplerobot_collision_detector.py b/sample/SampleRobot/samplerobot_collision_detector.py index c3870f78fd5..4058e5dc634 100755 --- a/sample/SampleRobot/samplerobot_collision_detector.py +++ b/sample/SampleRobot/samplerobot_collision_detector.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -31,11 +31,11 @@ def init (): col_safe_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,0.15708,-0.113446,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0] col_fail_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.845363,0.03992,0.250074,-1.32816,0.167513,0.016204,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0] hrpsys_version = hcf.co.ref.get_component_profile().version - print >> sys.stderr, "hrpsys_version = %s"%hrpsys_version + print("hrpsys_version = %s"%hrpsys_version, file=sys.stderr) # demo functions def demoCollisionCheckSafe (): - print >> sys.stderr, "1. CollisionCheck in safe pose" + print("1. CollisionCheck in safe pose", file=sys.stderr) hcf.seq_svc.setJointAngles(col_safe_pose, 3.0); hcf.waitInterpolation(); counter = 0 @@ -45,75 +45,75 @@ def demoCollisionCheckSafe (): assert(counter != 20) cs=hcf.co_svc.getCollisionStatus()[1] if cs.safe_posture: - print >> sys.stderr, " => Safe pose" + print(" => Safe pose", file=sys.stderr) assert(cs.safe_posture is True) def demoCollisionCheckFail (): - print >> sys.stderr, "2. CollisionCheck in fail pose" + print("2. CollisionCheck in fail pose", file=sys.stderr) hcf.seq_svc.setJointAngles(col_fail_pose, 3.0); hcf.waitInterpolation(); cs=hcf.co_svc.getCollisionStatus()[1] if not cs.safe_posture: - print >> sys.stderr, " => Successfully stop fail pose" + print(" => Successfully stop fail pose", file=sys.stderr) assert((not cs.safe_posture) is True) hcf.seq_svc.setJointAngles(col_safe_pose, 3.0); hcf.waitInterpolation(); cs=hcf.co_svc.getCollisionStatus()[1] if cs.safe_posture: - print >> sys.stderr, " => Successfully return to safe pose" + print(" => Successfully return to safe pose", file=sys.stderr) assert(cs.safe_posture is True) def demoCollisionCheckFailWithSetTolerance (): - print >> sys.stderr, "3. CollisionCheck in fail pose with 0.1[m] tolerance" + print("3. CollisionCheck in fail pose with 0.1[m] tolerance", file=sys.stderr) hcf.co_svc.setTolerance("all", 0.1); # [m] hcf.seq_svc.setJointAngles(col_fail_pose, 1.0); hcf.waitInterpolation(); cs=hcf.co_svc.getCollisionStatus()[1] if not cs.safe_posture: - print >> sys.stderr, " => Successfully stop fail pose (0.1[m] tolerance)" + print(" => Successfully stop fail pose (0.1[m] tolerance)", file=sys.stderr) assert((not cs.safe_posture) is True) hcf.co_svc.setTolerance("all", 0.0); # [m] hcf.seq_svc.setJointAngles(col_safe_pose, 3.0); hcf.waitInterpolation(); cs=hcf.co_svc.getCollisionStatus()[1] if cs.safe_posture: - print >> sys.stderr, " => Successfully return to safe pose" + print(" => Successfully return to safe pose", file=sys.stderr) assert(cs.safe_posture is True) def demoCollisionDisableEnable (): - print >> sys.stderr, "4. CollisionDetection enable and disable" + print("4. CollisionDetection enable and disable", file=sys.stderr) hcf.seq_svc.setJointAngles(col_safe_pose, 1.0); hcf.waitInterpolation(); if hcf.co_svc.disableCollisionDetection(): - print >> sys.stderr, " => Successfully disabled when no collision" + print(" => Successfully disabled when no collision", file=sys.stderr) assert(hcf.co_svc.disableCollisionDetection() is True) if hcf.co_svc.enableCollisionDetection(): - print >> sys.stderr, " => Successfully enabled when no collision" + print(" => Successfully enabled when no collision", file=sys.stderr) assert(hcf.co_svc.enableCollisionDetection() is True) hcf.seq_svc.setJointAngles(col_fail_pose, 1.0); hcf.waitInterpolation(); if not hcf.co_svc.disableCollisionDetection(): - print >> sys.stderr, " => Successfully inhibit disabling when collision" + print(" => Successfully inhibit disabling when collision", file=sys.stderr) assert((not hcf.co_svc.disableCollisionDetection()) is True) hcf.seq_svc.setJointAngles(col_safe_pose, 1.0); hcf.waitInterpolation(); def demoCollisionMask (): if hcf.abc_svc != None: - print >> sys.stderr, "5. Collision mask test" + print("5. Collision mask test", file=sys.stderr) hcf.co_svc.setTolerance("all", 0); # [m] hcf.startAutoBalancer() - print >> sys.stderr, " 5.1 Collision mask among legs : Check RLEG_ANKLE_R - LLEG_ANKLE_R" - print >> sys.stderr, " Desired behavior : Robot stops when legs collision." + print(" 5.1 Collision mask among legs : Check RLEG_ANKLE_R - LLEG_ANKLE_R", file=sys.stderr) + print(" Desired behavior : Robot stops when legs collision.", file=sys.stderr) hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")])]) hcf.abc_svc.waitFootSteps(); hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0],[1,0,0,0],"lleg")])]) hcf.abc_svc.waitFootSteps(); - print >> sys.stderr, " => Successfully mask works. Legs joints stops when collision." - print >> sys.stderr, " 5.2 Collision mask between leg and arm : Check RLEG_HIP_R and RARM_WRIST*" - print >> sys.stderr, " Desired behavior : Leg joints moves and arm joints stops when collision." + print(" => Successfully mask works. Legs joints stops when collision.", file=sys.stderr) + print(" 5.2 Collision mask between leg and arm : Check RLEG_HIP_R and RARM_WRIST*", file=sys.stderr) + print(" Desired behavior : Leg joints moves and arm joints stops when collision.", file=sys.stderr) hcf.seq_svc.setJointAngles(col_safe_pose, 1.0); hcf.waitInterpolation(); hcf.abc_svc.goVelocity(0,0,0); @@ -122,19 +122,19 @@ def demoCollisionMask (): hcf.seq_svc.setJointAngles(col_safe_pose, 3.0); hcf.waitInterpolation(); hcf.abc_svc.goStop(); - print >> sys.stderr, " => Successfully mask works. Arm joints stops and leg joints moves." - print >> sys.stderr, " 5.3 Collision mask between leg and arm : Check RLEG_HIP_R and RARM_WRIST* and RLEG_ANKLE_R and LLEG_ANKLE_R (combination of 5.1 and 5.2)" - print >> sys.stderr, " Desired behavior : First, arm stops and legs moves." + print(" => Successfully mask works. Arm joints stops and leg joints moves.", file=sys.stderr) + print(" 5.3 Collision mask between leg and arm : Check RLEG_HIP_R and RARM_WRIST* and RLEG_ANKLE_R and LLEG_ANKLE_R (combination of 5.1 and 5.2)", file=sys.stderr) + print(" Desired behavior : First, arm stops and legs moves.", file=sys.stderr) hcf.seq_svc.setJointAngles(col_safe_pose, 1.0); hcf.waitInterpolation(); - print >> sys.stderr, " Desired behavior : Next, arm keeps stopping and legs stops." + print(" Desired behavior : Next, arm keeps stopping and legs stops.", file=sys.stderr) hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")])]) hcf.abc_svc.waitFootSteps(); hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0],[1,0,0,0],"lleg")])]) hcf.abc_svc.waitFootSteps(); - print >> sys.stderr, " => Successfully mask works with combined situation." + print(" => Successfully mask works with combined situation.", file=sys.stderr) hcf.stopAutoBalancer() def demo(): diff --git a/sample/SampleRobot/samplerobot_data_logger.py b/sample/SampleRobot/samplerobot_data_logger.py index 7f76d04b385..60b24c2acb8 100755 --- a/sample/SampleRobot/samplerobot_data_logger.py +++ b/sample/SampleRobot/samplerobot_data_logger.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -25,23 +25,23 @@ def init (): hcf.waitInterpolation() def demoSaveLog(): - print >> sys.stderr, "1. Save log" + print("1. Save log", file=sys.stderr) # Save log files for each ports as /tmp/test-samplerobot-log.* # file names are /tmp/test-samplerobot-log.[RTCName]_[PortName], c.f., /tmp/test-samplerobot-log.sh_qOut ... etc hcf.saveLog("/tmp/test-samplerobot-log") ret = os.path.exists("/tmp/test-samplerobot-log.sh_qOut") if ret: - print >> sys.stderr, " save() =>OK" + print(" save() =>OK", file=sys.stderr) assert(ret is True) def demoClearLog(): - print >> sys.stderr, "2. Clear buffer" + print("2. Clear buffer", file=sys.stderr) hcf.clearLog() - print >> sys.stderr, " clear() =>OK" + print(" clear() =>OK", file=sys.stderr) assert(True) def demoSetMaxLogLength(): - print >> sys.stderr, "3. Set max ring-buffer length : 100 [loop] * 0.002 [s] = 0.2 [s] data" + print("3. Set max ring-buffer length : 100 [loop] * 0.002 [s] = 0.2 [s] data", file=sys.stderr) hcf.setMaxLogLength(100) hcf.seq_svc.setJointAngles(initial_pose, 0.2) # wait hcf.waitInterpolation() @@ -49,7 +49,7 @@ def demoSetMaxLogLength(): from subprocess import check_output ret = check_output(['wc', '-l', '/tmp/test-samplerobot-log.sh_qOut']).split(" ")[0] == '100' if ret: - print >> sys.stderr, " maxLength() =>OK" + print(" maxLength() =>OK", file=sys.stderr) assert(ret is True) def demo (): diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index 7b37214a85d..2c974edcedc 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -26,7 +26,7 @@ def init (): wrench_command0 = [0.0]*24 wrench_command1 = [1.0]*24 hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) def arrayDistance (angle1, angle2): return sum([abs(i-j) for (i,j) in zip(angle1,angle2)]) @@ -48,7 +48,7 @@ def getWrenchArray (): # demo functions def demoEmergencyStopJointAngle (): - print >> sys.stderr, "1. test stopMotion and releaseMotion for joint angle" + print("1. test stopMotion and releaseMotion for joint angle", file=sys.stderr) hcf.es_svc.releaseMotion() hcf.seq_svc.setJointAngles(init_pose, 1.0) hcf.waitInterpolation() @@ -56,57 +56,57 @@ def demoEmergencyStopJointAngle (): tmp_angle1 = hcf.getActualState().angle play_time = 10 hcf.seq_svc.setJointAngles(reset_pose, play_time) - print >> sys.stderr, " send angle_vector of %d [sec]" % play_time + print(" send angle_vector of %d [sec]" % play_time, file=sys.stderr) time.sleep(4) - print >> sys.stderr, " check whether robot pose is changing" + print(" check whether robot pose is changing", file=sys.stderr) tmp_angle2 = hcf.getActualState().angle if arrayApproxEqual(init_pose, tmp_angle1) and not(arrayApproxEqual(tmp_angle1, tmp_angle2)): - print >> sys.stderr, " => robot is moving." + print(" => robot is moving.", file=sys.stderr) assert (arrayApproxEqual(init_pose, tmp_angle1) and not(arrayApproxEqual(tmp_angle1, tmp_angle2))) - print >> sys.stderr, " stop motion" + print(" stop motion", file=sys.stderr) hcf.es_svc.stopMotion() time.sleep(0.1) - print >> sys.stderr, " check whether robot pose remained still" + print(" check whether robot pose remained still", file=sys.stderr) tmp_angle1 = hcf.getActualState().angle time.sleep(3) tmp_angle2 = hcf.getActualState().angle if arrayApproxEqual(tmp_angle1, tmp_angle2): - print >> sys.stderr, " => robot is not moving. stopMotion is working succesfully." + print(" => robot is not moving. stopMotion is working succesfully.", file=sys.stderr) assert (arrayApproxEqual(tmp_angle1, tmp_angle2)) - print >> sys.stderr, " release motion" + print(" release motion", file=sys.stderr) hcf.es_svc.releaseMotion() - print >> sys.stderr, " check whether robot pose changed" + print(" check whether robot pose changed", file=sys.stderr) tmp_angle1 = hcf.getActualState().angle hcf.waitInterpolation() time.sleep(0.1) tmp_angle2 = hcf.getActualState().angle if (not(arrayApproxEqual(tmp_angle1, tmp_angle2)) and arrayApproxEqual(tmp_angle2, reset_pose)): - print >> sys.stderr, " => robot is moving. releaseMotion is working succesfully." + print(" => robot is moving. releaseMotion is working succesfully.", file=sys.stderr) assert(not(arrayApproxEqual(tmp_angle1, tmp_angle2)) and arrayApproxEqual(tmp_angle2, reset_pose)) hcf.es_svc.releaseMotion() hcf.seq_svc.setJointAngles(init_pose, 1.0) hcf.waitInterpolation() def demoEmergencyStopJointAngleWithKeyInteracton (): - print >> sys.stderr, "1. test stopMotion and releaseMotion with key interaction for joint angle" + print("1. test stopMotion and releaseMotion with key interaction for joint angle", file=sys.stderr) pose_list = [reset_pose, init_pose] * 4 play_time = 5 hcf.seq_svc.playPattern(pose_list, [[0,0,0]]*len(pose_list), [[0,0,0]]*len(pose_list), [play_time]*len(pose_list)) - print >> sys.stderr, " send angle_vector_sequence of %d [sec]" % (play_time*len(pose_list)) - print >> sys.stderr, " press Enter to stop / release motion" + print(" send angle_vector_sequence of %d [sec]" % (play_time*len(pose_list)), file=sys.stderr) + print(" press Enter to stop / release motion", file=sys.stderr) while True: raw_input() - print >> sys.stderr, " stop motion" + print(" stop motion", file=sys.stderr) hcf.es_svc.stopMotion() if hcf.seq_svc.isEmpty(): break raw_input() - print >> sys.stderr, " release motion" + print(" release motion", file=sys.stderr) hcf.es_svc.releaseMotion() if hcf.seq_svc.isEmpty(): break hcf.es_svc.releaseMotion() def demoEmergencyStopWrench (): - print >> sys.stderr, "2. test stopMotion and releaseMotion for wrench" + print("2. test stopMotion and releaseMotion for wrench", file=sys.stderr) hcf.es_svc.releaseMotion() hcf.seq_svc.setJointAngles(init_pose, 1.0) hcf.seq_svc.setWrenches(wrench_command0, 1.0) @@ -115,51 +115,51 @@ def demoEmergencyStopWrench (): tmp_wrench1 = getWrenchArray() play_time = 10 hcf.seq_svc.setWrenches(wrench_command1, play_time) - print >> sys.stderr, " send wrench command of %d [sec]" % play_time + print(" send wrench command of %d [sec]" % play_time, file=sys.stderr) time.sleep(4) - print >> sys.stderr, " check whether wrench is changing" + print(" check whether wrench is changing", file=sys.stderr) tmp_wrench2 = getWrenchArray() if arrayApproxEqual(wrench_command0, tmp_wrench1) and not(arrayApproxEqual(tmp_wrench1, tmp_wrench2)): - print >> sys.stderr, " => wrench is changing." + print(" => wrench is changing.", file=sys.stderr) assert (arrayApproxEqual(wrench_command0, tmp_wrench1) and not(arrayApproxEqual(tmp_wrench1, tmp_wrench2))) - print >> sys.stderr, " stop motion" + print(" stop motion", file=sys.stderr) hcf.es_svc.stopMotion() time.sleep(0.1) - print >> sys.stderr, " check whether wrench remained still" + print(" check whether wrench remained still", file=sys.stderr) tmp_wrench1 = getWrenchArray() time.sleep(3) tmp_wrench2 = getWrenchArray() if arrayApproxEqual(tmp_wrench1, tmp_wrench2): - print >> sys.stderr, " => wrench is not changing. stopMotion is working succesfully." + print(" => wrench is not changing. stopMotion is working succesfully.", file=sys.stderr) assert (arrayApproxEqual(tmp_wrench1, tmp_wrench2)) - print >> sys.stderr, " release motion" + print(" release motion", file=sys.stderr) hcf.es_svc.releaseMotion() - print >> sys.stderr, " check whether wrench changed" + print(" check whether wrench changed", file=sys.stderr) tmp_wrench1 = getWrenchArray() hcf.waitInterpolation() time.sleep(1.0) tmp_wrench2 = getWrenchArray() if (not(arrayApproxEqual(tmp_wrench1, tmp_wrench2)) and arrayApproxEqual(tmp_wrench2, wrench_command1)): - print >> sys.stderr, " => wrench is changing. releaseMotion is working succesfully." + print(" => wrench is changing. releaseMotion is working succesfully.", file=sys.stderr) assert(not(arrayApproxEqual(tmp_wrench1, tmp_wrench2)) and arrayApproxEqual(tmp_wrench2, wrench_command1)) hcf.es_svc.releaseMotion() hcf.seq_svc.setWrenches(wrench_command0, 1.0) hcf.waitInterpolation() def demoEmergencyStopReleaseWhenDeactivated(): - print >> sys.stderr, "3. test transition to release mode when deactivated" - print >> sys.stderr, " stop motion" + print("3. test transition to release mode when deactivated", file=sys.stderr) + print(" stop motion", file=sys.stderr) hcf.es_svc.stopMotion() hcf.hes_svc.stopMotion() if((hcf.es_svc.getEmergencyStopperParam()[1].is_stop_mode == True) and (hcf.hes_svc.getEmergencyStopperParam()[1].is_stop_mode == True)): - print >> sys.stderr, " emergency stopper become stop mode succesfully" - print >> sys.stderr, " deactivate and activate es and hes" + print(" emergency stopper become stop mode succesfully", file=sys.stderr) + print(" deactivate and activate es and hes", file=sys.stderr) hcf.es.stop() hcf.hes.stop() hcf.es.start() hcf.hes.start() if((hcf.es_svc.getEmergencyStopperParam()[1].is_stop_mode == False) and (hcf.hes_svc.getEmergencyStopperParam()[1].is_stop_mode == False)): - print >> sys.stderr, " emergency stopper become release mode succesfully" + print(" emergency stopper become release mode succesfully", file=sys.stderr) assert((hcf.es_svc.getEmergencyStopperParam()[1].is_stop_mode == False) and (hcf.hes_svc.getEmergencyStopperParam()[1].is_stop_mode == False)) def demo(key_interaction=False): diff --git a/sample/SampleRobot/samplerobot_impedance_controller.py b/sample/SampleRobot/samplerobot_impedance_controller.py index 8ef9b096823..5a37675fd99 100755 --- a/sample/SampleRobot/samplerobot_impedance_controller.py +++ b/sample/SampleRobot/samplerobot_impedance_controller.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -19,7 +19,7 @@ def init (): hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) # set initial pose from sample/controller/SampleController/etc/Sample.pos initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] hcf.seq_svc.setJointAngles(initial_pose, 2.0) @@ -27,7 +27,7 @@ def init (): def demoStartStopIMP (): # 0. startImpedance + stopImpedance python interface - print >> sys.stderr, "0. startImpedance + stopImpedance python interface" + print("0. startImpedance + stopImpedance python interface", file=sys.stderr) hcf.startImpedance("rarm") hcf.startImpedance("larm") hcf.stopImpedance("larm") @@ -35,17 +35,17 @@ def demoStartStopIMP (): def demoGetImpedanceControllerParam (): # 1. Getter check - print >> sys.stderr, "1. Getter check" + print("1. Getter check", file=sys.stderr) all_get_ret = [] for limb in ["rarm", "larm"]: all_get_ret.append(hcf.ic_svc.getImpedanceControllerParam(limb)[0]) - print >> sys.stderr, " all_get_ret = ", all_get_ret + print(" all_get_ret = ", all_get_ret, file=sys.stderr) assert(all(all_get_ret)) - print >> sys.stderr, " getImpedanceControllerParam => OK" + print(" getImpedanceControllerParam => OK", file=sys.stderr) def demoSetImpedanceControllerParam (): # 2. Setter check - print >> sys.stderr, "2. Setter check" + print("2. Setter check", file=sys.stderr) all_set_ret = [] all_value_ret = [] for limb in ["rarm", "larm"]: @@ -56,13 +56,13 @@ def demoSetImpedanceControllerParam (): all_set_ret.append(hcf.ic_svc.setImpedanceControllerParam(limb, icp1)) [ret2, icp2]=hcf.ic_svc.getImpedanceControllerParam(limb) all_value_ret.append((icp1.M_r == icp2.M_r) and (icp1.D_r == icp2.D_r) and (icp1.K_r == icp2.K_r)) - print >> sys.stderr, " all_set_ret = ", all_set_ret, ", all_value_ret = ", all_value_ret + print(" all_set_ret = ", all_set_ret, ", all_value_ret = ", all_value_ret, file=sys.stderr) assert(all(all_set_ret) and all(all_value_ret)) - print >> sys.stderr, " setImpedanceControllerParam => OK" + print(" setImpedanceControllerParam => OK", file=sys.stderr) def demoStartImpedanceController (): # 3. Start impedance - print >> sys.stderr, "3. Start impedance" + print("3. Start impedance", file=sys.stderr) all_start_ret = [] all_mode_ret = [] # start @@ -78,13 +78,13 @@ def demoStartImpedanceController (): # "already start" check for limb in ["rarm", "larm"]: all_start_ret.append(not hcf.ic_svc.startImpedanceControllerNoWait(limb)) - print >> sys.stderr, " all_start_ret = ", all_start_ret, ", all_mode_ret = ", all_mode_ret + print(" all_start_ret = ", all_start_ret, ", all_mode_ret = ", all_mode_ret, file=sys.stderr) assert(all(all_start_ret) and all(all_mode_ret)) - print >> sys.stderr, " startImpedanceController => OK" + print(" startImpedanceController => OK", file=sys.stderr) def demoSetRefForce (): # 4. Set ref force and moment - print >> sys.stderr, "4. Set ref force and moment" + print("4. Set ref force and moment", file=sys.stderr) hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, @@ -112,7 +112,7 @@ def demoSetRefForce (): def demoStopImpedanceController (): # 5. Stop impedance - print >> sys.stderr, "5. Stop impedance" + print("5. Stop impedance", file=sys.stderr) all_stop_ret = [] all_mode_ret = [] # stop @@ -126,13 +126,13 @@ def demoStopImpedanceController (): # "already stop" check for limb in ["rarm", "larm"]: all_stop_ret.append(not hcf.ic_svc.stopImpedanceControllerNoWait(limb)) - print >> sys.stderr, " all_stop_ret = ", all_stop_ret, ", all_mode_ret = ", all_mode_ret + print(" all_stop_ret = ", all_stop_ret, ", all_mode_ret = ", all_mode_ret, file=sys.stderr) assert(all(all_stop_ret) and all(all_mode_ret)) - print >> sys.stderr, " stopImpedanceController => OK" + print(" stopImpedanceController => OK", file=sys.stderr) def demoArmTrackingCheck (): # 6. Arm tracking check - print >> sys.stderr, "6. Arm tracking check" + print("6. Arm tracking check", file=sys.stderr) hcf.ic_svc.startImpedanceController("rarm") hcf.setJointAngle("RARM_ELBOW", -40.0, 0.5); hcf.waitInterpolation() @@ -142,7 +142,7 @@ def demoArmTrackingCheck (): def demoWorldFrameCheck (): # 7. World frame check if hcf.kinematics_only_mode: - print >> sys.stderr, "7. World frame check" + print("7. World frame check", file=sys.stderr) # tempolarily set use_sh_base_pos_rpy icp=hcf.ic_svc.getImpedanceControllerParam("rarm")[1] icp.use_sh_base_pos_rpy = True @@ -157,12 +157,12 @@ def demoWorldFrameCheck (): hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.seq_svc.waitInterpolation() else: - print >> sys.stderr, "7. World frame check is not executed in non-kinematics-only-mode" + print("7. World frame check is not executed in non-kinematics-only-mode", file=sys.stderr) def demoWorldFrameRefForceCheck (): # 8. World frame ref-force check if hcf.kinematics_only_mode: - print >> sys.stderr, "8. World frame ref-force check" + print("8. World frame ref-force check", file=sys.stderr) # tempolarily set use_sh_base_pos_rpy icp=hcf.ic_svc.getImpedanceControllerParam("rarm")[1] icp.use_sh_base_pos_rpy = True @@ -195,11 +195,11 @@ def demoWorldFrameRefForceCheck (): hcf.seq_svc.setBaseRpy([0,0,0], 1.0); hcf.seq_svc.waitInterpolation(); else: - print >> sys.stderr, "8. World frame ref-force check is not executed in non-kinematics-only-mode" + print("8. World frame ref-force check is not executed in non-kinematics-only-mode", file=sys.stderr) def demoOCTDCheck (): # 1. Object Contact Turnaround Detector set param check - print >> sys.stderr, "1. Object Contact Turnaround Detector set param check" + print("1. Object Contact Turnaround Detector set param check", file=sys.stderr) ret9 = True detect_time_thre = 0.3 start_time_thre=0.3 @@ -209,10 +209,10 @@ def demoOCTDCheck (): octdp.start_time_thre = start_time_thre + number_disturbance hcf.octd_svc.setObjectContactTurnaroundDetectorParam(octdp); octdp2=hcf.octd_svc.getObjectContactTurnaroundDetectorParam()[1]; - print >> sys.stderr, " ", octdp2 + print(" ", octdp2, file=sys.stderr) ret9 = ret9 and (octdp2.detect_time_thre == detect_time_thre and octdp2.start_time_thre == start_time_thre) assert(ret9) - print >> sys.stderr, " => OK" + print(" => OK", file=sys.stderr) def demo(): diff --git a/sample/SampleRobot/samplerobot_kalman_filter.py b/sample/SampleRobot/samplerobot_kalman_filter.py index 0b9405cf37d..9fa7f64bb6c 100755 --- a/sample/SampleRobot/samplerobot_kalman_filter.py +++ b/sample/SampleRobot/samplerobot_kalman_filter.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -57,7 +57,7 @@ def init (): roll_pitch_pose3=[0.463158,0.281851,-0.0701,0.747965,-0.514677,-0.108534,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.486068,0.189331,-0.083976,1.08676,-0.76299,-0.139173,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] roll_pitch_poses = [roll_pitch_pose1, roll_pitch_pose2, roll_pitch_pose3] hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) def test_kf_plot (test_motion_func, optional_out_file_name): # time [s] # Reset KF and store data during motion @@ -92,7 +92,7 @@ def test_kf_plot (test_motion_func, optional_out_file_name): # time [s] "Actual yaw", "Estimated base yaw")) plt.savefig("/tmp/test-kf-samplerobot-data-{0}.eps".format(optional_out_file_name)) except: - print >> sys.stderr, "No plot" + print("No plot", file=sys.stderr) def test_bending_common (time, poses): hcf.seq_svc.setJointAngles(poses[1], time*0.25) @@ -131,14 +131,14 @@ def test_walk (): hcf.abc_svc.waitFootSteps() def demoGetKalmanFilterParameter(): - print >> sys.stderr, "1. getParameter" + print("1. getParameter", file=sys.stderr) ret=hcf.kf_svc.getKalmanFilterParam() if ret[0]: - print >> sys.stderr, " getKalmanFilterParam() => OK" + print(" getKalmanFilterParam() => OK", file=sys.stderr) assert(ret[0]) def demoSetKalmanFilterParameter(): - print >> sys.stderr, "2. setParameter" + print("2. setParameter", file=sys.stderr) kfp=hcf.kf_svc.getKalmanFilterParam()[1] kfp.Q_angle = 0.001; kfp.Q_rate = 0.003; @@ -147,7 +147,7 @@ def demoSetKalmanFilterParameter(): kfp2=hcf.kf_svc.getKalmanFilterParam()[1] ret2 = ret and kfp.Q_angle == kfp2.Q_angle and kfp.Q_rate == kfp2.Q_rate and kfp.R_angle == kfp2.R_angle if ret2: - print >> sys.stderr, " setKalmanFilterParam() => OK" + print(" setKalmanFilterParam() => OK", file=sys.stderr) assert(ret2) def demo(): diff --git a/sample/SampleRobot/samplerobot_reference_force_updater.py b/sample/SampleRobot/samplerobot_reference_force_updater.py index d4249cc7b2f..28cf3c83f59 100755 --- a/sample/SampleRobot/samplerobot_reference_force_updater.py +++ b/sample/SampleRobot/samplerobot_reference_force_updater.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -21,35 +21,35 @@ def init (): hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) if hcf.rfu != None: hcf.connectLoggerPort(hcf.rfu, 'ref_rhsensorOut') hcf.connectLoggerPort(hcf.rfu, 'ref_lhsensorOut') # demo functions def demoGetReferecenForceUpdateParam (): - print >> sys.stderr, "1. getParam" + print("1. getParam", file=sys.stderr) for limb_name in ['rarm', 'larm']: [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam(limb_name) assert(ret) [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm2') # Invalid name assert(not ret) - print >> sys.stderr, " =>OK" + print(" =>OK", file=sys.stderr) def demoSetReferecenForceUpdateParam (): - print >> sys.stderr, "2. setParam" - print >> sys.stderr, " Valid limb access" + print("2. setParam", file=sys.stderr) + print(" Valid limb access", file=sys.stderr) for limb_name in ['rarm', 'larm']: [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam(limb_name) ret = hcf.rfu_svc.setReferenceForceUpdaterParam(limb_name, rfup) assert(ret) - print >> sys.stderr, " Invalid limb access" + print(" Invalid limb access", file=sys.stderr) ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm2', rfup) # Invalid name assert(not ret) - print >> sys.stderr, " =>OK" + print(" =>OK", file=sys.stderr) def demoReferenceForceUpdater (): - print >> sys.stderr, "3. Reference Force Update" + print("3. Reference Force Update", file=sys.stderr) import numpy as np i=1; #print >> sys.stderr, i,". get param";i+=1 @@ -68,40 +68,40 @@ def demoReferenceForceUpdater (): hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop) for armName,portName in zip(['rarm', 'larm'],['ref_rhsensorOut','ref_lhsensorOut']): hcf.rfu_svc.setReferenceForceUpdaterParam(armName,p) - print >> sys.stderr, " 3."+str(i)+". set ref_force from seq [10,0,0]";i+=1 + print(" 3."+str(i)+". set ref_force from seq [10,0,0]", file=sys.stderr);i+=1 # set ref_force from seq hcf.seq_svc.setWrenches([0]*12+[10,0,0,0,0,0]*2,1);time.sleep(1) portData=checkDataPortFromLog(portName) - print >> sys.stderr, portName,portData[0:3] + print(portName,portData[0:3], file=sys.stderr) ret = np.linalg.norm(portData) > 9.9; assert (ret) # start/stop rfu - print >> sys.stderr, " 3."+str(i)+". start/stop param for " + armName; i+=1 + print(" 3."+str(i)+". start/stop param for " + armName, file=sys.stderr); i+=1 ##start rfu hcf.rfu_svc.startReferenceForceUpdater(armName);time.sleep(1) portData=checkDataPortFromLog(portName) - print >> sys.stderr, portName,portData[0:3] + print(portName,portData[0:3], file=sys.stderr) ret = np.linalg.norm(portData) < 0.1; assert (ret) ##stop rfu hcf.rfu_svc.stopReferenceForceUpdater(armName);time.sleep(1) portData=checkDataPortFromLog(portName) - print >> sys.stderr, portName,portData[0:3] + print(portName,portData[0:3], file=sys.stderr) ret = np.linalg.norm(portData) > 9.9; assert (ret) # reset ref_force from seq - print >> sys.stderr, " 3."+str(i)+". set ref_force from seq [0,0,0]";i+=1 + print(" 3."+str(i)+". set ref_force from seq [0,0,0]", file=sys.stderr);i+=1 hcf.seq_svc.setWrenches([0]*24,1);time.sleep(1) def demoSetReferecenForceUpdateParamWhileActive (): - print >> sys.stderr, "4. setParam while active" - print >> sys.stderr, " 4.0 Start" + print("4. setParam while active", file=sys.stderr) + print(" 4.0 Start", file=sys.stderr) hcf.rfu_svc.startReferenceForceUpdater('rarm') - print >> sys.stderr, " 4.1 Check setParam without any changes" + print(" 4.1 Check setParam without any changes", file=sys.stderr) [ret, rfup_org] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm') ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup_org) assert(ret) - print >> sys.stderr, " 4.2 Check setParam which cannot be changed while active" + print(" 4.2 Check setParam which cannot be changed while active", file=sys.stderr) [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm') rfup.frame = 'world' ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) @@ -114,39 +114,39 @@ def demoSetReferecenForceUpdateParamWhileActive (): rfup.update_time_ratio = rfup.update_time_ratio*10 ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) assert(not ret) - print >> sys.stderr, " 4.3 Check setParam which can be changed while active" + print(" 4.3 Check setParam which can be changed while active", file=sys.stderr) [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm') rfup.motion_dir = tmp_value = [0,0,-1] ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) - print >> sys.stderr, " motion_dir ..." + print(" motion_dir ...", file=sys.stderr) assert(ret and (map (lambda x,y : abs(x-y)<1e-5, hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].motion_dir, tmp_value))) rfup.p_gain = tmp_value = rfup.p_gain*0.1 ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) - print >> sys.stderr, " p_gain ..." + print(" p_gain ...", file=sys.stderr) assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].p_gain-tmp_value) < 1e-5)) rfup.d_gain = tmp_value = rfup.d_gain*0.1 ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) - print >> sys.stderr, " d_gain ..." + print(" d_gain ...", file=sys.stderr) assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].d_gain-tmp_value) < 1e-5)) rfup.i_gain = rfup.i_gain*0.1 ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) - print >> sys.stderr, " i_gain ..." + print(" i_gain ...", file=sys.stderr) assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].i_gain-tmp_value) < 1e-5)) rfup.is_hold_value = tmp_value = not rfup.is_hold_value ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) - print >> sys.stderr, " is_hold_value ..." + print(" is_hold_value ...", file=sys.stderr) assert(ret and hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].is_hold_value == tmp_value) - print >> sys.stderr, " 4.4 Stop" + print(" 4.4 Stop", file=sys.stderr) hcf.rfu_svc.stopReferenceForceUpdater('rarm') hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup_org) - print >> sys.stderr, " =>OK" + print(" =>OK", file=sys.stderr) def demoReferecenForceUpdateParamFootOriginExtMoment (): - print >> sys.stderr, "5. FootOriginExtMoment" + print("5. FootOriginExtMoment", file=sys.stderr) ret = hcf.rfu_svc.startReferenceForceUpdater('footoriginextmoment') ret = hcf.rfu_svc.stopReferenceForceUpdater('footoriginextmoment') and ret assert(ret) - print >> sys.stderr, " =>OK" + print(" =>OK", file=sys.stderr) def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-reference-force-updater-check-port"): hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py b/sample/SampleRobot/samplerobot_remove_force_offset.py index 35b7213e9c0..02be8be248e 100755 --- a/sample/SampleRobot/samplerobot_remove_force_offset.py +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -23,7 +23,7 @@ def init (): hcf.seq_svc.setJointAngles(initial_pose, 2.5) hcf.waitInterpolation() hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-remove-force-offset-check-param"): hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) @@ -34,23 +34,23 @@ def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-remove-for return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) def demoGetForceMomentOffsetParam (): - print >> sys.stderr, "1. GetForceMomentOffsetParam" + print("1. GetForceMomentOffsetParam", file=sys.stderr) for fs_name in ["rhsensor", "lhsensor"]: ret = hcf.rmfo_svc.getForceMomentOffsetParam(fs_name) if ret[0]: - print >> sys.stderr, " getForceMomentOffsetParam('", fs_name,"') => OK" + print(" getForceMomentOffsetParam('", fs_name,"') => OK", file=sys.stderr) assert(ret[0] is True) def demoSetForceMomentOffsetParam (): - print >> sys.stderr, "2. SetForceMomentOffsetParam" - print >> sys.stderr, " Force and moment are large because of link offsets" + print("2. SetForceMomentOffsetParam", file=sys.stderr) + print(" Force and moment are large because of link offsets", file=sys.stderr) saveLogForCheckParameter() for fs_name in ["rhsensor", "lhsensor"]: fm = numpy.linalg.norm(checkParameterFromLog("off_"+fs_name, save_log=False)) vret = fm > 5e-2 - print >> sys.stderr, " no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret + print(" no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret, file=sys.stderr) assert(vret) - print >> sys.stderr, " Set link offsets (link_offset_centroid and link_offset_mass are identified value)." + print(" Set link offsets (link_offset_centroid and link_offset_mass are identified value).", file=sys.stderr) # Get param r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1] r_fmop.link_offset_centroid = [0,0.0368,-0.076271] @@ -64,23 +64,23 @@ def demoSetForceMomentOffsetParam (): # Check values ret = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor") if ret[0] and ret[1].link_offset_mass == r_fmop.link_offset_mass and ret[1].link_offset_centroid == r_fmop.link_offset_centroid: - print >> sys.stderr, " getForceMomentOffsetParam('rhsensor') => OK" + print(" getForceMomentOffsetParam('rhsensor') => OK", file=sys.stderr) assert((ret[0] and ret[1].link_offset_mass == r_fmop.link_offset_mass and ret[1].link_offset_centroid == r_fmop.link_offset_centroid)) ret = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor") if ret[0] and ret[1].link_offset_mass == l_fmop.link_offset_mass and ret[1].link_offset_centroid == l_fmop.link_offset_centroid: - print >> sys.stderr, " getForceMomentOffsetParam('lhsensor') => OK" + print(" getForceMomentOffsetParam('lhsensor') => OK", file=sys.stderr) assert((ret[0] and ret[1].link_offset_mass == l_fmop.link_offset_mass and ret[1].link_offset_centroid == l_fmop.link_offset_centroid)) - print >> sys.stderr, " Force and moment are reduced" + print(" Force and moment are reduced", file=sys.stderr) saveLogForCheckParameter() for fs_name in ["rhsensor", "lhsensor"]: fm = numpy.linalg.norm(checkParameterFromLog("off_"+fs_name, save_log=False)) vret = fm < 5e-2 - print >> sys.stderr, " no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret + print(" no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret, file=sys.stderr) assert(vret) def demoDumpLoadForceMomentOffsetParams(): - print >> sys.stderr, "3. Dump and load parameter file" - print >> sys.stderr, " Get and set param" + print("3. Dump and load parameter file", file=sys.stderr) + print(" Get and set param", file=sys.stderr) r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1] r_fmop.link_offset_centroid = [0,0.0368,-0.076271] r_fmop.link_offset_mass = 0.80011 @@ -89,18 +89,18 @@ def demoDumpLoadForceMomentOffsetParams(): l_fmop.link_offset_mass = 0.80011 hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop) hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop) - print >> sys.stderr, " Dump param as file" + print(" Dump param as file", file=sys.stderr) ret = hcf.rmfo_svc.dumpForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat") - print >> sys.stderr, " Value check" + print(" Value check", file=sys.stderr) data_str=filter(lambda x : x.find("lhsensor") >= 0, open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n"))[0] vcheck = map(float, data_str.split(" ")[7:10]) == l_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == l_fmop.link_offset_mass data_str=filter(lambda x : x.find("rhsensor") >= 0, open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n"))[0] vcheck = vcheck and map(float, data_str.split(" ")[7:10]) == r_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == r_fmop.link_offset_mass import os if ret and os.path.exists("/tmp/test-rmfo-offsets.dat") and vcheck: - print >> sys.stderr, " dumpForceMomentOffsetParams => OK" + print(" dumpForceMomentOffsetParams => OK", file=sys.stderr) assert((ret and os.path.exists("/tmp/test-rmfo-offsets.dat") and vcheck)) - print >> sys.stderr, " Resetting values" + print(" Resetting values", file=sys.stderr) r_fmop2 = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1] r_fmop2.link_offset_centroid = [0,0,0] r_fmop2.link_offset_mass = 0 @@ -109,23 +109,23 @@ def demoDumpLoadForceMomentOffsetParams(): l_fmop2.link_offset_mass = 0 hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop2) hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop2) - print >> sys.stderr, " Load from file" + print(" Load from file", file=sys.stderr) ret = hcf.rmfo_svc.loadForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat") r_fmop3 = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1] l_fmop3 = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1] vcheck = r_fmop3.link_offset_mass == r_fmop.link_offset_mass and r_fmop3.link_offset_centroid == r_fmop.link_offset_centroid and l_fmop3.link_offset_mass == l_fmop.link_offset_mass and l_fmop3.link_offset_centroid == l_fmop.link_offset_centroid if ret and vcheck: - print >> sys.stderr, " loadForceMomentOffsetParams => OK" + print(" loadForceMomentOffsetParams => OK", file=sys.stderr) assert((ret and vcheck)) def demoRemoveForceSensorOffsetRMFO(): - print >> sys.stderr, "4. remove force sensor offset" - print >> sys.stderr, " Test valid calibration" + print("4. remove force sensor offset", file=sys.stderr) + print(" Test valid calibration", file=sys.stderr) ret = hcf.removeForceSensorOffsetRMFO(tm=1.0) # all sensors by default - print >> sys.stderr, " Test invalid calibration" + print(" Test invalid calibration", file=sys.stderr) ret = ret and not hcf.removeForceSensorOffsetRMFO(["testtest"], 1.0) # invalid sensor name if ret: - print >> sys.stderr, " removeforcesensorlinkoffset => OK" + print(" removeforcesensorlinkoffset => OK", file=sys.stderr) assert(ret) def demo(): diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py index b43db9a155d..5eecb0494cf 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -46,7 +46,7 @@ def init (): 'optionaldata':[0,1,0,0,0.1,0.1,0.1,0.1] # non realistic value } hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) hcf.seq_svc.removeJointGroup('larm') hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); hcf.seq_svc.waitInterpolation(); @@ -85,7 +85,7 @@ def checkServoStateFromLog(log_fname="/tmp/test-samplerobot-sequence-player-chec for l in fp: for s in map(int, l.split()[1:-1]): if s != 7: - print l + print(l) ret = False return ret @@ -95,44 +95,44 @@ def checkJointAngles (var_doc, eps=1e-7): else: p = var_doc['pos'] ret = checkArrayEquality(hcf.sh_svc.getCommand().jointRefs, p, eps) - print " pos => ", ret + print(" pos => ", ret) assert(ret is True) def checkJointAnglesBetween(from_doc, to_doc, eps=1e-7): p0 = from_doc if isinstance(from_doc, list) else from_doc['pos'] p1 = to_doc if isinstance( to_doc, list) else to_doc['pos'] ret = checkArrayBetween(p0, hcf.sh_svc.getCommand().jointRefs, p1, eps) - print " pos => ", ret + print(" pos => ", ret) assert(ret is True) def checkZmp(var_doc): zmp=hcf.sh_svc.getCommand().zmp ret = checkArrayEquality([zmp[0], zmp[1], zmp[2]], var_doc['zmp']) - print " zmp => ", ret + print(" zmp => ", ret) assert(ret is True) def checkWaist(var_doc, save_log=True): bpos=checkParameterFromLog("basePosOut", save_log=save_log) brpy=checkParameterFromLog("baseRpyOut", save_log=False) ret = checkArrayEquality([bpos[0], bpos[1], bpos[2], brpy[0], brpy[1], brpy[2]], var_doc['waist'], eps=1e-5) - print " waist => ", ret + print(" waist => ", ret) assert(ret is True) def checkTorque (var_doc, save_log=True): ret = checkArrayEquality(checkParameterFromLog("tqOut", save_log=save_log), var_doc['torque'], eps=1e-5) - print " torque => ", ret + print(" torque => ", ret) assert(ret is True) def checkWrenches (var_doc, save_log=True): if save_log: saveLogForCheckParameter() ret = checkArrayEquality(reduce(lambda x,y:x+y, map(lambda fs : checkParameterFromLog(fs+"Out", save_log=False), ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])), var_doc['wrenches'], eps=1e-5) - print " wrenches => ", ret + print(" wrenches => ", ret) assert(ret is True) def checkOptionalData (var_doc, save_log=True): ret = checkArrayEquality(checkParameterFromLog("optionalDataOut", save_log=save_log), var_doc['optionaldata'], eps=1e-5) - print " optionaldata => ", ret + print(" optionaldata => ", ret) assert(ret is True) def checkRobotState (var_doc): @@ -146,7 +146,7 @@ def checkRobotState (var_doc): # demo functions def demoSetJointAngles(): - print >> sys.stderr, "1. setJointAngles" + print("1. setJointAngles", file=sys.stderr) hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 1.0); hcf.seq_svc.waitInterpolation(); checkJointAngles(move_base_pose_doc) @@ -154,7 +154,7 @@ def demoSetJointAngles(): hcf.seq_svc.waitInterpolation(); checkJointAngles(reset_pose_doc) # check override - print >> sys.stderr, " check override" + print(" check override", file=sys.stderr) hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 5.0); time.sleep(2.5) hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); @@ -163,14 +163,14 @@ def demoSetJointAngles(): # check clear if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'): return - print >> sys.stderr, " check clear" + print(" check clear", file=sys.stderr) hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 5.0); time.sleep(2.5) hcf.seq_svc.clearJointAngles() checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc) def demoSetJointAnglesSequence(): - print >> sys.stderr, "2. setJointAnglesSequence" + print("2. setJointAnglesSequence", file=sys.stderr) hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,1.0]); hcf.seq_svc.waitInterpolation(); checkJointAngles(move_base_pose_doc) @@ -178,21 +178,21 @@ def demoSetJointAnglesSequence(): hcf.seq_svc.waitInterpolation(); checkJointAngles(reset_pose_doc) # check override - print >> sys.stderr, " check override" + print(" check override", file=sys.stderr) hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,5.0]) time.sleep(3.5) hcf.seq_svc.setJointAnglesSequence([reset_pose_doc['pos'],move_base_pose_doc['pos'],reset_pose_doc['pos']], [1.0,1.0,1.0]); hcf.seq_svc.waitInterpolation(); checkJointAngles(reset_pose_doc) # check clear - print >> sys.stderr, " check clear" + print(" check clear", file=sys.stderr) hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,5.0]) time.sleep(3.5) hcf.seq_svc.clearJointAngles() checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc) def demoSetJointAngle(): - print >> sys.stderr, "3. setJointAngle" + print("3. setJointAngle", file=sys.stderr) hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); hcf.seq_svc.waitInterpolation(); hcf.seq_svc.setJointAngle("WAIST_R", 10*3.14159/180.0, 1.0); @@ -220,7 +220,7 @@ def demoSetJointAngle(): # checkJointAnglesBetween(reset_pose_doc,p) def demoLoadPattern(): - print >> sys.stderr, "4. loadPattern" + print("4. loadPattern", file=sys.stderr) # dump pattern doc as loadPattern file dumpLoadPatternTestFile("/tmp/test-samplerobot-move-base-pose", move_base_pose_doc, 2.0); dumpLoadPatternTestFile("/tmp/test-samplerobot-reset-pose", reset_pose_doc, 2.0); @@ -233,7 +233,7 @@ def demoLoadPattern(): checkRobotState(reset_pose_doc) def demoSetZmp (): - print >> sys.stderr, "5. setZmp" + print("5. setZmp", file=sys.stderr) hcf.seq_svc.setZmp(move_base_pose_doc['zmp'], 1.0); hcf.seq_svc.waitInterpolation(); checkZmp(move_base_pose_doc) @@ -242,7 +242,7 @@ def demoSetZmp (): checkZmp(reset_pose_doc) def demoSetBasePosRpy (): - print >> sys.stderr, "6. setBasePos and setBaseRpy" + print("6. setBasePos and setBaseRpy", file=sys.stderr) hcf.seq_svc.setBasePos(move_base_pose_doc['waist'][0:3], 1.0); hcf.seq_svc.setBaseRpy(move_base_pose_doc['waist'][3:6], 1.0); hcf.seq_svc.waitInterpolation(); @@ -253,7 +253,7 @@ def demoSetBasePosRpy (): checkWaist(reset_pose_doc) def demoSetWrenches (): - print >> sys.stderr, "7. setWrenches" + print("7. setWrenches", file=sys.stderr) hcf.seq_svc.setWrenches(move_base_pose_doc['wrenches'], 1.0); hcf.seq_svc.waitInterpolation(); checkWrenches(move_base_pose_doc) @@ -262,7 +262,7 @@ def demoSetWrenches (): checkWrenches(reset_pose_doc) def demoSetJointAnglesOfGroup(): - print >> sys.stderr, "8. setJointAnglesOfGroup" + print("8. setJointAnglesOfGroup", file=sys.stderr) hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']) larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0] larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0] @@ -280,7 +280,7 @@ def demoSetJointAnglesOfGroup(): p1[i+19] = larm_pos1[i] checkJointAngles(p1) # check override - print >> sys.stderr, " check override" + print(" check override", file=sys.stderr) hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0); time.sleep(2.5) @@ -290,7 +290,7 @@ def demoSetJointAnglesOfGroup(): # check clear if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'): return - print >> sys.stderr, " check clear (clearJointAnglesOfGroup)" + print(" check clear (clearJointAnglesOfGroup)", file=sys.stderr) hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0); time.sleep(2.5) hcf.seq_svc.clearJointAnglesOfGroup('larm') @@ -300,7 +300,7 @@ def demoSetJointAnglesOfGroup(): hcf.seq_svc.waitInterpolationOfGroup('larm') checkJointAngles(p1) - print >> sys.stderr, " check clear clearOfGroup" + print(" check clear clearOfGroup", file=sys.stderr) hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0); time.sleep(2.5) hcf.seq_svc.clearOfGroup('larm', 0.0) @@ -311,7 +311,7 @@ def demoSetJointAnglesOfGroup(): checkJointAngles(p1) def demoSetJointAnglesSequenceOfGroup(): - print >> sys.stderr, "9. setJointAnglesOfGroup" + print("9. setJointAnglesOfGroup", file=sys.stderr) hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']) larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0] larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0] @@ -329,7 +329,7 @@ def demoSetJointAnglesSequenceOfGroup(): p1[i+19] = larm_pos1[i] checkJointAngles(p1) # check override - print >> sys.stderr, " check override" + print(" check override", file=sys.stderr) hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos0, larm_pos1, larm_pos0], [1.0, 1.0, 5.0]); time.sleep(3.5) @@ -337,7 +337,7 @@ def demoSetJointAnglesSequenceOfGroup(): hcf.seq_svc.waitInterpolationOfGroup('larm'); checkJointAngles(p1) # check clear - print >> sys.stderr, " check clear" + print(" check clear", file=sys.stderr) hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos0, larm_pos1, larm_pos0], [1.0, 1.0, 5.0]); time.sleep(3.5) hcf.seq_svc.clearJointAnglesOfGroup('larm') @@ -345,7 +345,7 @@ def demoSetJointAnglesSequenceOfGroup(): hcf.seq_svc.removeJointGroup('larm') def demoSetJointAnglesSequenceFull(): - print >> sys.stderr, "10. setJointAnglesSequenceFull" + print("10. setJointAnglesSequenceFull", file=sys.stderr) hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']], [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']], @@ -371,7 +371,7 @@ def demoSetJointAnglesSequenceFull(): hcf.waitInterpolation(); checkRobotState(reset_pose_doc) # check override - print >> sys.stderr, " check override" + print(" check override", file=sys.stderr) hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']], [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']], @@ -396,7 +396,7 @@ def demoSetJointAnglesSequenceFull(): hcf.waitInterpolation() checkRobotState(reset_pose_doc) # check clear - print >> sys.stderr, " check clear" + print(" check clear", file=sys.stderr) hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']], [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']], @@ -415,7 +415,7 @@ def demoSetTargetPose(): # reset wait position hcf.seq_svc.setBasePos([0.000000, 0.000000, 0.723500], 1.0); hcf.seq_svc.setBaseRpy([0.000000, 0.000000, 0.000000], 1.0); - print >> sys.stderr, "11. setTargetPose" + print("11. setTargetPose", file=sys.stderr) hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']) larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0] larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.785395, -0.636277, 0.0, 0.0] @@ -443,36 +443,36 @@ def demoSetTargetPose(): p1[i+19] = larm_pos1[i] checkJointAngles(p1) - print >> sys.stderr, " check setTargetPose" + print(" check setTargetPose", file=sys.stderr) hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm'); - print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0) + print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)) checkJointAngles(p0, 0.01) clearLogForCheckParameter(5000) hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm'); - print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1) + print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)) checkJointAngles(p1, 0.01) assert(checkServoStateFromLog() is True) hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0); hcf.seq_svc.waitInterpolationOfGroup('larm'); - print >> sys.stderr, " check setTargetPose without giving reference frame" + print(" check setTargetPose without giving reference frame", file=sys.stderr) hcf.seq_svc.setTargetPose('larm', pos0_world, rpy0_world, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm'); - print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0) + print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)) checkJointAngles(p0, 0.01) clearLogForCheckParameter(5000) hcf.seq_svc.setTargetPose('larm', pos1_world, rpy1_world, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm'); - print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1) + print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)) checkJointAngles(p1, 0.01) assert(checkServoStateFromLog() is True) - print >> sys.stderr, " check clear clearOfGroup" + print(" check clear clearOfGroup", file=sys.stderr) hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0) time.sleep(0.5) hcf.seq_svc.clearOfGroup('larm', 0.0) @@ -481,7 +481,7 @@ def demoSetTargetPose(): clearLogForCheckParameter(5000) hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm') - print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1) + print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)) checkJointAngles(p1, 0.1) assert(checkServoStateFromLog() is True) hcf.seq_svc.removeJointGroup('larm') diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index 766c9dfc59c..c2ac25262e4 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -37,7 +37,7 @@ def init (): hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.seq_svc.waitInterpolation() hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) def demo (): init() @@ -49,7 +49,7 @@ def demo (): demoErrorLimit() def demoTestAllLimitTables(): - print >> sys.stderr, "1. demo all jointLimitTables" + print("1. demo all jointLimitTables", file=sys.stderr) for table_idx in range(len(limit_table_list)/6): testLimitTables(table_idx, True, 5) @@ -80,8 +80,8 @@ def testLimitTables (table_idx=0, debug=True, loop_mod=1): self_llimits, self_ulimits] = getJointLimitTableInfo(table_idx) lret = testOneLimitTable(self_jointId, target_jointId, self_llimits, target_llimit, target_ulimit, -1, debug, loop_mod) uret = testOneLimitTable(self_jointId, target_jointId, self_ulimits, target_llimit, target_ulimit, 1, debug, loop_mod) - print >> sys.stderr, "lower limit check(", self_joint_name, ",", target_joint_name,")=", lret - print >> sys.stderr, "upper limit check(", self_joint_name, ",", target_joint_name,")=", uret + print("lower limit check(", self_joint_name, ",", target_joint_name,")=", lret, file=sys.stderr) + print("upper limit check(", self_joint_name, ",", target_joint_name,")=", uret, file=sys.stderr) assert(lret) assert(uret) @@ -92,7 +92,7 @@ def testOneLimitTable (self_jointId, target_jointId, limit_table, target_llimit, if idx%loop_mod != 0: # skip if loop_mod is specified continue if debug: - print "idx=",idx, + print("idx=",idx, end=' ') # A-1. set safe joint tmp_pose[target_jointId]=deg2rad(target_llimit + idx); tmp_pose[self_jointId]=deg2rad(limit_table[idx]); @@ -120,8 +120,8 @@ def testOneLimitTable (self_jointId, target_jointId, limit_table, target_llimit, ret2 = ret2 and abs(el_out2[self_jointId] - (limit_table[idx]+angle_violation)) > thre # Check result value is not violated value # C. results if debug: - print " ret = (", ret1, ",", ret2,")" - print " self=(o1=", rad2deg(el_out1[self_jointId]), ", o2=", rad2deg(el_out2[self_jointId]), ", limit=", limit_table[idx], ") ", " target=(o1=", rad2deg(el_out1[target_jointId]), ", o2=", rad2deg(el_out2[target_jointId]), ", limit=", target_llimit + idx, ") [deg]" + print(" ret = (", ret1, ",", ret2,")") + print(" self=(o1=", rad2deg(el_out1[self_jointId]), ", o2=", rad2deg(el_out2[self_jointId]), ", limit=", limit_table[idx], ") ", " target=(o1=", rad2deg(el_out1[target_jointId]), ", o2=", rad2deg(el_out2[target_jointId]), ", limit=", target_llimit + idx, ") [deg]") ret.append(ret1); ret.append(ret2); hcf.seq_svc.waitInterpolation() @@ -130,7 +130,7 @@ def testOneLimitTable (self_jointId, target_jointId, limit_table, target_llimit, return all(ret) def setAndCheckJointLimit (joint_name): - print >> sys.stderr, " ", joint_name + print(" ", joint_name, file=sys.stderr) # ulimit check link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0] hcf.seq_svc.setJointAngle(joint_name, math.radians(1)+link_info.ulimit[0], 1) @@ -140,7 +140,7 @@ def setAndCheckJointLimit (joint_name): # Use RobotHardware's command as SoftErrorLimiter joint angle output tmppose = hcf.getActualState().command ret = tmppose[link_info.jointId] <= link_info.ulimit[0] - print >> sys.stderr, " ulimit = ", ret, "(elout=", tmppose[link_info.jointId], ", limit=", link_info.ulimit[0], ")" + print(" ulimit = ", ret, "(elout=", tmppose[link_info.jointId], ", limit=", link_info.ulimit[0], ")", file=sys.stderr) assert(ret) # llimit check hcf.seq_svc.setJointAngle(joint_name, math.radians(-1)+link_info.llimit[0], 1) @@ -150,14 +150,14 @@ def setAndCheckJointLimit (joint_name): # Use RobotHardware's command as SoftErrorLimiter joint angle output tmppose = hcf.getActualState().command ret = tmppose[link_info.jointId] >= link_info.llimit[0] - print >> sys.stderr, " llimit = ", ret, "(elout=", tmppose[link_info.jointId], ", limit=", link_info.llimit[0], ")" + print(" llimit = ", ret, "(elout=", tmppose[link_info.jointId], ", limit=", link_info.llimit[0], ")", file=sys.stderr) assert(ret) # go to initial hcf.seq_svc.setJointAngles(initial_pose, 1.0) hcf.waitInterpolation() def demoPositionLimit(): - print >> sys.stderr, "2. Check Position limit" + print("2. Check Position limit", file=sys.stderr) setAndCheckJointLimit('LARM_WRIST_Y') setAndCheckJointLimit('LARM_WRIST_P') setAndCheckJointLimit('LARM_SHOULDER_P') @@ -166,10 +166,10 @@ def setAndCheckJointVelocityLimit (joint_name, thre=1e-5, dt=0.002): link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0] # lvlimit and uvlimit existence check if not(len(link_info.lvlimit) == 1 and len(link_info.uvlimit) == 1): - print >> sys.stderr, " ", joint_name, " test neglected because no lvlimit and uvlimit are found." + print(" ", joint_name, " test neglected because no lvlimit and uvlimit are found.", file=sys.stderr) return for is_upper_limit in [True, False]: # uvlimit or lvlimit - print >> sys.stderr, " ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit" + print(" ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit", file=sys.stderr) # Disable error limit for checking vel limit hcf.el_svc.setServoErrorLimit("all", 100000) # Test motion and logging @@ -202,19 +202,19 @@ def setAndCheckJointVelocityLimit (joint_name, thre=1e-5, dt=0.002): hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5) hcf.waitInterpolation() # Check flags and print - print >> sys.stderr, " is_reached =", is_reached, ", is_vel_limited =", is_vel_limited, - print >> sys.stderr, ", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_vel =", max_ret_vel, "[rad/s], vel_limit =", vel_limit, "[rad/s]" + print(" is_reached =", is_reached, ", is_vel_limited =", is_vel_limited, end=' ', file=sys.stderr) + print(", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_vel =", max_ret_vel, "[rad/s], vel_limit =", vel_limit, "[rad/s]", file=sys.stderr) assert(is_reached and is_vel_limited) def demoVelocityLimit(): - print >> sys.stderr, "3. Check Velocity limit" + print("3. Check Velocity limit", file=sys.stderr) setAndCheckJointVelocityLimit('LARM_WRIST_Y') setAndCheckJointVelocityLimit('LARM_WRIST_P') def setAndCheckJointErrorLimit (joint_name, thre=1e-5): link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0] for is_upper_limit in [True, False]: # uvlimit or lvlimit - print >> sys.stderr, " ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit" + print(" ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit", file=sys.stderr) # Disable error limit for checking vel limit error_limit = 0.002 if is_upper_limit else -0.002 # [rad] hcf.el_svc.setServoErrorLimit("all", abs(error_limit)) @@ -250,12 +250,12 @@ def setAndCheckJointErrorLimit (joint_name, thre=1e-5): hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5) hcf.waitInterpolation() # Check flags and print - print >> sys.stderr, " is_reached =", is_reached, ", is_err_limited =", is_err_limited, - print >> sys.stderr, ", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_err =", max_ret_err, "[rad], err_limit =", error_limit, "[rad]" + print(" is_reached =", is_reached, ", is_err_limited =", is_err_limited, end=' ', file=sys.stderr) + print(", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_err =", max_ret_err, "[rad], err_limit =", error_limit, "[rad]", file=sys.stderr) assert(is_reached and is_err_limited) def demoErrorLimit(): - print >> sys.stderr, "4. Check Error limit" + print("4. Check Error limit", file=sys.stderr) setAndCheckJointErrorLimit('LARM_WRIST_Y') setAndCheckJointErrorLimit('LARM_WRIST_P') diff --git a/sample/SampleRobot/samplerobot_stabilizer.py b/sample/SampleRobot/samplerobot_stabilizer.py index ae97eede37b..8fcb821b210 100755 --- a/sample/SampleRobot/samplerobot_stabilizer.py +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -26,7 +26,7 @@ def init (): initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] half_sitting_pose = [-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): # on < 315.5.0 this outputs huge error log message hcf.seq_svc.setJointAngles(initial_pose, 2.0) @@ -45,12 +45,12 @@ def calcCOP (): [(cop_info[1]+cop_info[1+3])/(cop_info[2]+cop_info[2+3]),(cop_info[0]+cop_info[0+3])/(cop_info[2]+cop_info[2+3])]] # total ZMP def demoGetParameter(): - print >> sys.stderr, "1. getParameter" + print("1. getParameter", file=sys.stderr) stp = hcf.st_svc.getParameter() - print >> sys.stderr, " getParameter() => OK" + print(" getParameter() => OK", file=sys.stderr) def demoSetParameter(): - print >> sys.stderr, "2. setParameter" + print("2. setParameter", file=sys.stderr) stp_org = hcf.st_svc.getParameter() # for tpcc stp_org.k_tpcc_p=[0.2, 0.2] @@ -88,7 +88,7 @@ def demoSetParameter(): stp = hcf.st_svc.getParameter() vcheck = stp.k_tpcc_p == stp_org.k_tpcc_p and stp.k_tpcc_x == stp_org.k_tpcc_x and stp.k_brot_p == stp_org.k_brot_p if vcheck: - print >> sys.stderr, " setParameter() => OK", vcheck + print(" setParameter() => OK", vcheck, file=sys.stderr) assert(vcheck) def changeContactDecisionThre (thre): @@ -115,14 +115,14 @@ def checkActualBaseAttitude(thre=5.0): # degree ''' act_rpy = checkParameterFromLog("WAIST")[3:] ret = abs(math.degrees(act_rpy[0])) < thre and abs(math.degrees(act_rpy[1])) < thre - print >> sys.stderr, " ret = ", ret, ", actual base rpy = (", act_rpy, ")" + print(" ret = ", ret, ", actual base rpy = (", act_rpy, ")", file=sys.stderr) return ret def printActualBase(): '''Print actual base pos and rot ''' act_base = checkParameterFromLog("WAIST") - print >> sys.stderr, " actual base pos = ", act_base[0:3], "[m], actual base rpy = ", act_base[3:], "[rad]" + print(" actual base pos = ", act_base[0:3], "[m], actual base rpy = ", act_base[3:], "[rad]", file=sys.stderr) def changeSTAlgorithm (new_st_alg): stp = hcf.st_svc.getParameter() @@ -136,7 +136,7 @@ def changeSTAlgorithm (new_st_alg): hcf.waitInterpolation() def demoSTLoadPattern (): - print >> sys.stderr, "3. EEFMQP st + SequencePlayer loadPattern" + print("3. EEFMQP st + SequencePlayer loadPattern", file=sys.stderr) if hcf.pdc: # Set initial pose of samplerobot-gopos000 before starting of ST hcf.seq_svc.setJointAnglesSequenceFull([[0.000242, -0.403476, -0.000185, 0.832071, -0.427767, -6.928952e-05, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, 0.000242, -0.403469, -0.000185, 0.832073, -0.427775, -6.928781e-05, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], # jvss @@ -163,13 +163,13 @@ def demoSTLoadPattern (): hcf.waitInterpolation() ret = checkActualBaseAttitude() if ret: - print >> sys.stderr, " ST + loadPattern => OK" + print(" ST + loadPattern => OK", file=sys.stderr) assert(ret) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demoStartStopTPCCST (): - print >> sys.stderr, "4. start and stop TPCC st" + print("4. start and stop TPCC st", file=sys.stderr) if hcf.pdc: # setup controllers hcf.startAutoBalancer() @@ -179,13 +179,13 @@ def demoStartStopTPCCST (): #hcf.abc_svc.waitFootSteps() ret = checkActualBaseAttitude() if ret: - print >> sys.stderr, " Start and Stop Stabilizer => OK" + print(" Start and Stop Stabilizer => OK", file=sys.stderr) assert(ret) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demoStartStopEEFMQPST (): - print >> sys.stderr, "5. start and stop EEFMQP st" + print("5. start and stop EEFMQP st", file=sys.stderr) if hcf.pdc: # setup controllers hcf.startAutoBalancer() @@ -195,13 +195,13 @@ def demoStartStopEEFMQPST (): hcf.abc_svc.waitFootSteps() ret = checkActualBaseAttitude() if ret: - print >> sys.stderr, " Start and Stop Stabilizer => OK" + print(" Start and Stop Stabilizer => OK", file=sys.stderr) assert(ret) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demoSTStairWalk (): - print >> sys.stderr, "6. EEFMQPCOP + stair" + print("6. EEFMQPCOP + stair", file=sys.stderr) if hcf.pdc: # setup controllers printActualBase() @@ -241,13 +241,13 @@ def demoSTStairWalk (): ret = checkActualBaseAttitude() printActualBase() if ret: - print >> sys.stderr, " ST + Stair => OK" + print(" ST + Stair => OK", file=sys.stderr) assert(ret) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demoSTToeHeelWalk (): - print >> sys.stderr, "7. EEFMQPCOP + toeheel" + print("7. EEFMQPCOP + toeheel", file=sys.stderr) if hcf.pdc: # setup controllers hcf.startAutoBalancer() @@ -296,13 +296,13 @@ def demoSTToeHeelWalk (): hcf.co_svc.enableCollisionDetection() ret = checkActualBaseAttitude() if ret: - print >> sys.stderr, " ST + ToeHeel => OK" + print(" ST + ToeHeel => OK", file=sys.stderr) assert(ret) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demoSTTurnWalk (): - print >> sys.stderr, "8. EEFMQPCOP st + Turn walk" + print("8. EEFMQPCOP st + Turn walk", file=sys.stderr) if hcf.pdc: hcf.startAutoBalancer() hcf.co_svc.disableCollisionDetection() @@ -323,15 +323,15 @@ def demoSTTurnWalk (): hcf.co_svc.enableCollisionDetection() ret = checkActualBaseAttitude() if ret: - print >> sys.stderr, " ST + Turnwalk => OK" + print(" ST + Turnwalk => OK", file=sys.stderr) assert(ret) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demoSTTransitionAirGround (): # This example is from YoheiKakiuchi's comment : https://github.com/fkanehiro/hrpsys-base/issues/1098, https://github.com/fkanehiro/hrpsys-base/pull/1102#issuecomment-284609203 - print >> sys.stderr, "9. ST Transition (in the air and on the ground)" + print("9. ST Transition (in the air and on the ground)", file=sys.stderr) if hcf.pdc: # Init stp_org = hcf.st_svc.getParameter() @@ -339,24 +339,24 @@ def demoSTTransitionAirGround (): stp.transition_time = 0.1; # for fast checking hcf.st_svc.setParameter(stp) # Tests - print >> sys.stderr, " 9-1. Check in the air" + print(" 9-1. Check in the air", file=sys.stderr) hcf.startStabilizer() mimicInTheAir() hcf.setJointAngles(hcf.getJointAngles(), stp.transition_time);hcf.waitInterpolation() # Wait transition cmode1 = hcf.st_svc.getParameter().controller_mode vcheck1 = (cmode1 == OpenHRP.StabilizerService.MODE_AIR) - print >> sys.stderr, " 9-2. Check on the ground" + print(" 9-2. Check on the ground", file=sys.stderr) mimicOnTheGround() hcf.setJointAngles(hcf.getJointAngles(), stp.transition_time);hcf.waitInterpolation() # Wait transition cmode2 = hcf.st_svc.getParameter().controller_mode vcheck2 = (cmode2 == OpenHRP.StabilizerService.MODE_ST) - print >> sys.stderr, " 9-3. Check in the air and then stopST" + print(" 9-3. Check in the air and then stopST", file=sys.stderr) mimicInTheAir() hcf.setJointAngles(hcf.getJointAngles(), 0.01);hcf.waitInterpolation() # Wait until in the air flag is invoked in onExecute hcf.stopStabilizer() cmode3 = hcf.st_svc.getParameter().controller_mode vcheck3 = (cmode3 == OpenHRP.StabilizerService.MODE_IDLE) - print >> sys.stderr, " 9-4. Check on the ground" + print(" 9-4. Check on the ground", file=sys.stderr) mimicOnTheGround() hcf.setJointAngles(hcf.getJointAngles(), 0.01);hcf.waitInterpolation() # Wait until on the ground flag is invoked in onExecute hcf.startStabilizer() @@ -365,15 +365,15 @@ def demoSTTransitionAirGround (): # Finsh hcf.st_svc.setParameter(stp_org) vcheck_list = [vcheck1, vcheck2, vcheck3, vcheck4] - print >> sys.stderr, " ST Transition Air Ground vcheck = ", vcheck_list, ", cmode = ", [cmode1, cmode2, cmode3, cmode4] + print(" ST Transition Air Ground vcheck = ", vcheck_list, ", cmode = ", [cmode1, cmode2, cmode3, cmode4], file=sys.stderr) if all(vcheck_list): - print >> sys.stderr, " ST Transition Air Ground => OK" + print(" ST Transition Air Ground => OK", file=sys.stderr) assert(all(vcheck_list)) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demoSTRootRotChange (): - print >> sys.stderr, "10. ST root rot change" + print("10. ST root rot change", file=sys.stderr) if hcf.pdc: # 10deg root_rot_x_pose=[-0.240857,-0.634561,0.012382,1.30211,-0.669201,0.073893,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.232865,-0.555515,0.011753,1.1356,-0.581653,0.06476,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] @@ -385,33 +385,33 @@ def demoSTRootRotChange (): root_rot_xyz_pose=[-0.378611,-0.81283,-0.238181,1.23534,-0.577915,0.061071,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.351695,-0.768514,-0.225097,1.05221,-0.442267,0.050849,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] hcf.startAutoBalancer(); changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP) - print >> sys.stderr, " init" + print(" init", file=sys.stderr) checkActualBaseAttitude() hcf.seq_svc.setJointAngles(root_rot_x_pose, 1.0);hcf.waitInterpolation(); hcf.seq_svc.setJointAngles(root_rot_x_pose, 1.0);hcf.waitInterpolation(); # dummy for wait - print >> sys.stderr, " root rot x done." + print(" root rot x done.", file=sys.stderr) checkActualBaseAttitude() hcf.seq_svc.setJointAngles(root_rot_y_pose, 1.0);hcf.waitInterpolation(); hcf.seq_svc.setJointAngles(root_rot_y_pose, 1.0);hcf.waitInterpolation(); # dummy for wait - print >> sys.stderr, " root rot y done." + print(" root rot y done.", file=sys.stderr) checkActualBaseAttitude() hcf.seq_svc.setJointAngles(root_rot_z_pose, 1.0);hcf.waitInterpolation(); hcf.seq_svc.setJointAngles(root_rot_z_pose, 1.0);hcf.waitInterpolation(); # dummy for wait - print >> sys.stderr, " root rot z done." + print(" root rot z done.", file=sys.stderr) checkActualBaseAttitude() hcf.seq_svc.setJointAngles(root_rot_xyz_pose, 1.0);hcf.waitInterpolation(); hcf.seq_svc.setJointAngles(root_rot_xyz_pose, 1.0);hcf.waitInterpolation(); # dummy for wait hcf.seq_svc.setJointAngles(initial_pose, 1.0);hcf.waitInterpolation(); - print >> sys.stderr, " root rot xyz done." + print(" root rot xyz done.", file=sys.stderr) ret = checkActualBaseAttitude() if ret: - print >> sys.stderr, " ST root rot change => OK" + print(" ST root rot change => OK", file=sys.stderr) assert(ret) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demoSTMimicRouchTerrainWalk (terrain_height_diff = 0.04): - print >> sys.stderr, "11. ST mimic rough terrain walk" + print("11. ST mimic rough terrain walk", file=sys.stderr) if hcf.pdc: hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.22,0.09,terrain_height_diff], [1,0,0,0], "lleg")]), @@ -425,10 +425,10 @@ def demoSTMimicRouchTerrainWalk (terrain_height_diff = 0.04): hcf.abc_svc.waitFootSteps(); ret = checkActualBaseAttitude() if ret: - print >> sys.stderr, " ST mimic rough terrain walk => OK" + print(" ST mimic rough terrain walk => OK", file=sys.stderr) assert(ret) else: - print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + print(" This sample is neglected in High-gain mode simulation", file=sys.stderr) def demo(): @@ -448,7 +448,7 @@ def demo(): demoSTRootRotChange() demoSTMimicRouchTerrainWalk() else: - print >> sys.stderr, "Skip st test because of missing sample1_bush.wrl" + print("Skip st test because of missing sample1_bush.wrl", file=sys.stderr) if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_terrain_walk.py b/sample/SampleRobot/samplerobot_terrain_walk.py index 5a9f474c66e..464294fa858 100755 --- a/sample/SampleRobot/samplerobot_terrain_walk.py +++ b/sample/SampleRobot/samplerobot_terrain_walk.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -59,7 +59,7 @@ def stairWalk(stair_height = 0.1524): # sample for SampleRobot.TerrainFloor.SlopeUpDown.xml def demoSlopeUpDown(): - print "Start stlop up down" + print("Start stlop up down") setupGaitGeneratorParam(True) hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]); fsList=[OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.8,-0.09,0.0], [1.0,0.0,2.775558e-17,0.0], "rleg")]), @@ -86,22 +86,22 @@ def demoSlopeUpDown(): # sample for SampleRobot.TerrainFloor.StairUp.xml def demoStairUp(): - print "Start stair up" + print("Start stair up") stairWalk() # sample for SampleRobot.TerrainFloor.StairDown.xml def demoStairDown(): - print "Start stair down" + print("Start stair down") hcf.abc_svc.goPos(0.05, 0.0, 0.0) hcf.abc_svc.waitFootSteps() stairWalk(-0.1524) def demoStairUpDown(): - print "Start stair up" + print("Start stair up") stairWalk() hcf.abc_svc.goPos(0.05, 0.0, 0.0) hcf.abc_svc.waitFootSteps() - print "Start stair down" + print("Start stair down") stairWalk(-0.1524) if __name__ == '__main__': diff --git a/sample/SampleRobot/samplerobot_virtual_force_sensor.py b/sample/SampleRobot/samplerobot_virtual_force_sensor.py index a5a70e4707e..4c470c11772 100755 --- a/sample/SampleRobot/samplerobot_virtual_force_sensor.py +++ b/sample/SampleRobot/samplerobot_virtual_force_sensor.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -31,26 +31,26 @@ def demo (): # vs check port_names = fsensor_names if all(map (lambda x : hcf.vs.port(x), fsensor_names)): - print hcf.vs.name(), "ports are OK (", port_names, ")" + print(hcf.vs.name(), "ports are OK (", port_names, ")") # seq check port_names = map (lambda x : x+"Ref", fsensor_names) if all(map (lambda x : hcf.seq.port(x), port_names)): - print hcf.seq.name(), "ports are OK (", port_names, ")" + print(hcf.seq.name(), "ports are OK (", port_names, ")") # sh check port_names = map (lambda x : x+"Out", fsensor_names) if all(map (lambda x : hcf.sh.port(x), port_names)): - print hcf.sh.name(), "ports are OK (", port_names, ")" + print(hcf.sh.name(), "ports are OK (", port_names, ")") port_names = map (lambda x : x+"In", fsensor_names) if all(map (lambda x : hcf.sh.port(x), port_names)): - print hcf.sh.name(), "ports are OK (", port_names, ")" + print(hcf.sh.name(), "ports are OK (", port_names, ")") # ic check port_names = map (lambda x : "ref_"+x+"In", fsensor_names) if all(map (lambda x : hcf.ic.port(x), port_names)): - print hcf.ic.name(), "ports are OK (", port_names, ")" + print(hcf.ic.name(), "ports are OK (", port_names, ")") # abc check port_names = map (lambda x : "ref_"+x+"In", fsensor_names) if all(map (lambda x : hcf.ic.port(x), port_names)): - print hcf.ic.name(), "ports are OK (", port_names, ")" + print(hcf.ic.name(), "ports are OK (", port_names, ")") # 2. Test impedance controller ret1=hcf.ic_svc.getImpedanceControllerParam("vrhsensor") @@ -75,7 +75,7 @@ def demo (): 0,0,0,0,0,0,], 2.0); hcf.seq_svc.waitInterpolation(); hcf.ic_svc.deleteImpedanceController("vrhsensor") - print "test ImpedanceController for virtual force sensor => OK" + print("test ImpedanceController for virtual force sensor => OK") if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_walk.py b/sample/SampleRobot/samplerobot_walk.py index 7172ca00e38..ebdc82d5a5b 100755 --- a/sample/SampleRobot/samplerobot_walk.py +++ b/sample/SampleRobot/samplerobot_walk.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * diff --git a/sample/SampleSpecialJointRobot/samplespecialjointrobot_auto_balancer.py b/sample/SampleSpecialJointRobot/samplespecialjointrobot_auto_balancer.py index c8a74313c99..0fe6e8760e3 100755 --- a/sample/SampleSpecialJointRobot/samplespecialjointrobot_auto_balancer.py +++ b/sample/SampleSpecialJointRobot/samplespecialjointrobot_auto_balancer.py @@ -4,7 +4,7 @@ from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -27,24 +27,24 @@ def init (): hcf.waitInterpolation() hcf.startAutoBalancer() hrpsys_version = hcf.seq.ref.get_component_profile().version - print("hrpsys_version = %s"%hrpsys_version) + print(("hrpsys_version = %s"%hrpsys_version)) def checkActualBaseAttitude(): rpy = rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation ret = math.degrees(rpy.r) < 0.1 and math.degrees(rpy.p) < 0.1 - print >> sys.stderr, " actual base rpy = ", ret, "(", rpy, ")" + print(" actual base rpy = ", ret, "(", rpy, ")", file=sys.stderr) assert (ret) return ret def demoGaitGeneratorNoToeHeelContact(): - print >> sys.stderr, "1. Do not use toe heel contact" + print("1. Do not use toe heel contact", file=sys.stderr) hcf.abc_svc.goPos(0.3, 0, 0); hcf.abc_svc.waitFootSteps() checkActualBaseAttitude() - print >> sys.stderr, " No toe heel contact=>OK" + print(" No toe heel contact=>OK", file=sys.stderr) def demoGaitGeneratorToeHeelContact(): - print >> sys.stderr, "2. Use toe heel contact" + print("2. Use toe heel contact", file=sys.stderr) ggp=hcf.abc_svc.getGaitGeneratorParam()[1]; ggp.toe_pos_offset_x = 1e-3*182.0; ggp.heel_pos_offset_x = 1e-3*-72.0; @@ -60,10 +60,10 @@ def demoGaitGeneratorToeHeelContact(): ggp.heel_angle = 0; hcf.abc_svc.setGaitGeneratorParam(ggp); checkActualBaseAttitude() - print >> sys.stderr, " Toe heel contact=>OK" + print(" Toe heel contact=>OK", file=sys.stderr) def demoGaitGeneratorToeHeelContactWithToeJoint(): - print >> sys.stderr, "3. Use toe heel contact with toe joint" + print("3. Use toe heel contact with toe joint", file=sys.stderr) ggp=hcf.abc_svc.getGaitGeneratorParam()[1]; ggp.toe_pos_offset_x = 1e-3*182.0; ggp.heel_pos_offset_x = 1e-3*-72.0; @@ -79,7 +79,7 @@ def demoGaitGeneratorToeHeelContactWithToeJoint(): ggp.heel_angle = 0; hcf.abc_svc.setGaitGeneratorParam(ggp); checkActualBaseAttitude() - print >> sys.stderr, " Toe heel contact with toe joint =>OK" + print(" Toe heel contact with toe joint =>OK", file=sys.stderr) def demo(): diff --git a/sample/simTest1.py b/sample/simTest1.py index 368b2c85ca6..0aaca5a533b 100644 --- a/sample/simTest1.py +++ b/sample/simTest1.py @@ -9,7 +9,7 @@ openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1") project = "file://"+openhrp_dir+"/share/OpenHRP-3.1/sample/project/Sample.xml" sim = rtm.findRTC("Simulator0") -print "sim:",sim +print("sim:",sim) sim.setProperty("project", project) sim.setProperty("useOLV", "1") sim.start() diff --git a/sample/simTest2.py b/sample/simTest2.py index 3b03d2f6a16..4d148741115 100644 --- a/sample/simTest2.py +++ b/sample/simTest2.py @@ -9,10 +9,10 @@ openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1") project = "file://"+openhrp_dir+"/share/OpenHRP-3.1/sample/project/SamplePD.xml" sim = rtm.findRTC("Simulator0") -print "sim:",sim +print("sim:",sim) sim.setProperty("project", project) vwr = rtm.findRTC("Viewer0") -print "vwr:",vwr +print("vwr:",vwr) vwr.setProperty("project", project) rtm.connectPorts(sim.port("state"), vwr.port("state")) vwr.start() diff --git a/sample/vcTest.py b/sample/vcTest.py index d5dfbf3d0a5..4e504210307 100644 --- a/sample/vcTest.py +++ b/sample/vcTest.py @@ -9,7 +9,7 @@ openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1") project = "file://"+openhrp_dir+"/share/OpenHRP-3.1/sample/project/SampleRobot_inHouse.xml" vc = rtm.findRTC("VirtualCamera0") -print "vc:",vc +print("vc:",vc) vc.setProperty("project", project) vc.setProperty("camera", "Robot:VISION_SENSOR1") vc.start() diff --git a/sample/visionTest.py b/sample/visionTest.py index cb60fd3f414..313ca2678f9 100644 --- a/sample/visionTest.py +++ b/sample/visionTest.py @@ -61,7 +61,7 @@ def jpeg(): jd.start() civ.start() time.sleep(3) - print "jpeg quality 95 -> 30" + print("jpeg quality 95 -> 30") je.setProperty("quality", "30") diff --git a/src/hrpsys/rtm.py b/src/hrpsys/rtm.py index 2ac1a7fc175..7d070206bc4 100644 --- a/src/hrpsys/rtm.py +++ b/src/hrpsys/rtm.py @@ -118,8 +118,8 @@ def start(self, ec=None, timeout=3.0): return True ret = ec.activate_component(self.ref) if ret != RTC.RTC_OK: - print ('[rtm.py] \033[31m Failed to start %s(%s)\033[0m' % \ - (self.name(), ret)) + print(('[rtm.py] \033[31m Failed to start %s(%s)\033[0m' % \ + (self.name(), ret))) return False tm = 0 while tm < timeout: @@ -127,8 +127,8 @@ def start(self, ec=None, timeout=3.0): return True time.sleep(0.01) tm += 0.01 - print ('[rtm.py] \033[31m Failed to start %s(timeout)\033[0m' % \ - self.name()) + print(('[rtm.py] \033[31m Failed to start %s(timeout)\033[0m' % \ + self.name())) return False ## @@ -145,8 +145,8 @@ def stop(self, ec=None, timeout=3.0): return True ret = ec.deactivate_component(self.ref) if ret != RTC.RTC_OK: - print ('[rtm.py] \033[31m Failed to stop %s(%s)\033[0m' % \ - (self.name(), ret)) + print(('[rtm.py] \033[31m Failed to stop %s(%s)\033[0m' % \ + (self.name(), ret))) return False tm = 0 while tm < timeout: @@ -154,8 +154,8 @@ def stop(self, ec=None, timeout=3.0): return True time.sleep(0.01) tm += 0.01 - print ('[rtm.py] \033[31m Failed to stop %s(timeout)\033[0m' % \ - self.name()) + print(('[rtm.py] \033[31m Failed to stop %s(timeout)\033[0m' % \ + self.name())) return False ## @@ -234,7 +234,7 @@ def load(self, basename, initfunc=""): try: self.ref.load_module(path, initfunc) except: - print("failed to load", path) + print(("failed to load", path)) ## # \brief create an instance of RT component @@ -246,7 +246,7 @@ def create(self, module, name=None): if name != None: rtc = findRTC(name) if rtc != None: - print('RTC named "' + name + '" already exists.') + print(('RTC named "' + name + '" already exists.')) return rtc args = module if name != None: @@ -332,7 +332,7 @@ def initCORBA(): mc.parseArgs(rtm_argv) if nshost != None: # these values can be set via other script like "import rtm; rtm.nshost=XXX" - print("\033[34m[rtm.py] nshost already set as " + str(nshost) + "\033[0m") + print(("\033[34m[rtm.py] nshost already set as " + str(nshost) + "\033[0m")) else: try: nshost = mc._argprop.getProperty("corba.nameservers").split(":")[0] @@ -340,10 +340,10 @@ def initCORBA(): raise except: nshost = socket.gethostname() # default - print("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nshost) + " as nshost \033[0m") + print(("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nshost) + " as nshost \033[0m")) if nsport != None: - print("\033[34m[rtm.py] nsport already set as " + str(nsport) + "\033[0m") + print(("\033[34m[rtm.py] nsport already set as " + str(nsport) + "\033[0m")) else: try: nsport = int(mc._argprop.getProperty("corba.nameservers").split(":")[1]) @@ -351,15 +351,15 @@ def initCORBA(): raise except: nsport = 15005 # default - print("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nsport) + " as nsport \033[0m") + print(("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nsport) + " as nsport \033[0m")) if mgrhost != None: - print("\033[34m[rtm.py] mgrhost already set as " + str(mgrhost) + "\033[0m") + print(("\033[34m[rtm.py] mgrhost already set as " + str(mgrhost) + "\033[0m")) else: mgrhost = nshost if mgrport != None: - print("\033[34m[rtm.py] mgrport already set as " + str(mgrport) + "\033[0m") + print(("\033[34m[rtm.py] mgrport already set as " + str(mgrport) + "\033[0m")) else: try: mgrport = int(mc._argprop.getProperty("corba.master_manager").split(":")[1]) @@ -367,10 +367,10 @@ def initCORBA(): raise except: mgrport = 2810 # default - print("\033[34m[rtm.py] Failed to parse corba.master_manager, use " + str(mgrport) + "\033[0m") + print(("\033[34m[rtm.py] Failed to parse corba.master_manager, use " + str(mgrport) + "\033[0m")) - print("\033[34m[rtm.py] configuration ORB with %s:%s\033[0m"%(nshost, nsport)) - print("\033[34m[rtm.py] configuration RTCManager with %s:%s\033[0m"%(mgrhost, mgrport)) + print(("\033[34m[rtm.py] configuration ORB with %s:%s\033[0m"%(nshost, nsport))) + print(("\033[34m[rtm.py] configuration RTCManager with %s:%s\033[0m"%(mgrhost, mgrport))) os.environ['ORBInitRef'] = 'NameService=corbaloc:iiop:%s:%s/NameService' % \ (nshost, nsport) @@ -392,7 +392,7 @@ def initCORBA(): 'Make sure the hostname is correct and the Nameserver is running.\n' + str(e)) except Exception: _, e, _ = sys.exc_info() - print(str(e)) + print((str(e))) return None @@ -424,7 +424,7 @@ def findObject(name, kind="", rnc=None): if not rnc: rnc = rootnc if not rnc: - print("[ERROR] findObject(%r,kind=%r,rnc=%r) rootnc is not found" % (name, kind, rnc)) + print(("[ERROR] findObject(%r,kind=%r,rnc=%r) rootnc is not found" % (name, kind, rnc))) return rnc.resolve(path) ## @@ -436,8 +436,8 @@ def findObject(name, kind="", rnc=None): # def findRTCmanager(hostname=None, rnc=None): if not rootnc: - print("[ERROR] findRTCmanager(hostname=%r,rnc=%r) rootnc is not defined, need to call initCORBA()" % \ - (hostname, rnc)) + print(("[ERROR] findRTCmanager(hostname=%r,rnc=%r) rootnc is not defined, need to call initCORBA()" % \ + (hostname, rnc))) if not hostname: hostname = nshost cxt = None @@ -462,7 +462,7 @@ def getManagerFromNS(hostname, mgr=None): def getManagerDirectly(hostname, mgr=None): global orb, mgrport corbaloc = "corbaloc:iiop:" + hostname + ":" + str(mgrport) + "/manager" - print("\033[34m[rtm.py] trying to findRTCManager on port" + str(mgrport) + "\033[0m") + print(("\033[34m[rtm.py] trying to findRTCManager on port" + str(mgrport) + "\033[0m")) try: obj = orb.string_to_object(corbaloc) mgr = RTCmanager(obj._narrow(RTM.Manager)) @@ -544,10 +544,10 @@ def serializeComponents(rtcs, stopEC=True): else: print('error in add_component()') else: - print(rtc.name() + 'is already serialized') + print((rtc.name() + 'is already serialized')) except Exception: _, e, _ = sys.exc_info() - print("[rtm.py] \033[31m error in serialize %s of %s %s\033[0m" % (rtc.name(), [[r, r.name()] for r in rtcs], str(e))) + print(("[rtm.py] \033[31m error in serialize %s of %s %s\033[0m" % (rtc.name(), [[r, r.name()] for r in rtcs], str(e)))) raise e ## @@ -578,7 +578,7 @@ def disconnectPorts(outP, inP): if len(ports) == 2: pname = ports[1].get_port_profile().name if pname == iname: - print('[rtm.py] Disconnect %s - %s' %(op.name, iname)) + print(('[rtm.py] Disconnect %s - %s' %(op.name, iname))) outP.disconnect(con_prof.connector_id) return True return False @@ -609,21 +609,21 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength if not isinstance(inPs, list): inPs = [inPs] if not outP: - print('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ - (outP, [inP.get_port_profile().name if inP else inP for inP in inPs], inPs)) + print(('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ + (outP, [inP.get_port_profile().name if inP else inP for inP in inPs], inPs))) return for inP in inPs: if not inP: - print('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ - (outP.get_port_profile().name, inP, inPs)) + print(('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ + (outP.get_port_profile().name, inP, inPs))) continue if isConnected(outP, inP) == True: - print('[rtm.py] %s and %s are already connected' % \ - (outP.get_port_profile().name, inP.get_port_profile().name)) + print(('[rtm.py] %s and %s are already connected' % \ + (outP.get_port_profile().name, inP.get_port_profile().name))) continue if dataTypeOfPort(outP) != dataTypeOfPort(inP): - print('[rtm.py] \033[31m %s and %s have different data types\033[0m' % \ - (outP.get_port_profile().name, inP.get_port_profile().name)) + print(('[rtm.py] \033[31m %s and %s have different data types\033[0m' % \ + (outP.get_port_profile().name, inP.get_port_profile().name))) continue nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any(interfaceType)) nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any(dataflow)) @@ -634,8 +634,8 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength nv7 = SDOPackage.NameValue("dataport.data_type", any.to_any(dataTypeOfPort(outP))) con_prof = RTC.ConnectorProfile("connector0", "", [outP, inP], [nv1, nv2, nv3, nv4, nv5, nv6, nv7]) - print('[rtm.py] Connect ' + outP.get_port_profile().name + ' - ' + \ - inP.get_port_profile().name+' (dataflow_type='+dataflow+', subscription_type='+ subscription+', bufferlength='+str(bufferlength)+', push_rate='+str(rate)+', push_policy='+pushpolicy+')') + print(('[rtm.py] Connect ' + outP.get_port_profile().name + ' - ' + \ + inP.get_port_profile().name+' (dataflow_type='+dataflow+', subscription_type='+ subscription+', bufferlength='+str(bufferlength)+', push_rate='+str(rate)+', push_policy='+pushpolicy+')')) ret, prof = inP.connect(con_prof) if ret != RTC.RTC_OK: print("failed to connect") @@ -845,7 +845,7 @@ def findService(rtc, port_name, type_name, instance_name): else: p = rtc.port(port_name) if p == None: - print("can't find a port named" + port_name) + print(("can't find a port named" + port_name)) return None else: port_prof = [p.get_port_profile()] @@ -861,7 +861,7 @@ def findService(rtc, port_name, type_name, instance_name): aif.polarity == PROVIDED: port = pp.port_ref if port == None: - print("can't find a service named", instance_name) + print(("can't find a service named", instance_name)) return None con_prof = RTC.ConnectorProfile("noname", "", [port], []) ret, con_prof = port.connect(con_prof) diff --git a/test/test-hostname.py b/test/test-hostname.py index a1fafadc27c..69fd17f8b1a 100755 --- a/test/test-hostname.py +++ b/test/test-hostname.py @@ -34,7 +34,7 @@ def check_initCORBA(self, nshost, nsport=2809): if ms and rh : break time.sleep(1) - print >>sys.stderr, "wait for RTCmanager=%r, RTC(RobotHardware0)=%r"%(ms,rh) + print("wait for RTCmanager=%r, RTC(RobotHardware0)=%r"%(ms,rh), file=sys.stderr) count += 1 self.assertTrue(ms and rh) except Exception as e: @@ -55,16 +55,16 @@ def test_X_unknown(self): try: self.check_initCORBA('unknown') except SystemExit as e: - print "[This is Expected Failure]" - print str(e.message) + print("[This is Expected Failure]") + print(str(e.message)) @unittest.expectedFailure def test_X_123_45_67_89(self): try: self.check_initCORBA('123.45.67.89') except SystemExit as e: - print "[This is Expected Failure]" - print str(e.message) + print("[This is Expected Failure]") + print(str(e.message)) #unittest.main() if __name__ == '__main__': diff --git a/test/test-jointangle.py b/test/test-jointangle.py index 758cf6ae390..5aea5df0a9d 100755 --- a/test/test-jointangle.py +++ b/test/test-jointangle.py @@ -44,14 +44,14 @@ def setUpClass(self): def test_set_if_find_log(self): h = PA10() h.findComps() - print >>sys.stderr, "log=",h.log, "log_svc=",h.log_svc + print("log=",h.log, "log_svc=",h.log_svc, file=sys.stderr) self.assertTrue(h.log) self.assertTrue(h.log_svc) def test_get_joint_angles(self): h = PA10() h.findComps() - print >>sys.stderr, h.getJointAngles() + print(h.getJointAngles(), file=sys.stderr) self.assertEqual(len(h.getJointAngles()), int(9)) def test_set_joint_angles(self): diff --git a/test/test-pkgconfig.py b/test/test-pkgconfig.py index 93f41fe70f3..46261afa3b6 100755 --- a/test/test-pkgconfig.py +++ b/test/test-pkgconfig.py @@ -57,13 +57,13 @@ def test_files_for_hrpsys_ros_bridge(self): def test_compile_iob(self): global PID cmd = "%s pkg-config hrpsys-base --cflags --libs"%(self.PKG_CONFIG_PATH) - print "`"+cmd+"` =",check_output(cmd, shell=True, stderr=STDOUT) + print("`"+cmd+"` =",check_output(cmd, shell=True, stderr=STDOUT)) ret = call("gcc -o hrpsys-sample-pkg-config /tmp/%d-hrpsys-sample.cpp `%s` -lhrpIo"%(PID,cmd), shell=True) self.assertTrue(ret==0) def test_idlfile(self): cmd = "%s pkg-config hrpsys-base --variable=idldir"%(self.PKG_CONFIG_PATH) - print "`"+cmd+"`/RobotHardwareService.idl = ",os.path.join(check_output(cmd, shell=True, stderr=STDOUT).rstrip(), "RobotHardwareService.idl") + print("`"+cmd+"`/RobotHardwareService.idl = ",os.path.join(check_output(cmd, shell=True, stderr=STDOUT).rstrip(), "RobotHardwareService.idl")) self.assertTrue(os.path.exists(os.path.join(check_output(cmd, shell=True).rstrip(), "RobotHardwareService.idl"))) #unittest.main() diff --git a/test/test-robot-hardware.py b/test/test-robot-hardware.py index 10fb51e4bd1..4d6e89be56f 100755 --- a/test/test-robot-hardware.py +++ b/test/test-robot-hardware.py @@ -23,14 +23,14 @@ def test_rh_service(self): rtm.initCORBA() rh = rtm.findRTC("RobotHardware0") rh_svc = rtm.narrow(rh.service("service0"), "RobotHardwareService") - print "RTC(RobotHardware0)={0}, {1}".format(rh,rh_svc) + print("RTC(RobotHardware0)={0}, {1}".format(rh,rh_svc)) self.assertTrue(rh and rh_svc) rh.start() self.assertTrue(rh.isActive()) self.assertTrue(rh_svc.getStatus()) except Exception as e: - print "{0}, RTC(RobotHardware0)={1}, {2}".format(str(e),rh,rh_svc) + print("{0}, RTC(RobotHardware0)={1}, {2}".format(str(e),rh,rh_svc)) self.fail() pass diff --git a/test/test-samplerobot.py b/test/test-samplerobot.py index 8661f488fe9..034c82ebc5f 100755 --- a/test/test-samplerobot.py +++ b/test/test-samplerobot.py @@ -44,7 +44,7 @@ def setUpClass(self): def test_set_if_find_log(self): h = SampleRobot() h.findComps() - print >>sys.stderr, "log=",h.log, "log_svc=",h.log_svc + print("log=",h.log, "log_svc=",h.log_svc, file=sys.stderr) self.assertTrue(h.log) self.assertTrue(h.log_svc) From fdec77db13bc4e301deb936d2c0092868f21f599 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:39:13 +0900 Subject: [PATCH 06/26] util:simulator/CMakeLists.txt: force link to python38 --- util/simulator/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/util/simulator/CMakeLists.txt b/util/simulator/CMakeLists.txt index beb53389d08..b87c2c2168f 100644 --- a/util/simulator/CMakeLists.txt +++ b/util/simulator/CMakeLists.txt @@ -32,9 +32,9 @@ add_library(hrpsysext SHARED PyShape.cpp ) -find_package(Boost REQUIRED COMPONENTS python) +find_package(Boost REQUIRED COMPONENTS python38) target_link_libraries(hrpsysext - Boost::python + Boost::python38 hrpsysUtil ${PYTHON_LIBRARIES} ) From ecfefb92ab47866b35cf206e6064f9c06d338a2e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:41:15 +0900 Subject: [PATCH 07/26] fix for absolute/relative import for Python3 --- src/hrpsys/hrpsys_config.py | 9 +++++---- src/hrpsys/waitInput.py | 10 +++------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/hrpsys/hrpsys_config.py b/src/hrpsys/hrpsys_config.py index 9038aa40369..770758d1e04 100755 --- a/src/hrpsys/hrpsys_config.py +++ b/src/hrpsys/hrpsys_config.py @@ -3,16 +3,17 @@ import os import rtm -from rtm import * -from OpenHRP import * +from .rtm import * +from .OpenHRP import * from hrpsys import * # load ModelLoader from hrpsys import ImpedanceControllerService_idl -from waitInput import waitInputConfirm +from .waitInput import waitInputConfirm import socket import time import subprocess -from distutils.version import StrictVersion +from packaging.version import parse as StrictVersion + # copy from transformations.py, Christoph Gohlke, The Regents of the University of California diff --git a/src/hrpsys/waitInput.py b/src/hrpsys/waitInput.py index 1019e67992f..1a1eb203fd3 100644 --- a/src/hrpsys/waitInput.py +++ b/src/hrpsys/waitInput.py @@ -1,7 +1,7 @@ -from Tkinter import * -from tkMessageBox import * +from tkinter import * +from tkinter.messagebox import * import datetime -import __builtin__ +import builtins import threading def waitInputConfirm(msg): @@ -244,7 +244,3 @@ def waitInputMenu(menu): thr = threading.Thread(target=waitInputMenuMain, args=(menu,)) thr.start() return thr - -__builtin__.waitInputConfirm = waitInputConfirm -__builtin__.waitInputSelect = waitInputSelect -__builtin__.waitInputMenu = waitInputMenu From d9ab67806d152cbfad78d93c7127fcc6170c7c25 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:43:29 +0900 Subject: [PATCH 08/26] fix for python3 unicode? --- src/hrpsys/hrpsys_config.py | 2 ++ src/hrpsys/rtm.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/hrpsys/hrpsys_config.py b/src/hrpsys/hrpsys_config.py index 770758d1e04..36e6712ae53 100755 --- a/src/hrpsys/hrpsys_config.py +++ b/src/hrpsys/hrpsys_config.py @@ -796,6 +796,8 @@ def getRTCInstanceList(self, verbose=True): def parseUrl(self, url): if '$(PROJECT_DIR)' in url: path = subprocess.Popen(['pkg-config', 'openhrp3.1', '--variable=prefix'], stdout=subprocess.PIPE).communicate()[0].rstrip() + if isinstance(path, bytes): + path = path.decode('utf-8') path = os.path.join(path, 'share/OpenHRP-3.1/sample/project') url = url.replace('$(PROJECT_DIR)', path) return url diff --git a/src/hrpsys/rtm.py b/src/hrpsys/rtm.py index 7d070206bc4..22e17cdbd57 100644 --- a/src/hrpsys/rtm.py +++ b/src/hrpsys/rtm.py @@ -12,6 +12,8 @@ import time import re +if sys.version_info[0] >= 3: + unicode = str ## # \brief root naming context # From a7ace0ea18c7d9a9b6795489c97a5a07e82c29b5 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 15:43:59 +0900 Subject: [PATCH 09/26] hrpsys_config.py: return list, not filter --- src/hrpsys/hrpsys_config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hrpsys/hrpsys_config.py b/src/hrpsys/hrpsys_config.py index 36e6712ae53..2f77f2fd537 100755 --- a/src/hrpsys/hrpsys_config.py +++ b/src/hrpsys/hrpsys_config.py @@ -770,7 +770,7 @@ def getJointAngleControllerList(self): ''' controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el] - return filter(lambda c: c != None, controller_list) # only return existing controllers + return list(filter(lambda c: c != None, controller_list)) # only return existing controllers def getRTCInstanceList(self, verbose=True): '''!@brief From a19fc79622487c85dfa57c7ca6c1547565e07c9a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 2 Apr 2024 17:09:48 +0900 Subject: [PATCH 10/26] CMakeLists.txt: test subdirectory does not have CMakeLists.txt --- CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ed670dfa773..9e333cab9ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -217,9 +217,6 @@ if (NOT QNXNTO) add_subdirectory(util) endif() add_subdirectory(sample) -if(CATKIN_ENABLE_TESTING) - add_subdirectory(test) -endif() #if catkin environment string(REGEX MATCH "catkin" need_catkin "$ENV{_}") From 5f642a16246810d0665301fe9f68f36cc7f99421 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 14:25:57 +0900 Subject: [PATCH 11/26] apply 2to3 -f print in sample, util directory --- sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in | 6 +++--- sample/SampleRobot/rtc/samplerobot_sample_component.py.in | 2 +- util/simulator/hrpsys-simulator-jython.in | 2 +- util/simulator/hrpsys-simulator-python.in | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in b/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in index bf687b60ef1..89858f1c2e8 100755 --- a/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in +++ b/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in @@ -4,7 +4,7 @@ try: from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * @@ -63,7 +63,7 @@ def demo(): # 1. getParameter ret=hcf.kf_svc.getKalmanFilterParam() if ret[0]: - print "getKalmanFilterParam() => OK" + print("getKalmanFilterParam() => OK") # 2. setParameter kfp=hcf.kf_svc.getKalmanFilterParam()[1] kfp.Q_angle = 0.001; @@ -72,7 +72,7 @@ def demo(): ret=hcf.kf_svc.setKalmanFilterParam(kfp) kfp2=hcf.kf_svc.getKalmanFilterParam()[1] if ret and kfp.Q_angle == kfp2.Q_angle and kfp.Q_rate == kfp2.Q_rate and kfp.R_angle == kfp2.R_angle: - print "setKalmanFilterParam() => OK" + print("setKalmanFilterParam() => OK") # 3. check log and plot offset = math.radians(85) # we need this because we rotate waist of this robot in xml file. diff --git a/sample/SampleRobot/rtc/samplerobot_sample_component.py.in b/sample/SampleRobot/rtc/samplerobot_sample_component.py.in index 85324edc112..a0a10c78093 100755 --- a/sample/SampleRobot/rtc/samplerobot_sample_component.py.in +++ b/sample/SampleRobot/rtc/samplerobot_sample_component.py.in @@ -4,7 +4,7 @@ try: from hrpsys.hrpsys_config import * import OpenHRP except: - print "import without hrpsys" + print("import without hrpsys") import rtm from rtm import * from OpenHRP import * diff --git a/util/simulator/hrpsys-simulator-jython.in b/util/simulator/hrpsys-simulator-jython.in index b1873295f89..7899fea6e9a 100755 --- a/util/simulator/hrpsys-simulator-jython.in +++ b/util/simulator/hrpsys-simulator-jython.in @@ -18,7 +18,7 @@ def runSimulator(*args): argc = len(sys.argv) if argc < 2: - print "Usage: hrpsys-simulator-jython project.xml [script.py] [options for hrpsys-simulator]" + print("Usage: hrpsys-simulator-jython project.xml [script.py] [options for hrpsys-simulator]") sys.exit(1) if sys.argv[2][(len(sys.argv[2])-3):] == ".py": diff --git a/util/simulator/hrpsys-simulator-python.in b/util/simulator/hrpsys-simulator-python.in index d051680295a..f404a5731e6 100755 --- a/util/simulator/hrpsys-simulator-python.in +++ b/util/simulator/hrpsys-simulator-python.in @@ -13,7 +13,7 @@ sys.path.append(os.getcwd()) argc = len(sys.argv) if argc < 2: - print "Usage: hrpsys-simulator project.xml [script.py]" + print("Usage: hrpsys-simulator project.xml [script.py]") else: project = sys.argv[1] From e32eea586df4b8c61ccfff8e3eab8e57f73e2792 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 14:29:18 +0900 Subject: [PATCH 12/26] apply 2to3 -f filter in sample directory and hrpsys_config.py --- .../samplerobot_emergency_stopper.py | 2 +- .../samplerobot_soft_error_limiter.py | 2 +- src/hrpsys/hrpsys_config.py | 43 +++++++++---------- 3 files changed, 23 insertions(+), 24 deletions(-) diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index 2c974edcedc..c3fee45d9a1 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -19,7 +19,7 @@ def init (): hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") if hcf.es != None: - for sen in filter(lambda x: x.type == "Force", hcf.sensors): + for sen in [x for x in hcf.sensors if x.type == "Force"]: hcf.connectLoggerPort(hcf.es, sen.name+"Out") init_pose = [0]*29 reset_pose = [0.0,-0.772215,0.0,1.8338,-1.06158,0.0,0.523599,0.0,0.0,-2.44346,0.15708,-0.113446,0.637045,0.0,-0.772215,0.0,1.8338,-1.06158,0.0,0.523599,0.0,0.0,-2.44346,-0.15708,-0.113446,-0.637045,0.0,0.0,0.0] diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index c2ac25262e4..dceb7e6785b 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -17,7 +17,7 @@ # Tempolarily remove tc which is limit position range def getRTCList (): - return filter(lambda x : x[0]!='tc', hcf.getRTCListUnstable()) + return [x for x in hcf.getRTCListUnstable() if x[0]!='tc'] def init (): global hcf, initial_pose, limit_table_list, bodyinfo, hrpsys_version diff --git a/src/hrpsys/hrpsys_config.py b/src/hrpsys/hrpsys_config.py index 2f77f2fd537..0586bad84ee 100755 --- a/src/hrpsys/hrpsys_config.py +++ b/src/hrpsys/hrpsys_config.py @@ -336,10 +336,10 @@ def connectComps(self): # connection for kf if self.kf: # currently use first acc and rate sensors for kf - s_acc = filter(lambda s: s.type == 'Acceleration', self.sensors) + s_acc = [s for s in self.sensors if s.type == 'Acceleration'] if (len(s_acc) > 0) and self.rh.port(s_acc[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming rule of gsensor and gyrometer connectPorts(self.rh.port(s_acc[0].name), self.kf.port('acc')) - s_rate = filter(lambda s: s.type == 'RateGyro', self.sensors) + s_rate = [s for s in self.sensors if s.type == 'RateGyro'] if (len(s_rate) > 0) and self.rh.port(s_rate[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming rule of gsensor and gyrometer connectPorts(self.rh.port(s_rate[0].name), self.kf.port("rate")) connectPorts(self.rh.port("q"), self.kf.port("qCurrent")) @@ -447,7 +447,7 @@ def connectComps(self): # connectPorts(self.kf.port("rpy"), self.ic.port("rpy")) connectPorts(self.kf.port("rpy"), self.rmfo.port("rpy")) connectPorts(self.rh.port("q"), self.rmfo.port("qCurrent")) - for sen in filter(lambda x: x.type == "Force", self.sensors): + for sen in [x for x in self.sensors if x.type == "Force"]: connectPorts(self.rh.port(sen.name), self.rmfo.port(sen.name)) if self.ic: connectPorts(self.rmfo.port("off_" + sen.name), @@ -459,7 +459,7 @@ def connectComps(self): connectPorts(self.rmfo.port("off_" + sen.name), self.st.port(sen.name)) elif self.ic: # if the robot does not have rmfo and kf, but have ic - for sen in filter(lambda x: x.type == "Force", self.sensors): + for sen in [x for x in self.sensors if x.type == "Force"]: connectPorts(self.rh.port(sen.name), self.ic.port(sen.name)) @@ -488,8 +488,8 @@ def connectComps(self): connectPorts(self.tf.port("tauOut"), self.vs.port("tauIn")) # virtual force sensors if self.ic: - for vfp in filter(lambda x: str.find(x, 'v') >= 0 and - str.find(x, 'sensor') >= 0, self.vs.ports.keys()): + for vfp in [x for x in self.vs.ports.keys() if str.find(x, 'v') >= 0 and + str.find(x, 'sensor') >= 0]: connectPorts(self.vs.port(vfp), self.ic.port(vfp)) # connection for co if self.co: @@ -501,7 +501,7 @@ def connectComps(self): if self.kf: connectPorts(self.kf.port("rpy"), self.octd.port("rpy")) if self.rmfo: - for sen in filter(lambda x: x.type == "Force", self.sensors): + for sen in [x for x in self.sensors if x.type == "Force"]: connectPorts(self.rmfo.port("off_" + sen.name), self.octd.port(sen.name)) # connection for gc @@ -563,10 +563,10 @@ def connectComps(self): # connection for acf if self.acf: # currently use first acc and rate sensors for acf - s_acc = filter(lambda s: s.type == 'Acceleration', self.sensors) + s_acc = [s for s in self.sensors if s.type == 'Acceleration'] if (len(s_acc) > 0) and self.rh.port(s_acc[0].name) != None: connectPorts(self.rh.port(s_acc[0].name), self.acf.port('accIn')) - s_rate = filter(lambda s: s.type == 'RateGyro', self.sensors) + s_rate = [s for s in self.sensors if s.type == 'RateGyro'] if (len(s_rate) > 0) and self.rh.port(s_rate[0].name) != None: connectPorts(self.rh.port(s_rate[0].name), self.acf.port("rateIn")) if self.kf: @@ -770,7 +770,7 @@ def getJointAngleControllerList(self): ''' controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el] - return list(filter(lambda c: c != None, controller_list)) # only return existing controllers + return list([c for c in controller_list if c != None]) # only return existing controllers def getRTCInstanceList(self, verbose=True): '''!@brief @@ -825,17 +825,16 @@ def getSensors(self, url): return [] else: return sum(map(lambda x: x.sensors, - filter(lambda x: len(x.sensors) > 0, - self.getBodyInfo(url)._get_links())), []) # sum is for list flatten + [x for x in self.getBodyInfo(url)._get_links() if len(x.sensors) > 0]), []) # sum is for list flatten # public method to get sensors list def getForceSensorNames(self): '''!@brief Get list of force sensor names. Returns existence force sensors and virtual force sensors. self.sensors and virtual force sensors are assumed. ''' - ret = map (lambda x : x.name, filter(lambda x: x.type == "Force", self.sensors)) + ret = map (lambda x : x.name, [x for x in self.sensors if x.type == "Force"]) if self.vs != None: - ret += filter(lambda x: str.find(x, 'v') >= 0 and str.find(x, 'sensor') >= 0, self.vs.ports.keys()) + ret += [x for x in self.vs.ports.keys() if str.find(x, 'v') >= 0 and str.find(x, 'sensor') >= 0] return ret def connectLoggerPort(self, artc, sen_name, log_name=None): @@ -916,13 +915,13 @@ def setupLogger(self, maxLength=4000): self.connectLoggerPort(self.rh, 'servoState') if self.simulation_mode: self.connectLoggerPort(self.rh, 'WAIST') - for sen in filter(lambda x: x.type == "Force", self.sensors): + for sen in [x for x in self.sensors if x.type == "Force"]: self.connectLoggerPort(self.sh, sen.name + "Out") if self.rmfo != None: - for sen in filter(lambda x: x.type == "Force", self.sensors): + for sen in [x for x in self.sensors if x.type == "Force"]: self.connectLoggerPort(self.rmfo, "off_"+sen.name) if self.rfu != None: - for sen in filter(lambda x: x.type == "Force", self.sensors): + for sen in [x for x in self.sensors if x.type == "Force"]: self.connectLoggerPort(self.rfu, "ref_"+sen.name+"Out") if self.octd != None: self.connectLoggerPort(self.octd, "octdData") @@ -2167,13 +2166,13 @@ def startDefaultUnstableControllers (self, ic_limbs=[], abc_limbs=[]): ''' print(self.configurator_name + "Start Default Unstable Controllers") # Check robot setting - is_legged_robot = map(lambda x: x[0], filter(lambda x : re.match(".*leg", x[0]), self.Groups)) + is_legged_robot = map(lambda x: x[0], [x for x in self.Groups if re.match(".*leg", x[0])]) # Select all arms in "Groups" for impedance as a default setting if not ic_limbs: - ic_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups)) + ic_limbs = map(lambda x: x[0], [x for x in self.Groups if re.match(".*(arm)", x[0])]) # Select all arms/legs in "Groups" for autobalancer as a default setting if not abc_limbs: - abc_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(leg|arm)", x[0]), self.Groups)) + abc_limbs = map(lambda x: x[0], [x for x in self.Groups if re.match(".*(leg|arm)", x[0])]) # Start controllers for limb in ic_limbs: self.ic_svc.startImpedanceControllerNoWait(limb) @@ -2197,10 +2196,10 @@ def stopDefaultUnstableControllers (self, ic_limbs=[]): ''' print(self.configurator_name + "Stop Default Unstable Controllers") # Check robot setting - is_legged_robot = map(lambda x: x[0], filter(lambda x : re.match(".*leg", x[0]), self.Groups)) + is_legged_robot = map(lambda x: x[0], [x for x in self.Groups if re.match(".*leg", x[0])]) # Select all arms in "Groups" for impedance as a default setting if not ic_limbs: - ic_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups)) + ic_limbs = map(lambda x: x[0], [x for x in self.Groups if re.match(".*(arm)", x[0])]) # Stop controllers if is_legged_robot: self.stopStabilizer() From 6fabc219ce78fa74e312dfabf1287337b9556c37 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 14:38:17 +0900 Subject: [PATCH 13/26] apply 2to3 -f map in sample directory, hrpsys_config.py --- .../sample4legrobot_stabilizer.py | 2 +- .../sample6dofrobot_kalman_filter.py.in | 6 ++--- .../SampleRobot/samplerobot_auto_balancer.py | 14 ++++++------ .../SampleRobot/samplerobot_carry_object.py | 2 +- .../samplerobot_emergency_stopper.py | 4 ++-- .../SampleRobot/samplerobot_kalman_filter.py | 6 ++--- .../samplerobot_reference_force_updater.py | 4 ++-- .../samplerobot_remove_force_offset.py | 6 ++--- .../samplerobot_sequence_player.py | 14 ++++++------ .../samplerobot_soft_error_limiter.py | 10 ++++----- sample/SampleRobot/samplerobot_stabilizer.py | 4 ++-- .../samplerobot_virtual_force_sensor.py | 22 +++++++++---------- src/hrpsys/hrpsys_config.py | 17 +++++++------- 13 files changed, 55 insertions(+), 56 deletions(-) diff --git a/sample/Sample4LegRobot/sample4legrobot_stabilizer.py b/sample/Sample4LegRobot/sample4legrobot_stabilizer.py index b6cdd2b56b3..da280866dd7 100755 --- a/sample/Sample4LegRobot/sample4legrobot_stabilizer.py +++ b/sample/Sample4LegRobot/sample4legrobot_stabilizer.py @@ -60,7 +60,7 @@ def demoSetParameterAndStartST(): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp_org.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]) + stp_org.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]] stp_org.eefm_leg_inside_margin=tmp_leg_inside_margin stp_org.eefm_leg_outside_margin=tmp_leg_outside_margin stp_org.eefm_leg_front_margin=tmp_leg_front_margin diff --git a/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in b/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in index 89858f1c2e8..0dd55c715cb 100755 --- a/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in +++ b/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in @@ -42,11 +42,11 @@ def test_kf_plot (time, av1, av2, av3, optional_out_file_name): # time [s] for line in open("/tmp/test-kf-sample6dofrobot-{0}s.Sample6dofRobot(Robot)0_WAIST".format(time), "r"): act_rpy_ret.append(line.split(" ")[0:-1]) initial_sec=int(act_rpy_ret[0][0].split(".")[0]) - tm_list=map (lambda x : int(x[0].split(".")[0])-initial_sec + float(x[0].split(".")[1]) * 1e-6, act_rpy_ret) + tm_list=[int(x[0].split(".")[0])-initial_sec + float(x[0].split(".")[1]) * 1e-6 for x in act_rpy_ret] plt.clf() for idx in range(3): - plt.plot(tm_list, map(lambda x : 180.0 * float(x[4+idx]) / math.pi, act_rpy_ret)) - plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+idx]) / math.pi, kf_ret), ":") + plt.plot(tm_list, [180.0 * float(x[4+idx]) / math.pi for x in act_rpy_ret]) + plt.plot(tm_list, [180.0 * float(x[1+idx]) / math.pi for x in kf_ret], ":") plt.xlabel("Time [s]") plt.ylabel("Angle [deg]") plt.title("KF actual-estimated data (motion time = {0}[s], {1})".format(time,optional_out_file_name)) diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index 560ac1a4edc..734847e3c3f 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -55,7 +55,7 @@ def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-auto-balancer-chec def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-auto-balancer-check-param", save_log=True, rtc_name="SampleRobot(Robot)0"): if save_log: saveLogForCheckParameter(log_fname) - return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + return list(map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])) def checkActualBaseAttitude(ref_rpy = None, thre=0.1): # degree '''Check whether the robot falls down based on actual robot base-link attitude. @@ -109,7 +109,7 @@ def calcVelListFromPosList(pos_list, dt): vel_list=[] ppos=pos_list[0] for pos in pos_list: - vel_list.append(map(lambda x, y: (x-y)/dt, pos, ppos)); + vel_list.append(list(map(lambda x, y: (x-y)/dt, pos, ppos))); ppos=pos return vel_list @@ -122,15 +122,15 @@ def checkTooLargeABCCogAcc (acc_thre = 5.0): # [m/s^2] tm_list=[] for line in open("/tmp/test-abc-log.abc_cogOut", "r"): tm_list.append(float(line.split(" ")[0])); - cog_list.append(map(float, line.split(" ")[1:-1])); + cog_list.append(list(map(float, line.split(" ")[1:-1]))); cog_list=cog_list[:-1000] # ?? Neglect latter elements dt = tm_list[1]-tm_list[0] # [s] # Calculate velocity and acceleration dcog_list=calcVelListFromPosList(cog_list, dt) ddcog_list=calcVelListFromPosList(dcog_list, dt) # Check max - max_cogx_acc = max(map(lambda x : abs(x[0]), ddcog_list)) - max_cogy_acc = max(map(lambda x : abs(x[1]), ddcog_list)) + max_cogx_acc = max([abs(x[0]) for x in ddcog_list]) + max_cogy_acc = max([abs(x[1]) for x in ddcog_list]) ret = (max_cogx_acc < acc_thre) and (max_cogy_acc < acc_thre) print(" Check acc x = ", max_cogx_acc, ", y = ", max_cogy_acc, ", thre = ", acc_thre, "[m/s^2], ret = ", ret, file=sys.stderr) assert(ret) @@ -521,8 +521,8 @@ def demoGaitGeneratorOverwriteFootstepsBase(axis, overwrite_offset_idx = 1, init # Get remaining footstep [remain_fs, current_fs_idx]=hcf.abc_svc.getRemainingFootstepSequence()[1:] #print >> sys.stderr, remain_fs - print(" Remaining legs = ", map(lambda fs : fs.leg, remain_fs), file=sys.stderr) - print(" Remaining idx = ", map(lambda idx : current_fs_idx+idx, range(len(remain_fs))), file=sys.stderr) + print(" Remaining legs = ", [fs.leg for fs in remain_fs], file=sys.stderr) + print(" Remaining idx = ", [current_fs_idx+idx for idx in range(len(remain_fs))], file=sys.stderr) # Footstep index to be overwritten overwrite_fs_idx = current_fs_idx + overwrite_offset_idx print(" Overwrite index = ",overwrite_fs_idx, ", leg = ", remain_fs[overwrite_offset_idx].leg, file=sys.stderr) diff --git a/sample/SampleRobot/samplerobot_carry_object.py b/sample/SampleRobot/samplerobot_carry_object.py index 8e52cba2e74..a83c003d510 100755 --- a/sample/SampleRobot/samplerobot_carry_object.py +++ b/sample/SampleRobot/samplerobot_carry_object.py @@ -37,7 +37,7 @@ def demoSetParameter(): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp_org.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]) + stp_org.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]] stp_org.eefm_leg_inside_margin=tmp_leg_inside_margin stp_org.eefm_leg_outside_margin=tmp_leg_outside_margin stp_org.eefm_leg_front_margin=tmp_leg_front_margin diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index c3fee45d9a1..25cd9a5aca1 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -40,11 +40,11 @@ def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-emergency-stopper- def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-emergency-stopper-check-param", save_log=True, rtc_name="es"): if save_log: saveLogForCheckParameter(log_fname) - return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + return list(map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])) def getWrenchArray (): saveLogForCheckParameter() - return reduce(lambda x,y: x+y, (map(lambda fs : checkParameterFromLog(fs+"Out", save_log=False), ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor']))) + return reduce(lambda x,y: x+y, ([checkParameterFromLog(fs+"Out", save_log=False) for fs in ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor']])) # demo functions def demoEmergencyStopJointAngle (): diff --git a/sample/SampleRobot/samplerobot_kalman_filter.py b/sample/SampleRobot/samplerobot_kalman_filter.py index 9fa7f64bb6c..255aae8e341 100755 --- a/sample/SampleRobot/samplerobot_kalman_filter.py +++ b/sample/SampleRobot/samplerobot_kalman_filter.py @@ -75,15 +75,15 @@ def test_kf_plot (test_motion_func, optional_out_file_name): # time [s] act_rpy_ret.append(line.split(" ")[0:-1]) # Get time list initial_sec=int(act_rpy_ret[0][0].split(".")[0]) - tm_list=map (lambda x : int(x[0].split(".")[0])-initial_sec + float(x[0].split(".")[1]) * 1e-6, act_rpy_ret) + tm_list=[int(x[0].split(".")[0])-initial_sec + float(x[0].split(".")[1]) * 1e-6 for x in act_rpy_ret] # Plotting try: import matplotlib.pyplot as plt plt.clf() color_list = ['r', 'g', 'b'] for idx in range(3): - plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+3+idx]) / math.pi, act_rpy_ret), color=color_list[idx]) - plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+idx]) / math.pi, estimated_base_rpy_ret), "--", color=color_list[idx]) + plt.plot(tm_list, [180.0 * float(x[1+3+idx]) / math.pi for x in act_rpy_ret], color=color_list[idx]) + plt.plot(tm_list, [180.0 * float(x[1+idx]) / math.pi for x in estimated_base_rpy_ret], "--", color=color_list[idx]) plt.xlabel("Time [s]") plt.ylabel("Angle [deg]") plt.title("KF actual-estimated data (motion time = {0})".format(optional_out_file_name)) diff --git a/sample/SampleRobot/samplerobot_reference_force_updater.py b/sample/SampleRobot/samplerobot_reference_force_updater.py index 28cf3c83f59..d043a01a667 100755 --- a/sample/SampleRobot/samplerobot_reference_force_updater.py +++ b/sample/SampleRobot/samplerobot_reference_force_updater.py @@ -119,7 +119,7 @@ def demoSetReferecenForceUpdateParamWhileActive (): rfup.motion_dir = tmp_value = [0,0,-1] ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) print(" motion_dir ...", file=sys.stderr) - assert(ret and (map (lambda x,y : abs(x-y)<1e-5, hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].motion_dir, tmp_value))) + assert(ret and (list(map (lambda x,y : abs(x-y)<1e-5, hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].motion_dir, tmp_value)))) rfup.p_gain = tmp_value = rfup.p_gain*0.1 ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) print(" p_gain ...", file=sys.stderr) @@ -154,7 +154,7 @@ def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-reference-force-up def checkDataPortFromLog(port_name, log_fname="/tmp/test-samplerobot-reference-force-updater-check-port",save_log=True, rtc_name="rfu"): if save_log: saveLogForCheckParameter(log_fname) - return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + return list(map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])) def demo(): init() diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py b/sample/SampleRobot/samplerobot_remove_force_offset.py index 02be8be248e..8d862a9af3c 100755 --- a/sample/SampleRobot/samplerobot_remove_force_offset.py +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -31,7 +31,7 @@ def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-remove-force-offse def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-remove-force-offset-check-param", save_log=True, rtc_name="rmfo"): if save_log: saveLogForCheckParameter(log_fname) - return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + return list(map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])) def demoGetForceMomentOffsetParam (): print("1. GetForceMomentOffsetParam", file=sys.stderr) @@ -93,9 +93,9 @@ def demoDumpLoadForceMomentOffsetParams(): ret = hcf.rmfo_svc.dumpForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat") print(" Value check", file=sys.stderr) data_str=filter(lambda x : x.find("lhsensor") >= 0, open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n"))[0] - vcheck = map(float, data_str.split(" ")[7:10]) == l_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == l_fmop.link_offset_mass + vcheck = list(map(float, data_str.split(" ")[7:10])) == l_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == l_fmop.link_offset_mass data_str=filter(lambda x : x.find("rhsensor") >= 0, open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n"))[0] - vcheck = vcheck and map(float, data_str.split(" ")[7:10]) == r_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == r_fmop.link_offset_mass + vcheck = vcheck and list(map(float, data_str.split(" ")[7:10])) == r_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == r_fmop.link_offset_mass import os if ret and os.path.exists("/tmp/test-rmfo-offsets.dat") and vcheck: print(" dumpForceMomentOffsetParams => OK", file=sys.stderr) diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py index 5eecb0494cf..fbcdc32daa9 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -76,7 +76,7 @@ def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-sequence-player-ch def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-sequence-player-check-param", save_log=True, rtc_name="sh"): if save_log: saveLogForCheckParameter(log_fname) - return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + return list(map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])) def checkServoStateFromLog(log_fname="/tmp/test-samplerobot-sequence-player-check-param"): hcf.saveLog(log_fname) @@ -126,7 +126,7 @@ def checkTorque (var_doc, save_log=True): def checkWrenches (var_doc, save_log=True): if save_log: saveLogForCheckParameter() - ret = checkArrayEquality(reduce(lambda x,y:x+y, map(lambda fs : checkParameterFromLog(fs+"Out", save_log=False), ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])), var_doc['wrenches'], eps=1e-5) + ret = checkArrayEquality(reduce(lambda x,y:x+y, [checkParameterFromLog(fs+"Out", save_log=False) for fs in ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor']]), var_doc['wrenches'], eps=1e-5) print(" wrenches => ", ret) assert(ret is True) @@ -446,13 +446,13 @@ def demoSetTargetPose(): print(" check setTargetPose", file=sys.stderr) hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm'); - print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)) + print(list(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0))) checkJointAngles(p0, 0.01) clearLogForCheckParameter(5000) hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm'); - print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)) + print(list(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1))) checkJointAngles(p1, 0.01) assert(checkServoStateFromLog() is True) @@ -462,13 +462,13 @@ def demoSetTargetPose(): print(" check setTargetPose without giving reference frame", file=sys.stderr) hcf.seq_svc.setTargetPose('larm', pos0_world, rpy0_world, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm'); - print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)) + print(list(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0))) checkJointAngles(p0, 0.01) clearLogForCheckParameter(5000) hcf.seq_svc.setTargetPose('larm', pos1_world, rpy1_world, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm'); - print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)) + print(list(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1))) checkJointAngles(p1, 0.01) assert(checkServoStateFromLog() is True) @@ -481,7 +481,7 @@ def demoSetTargetPose(): clearLogForCheckParameter(5000) hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0) hcf.seq_svc.waitInterpolationOfGroup('larm') - print(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)) + print(list(map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1))) checkJointAngles(p1, 0.1) assert(checkServoStateFromLog() is True) hcf.seq_svc.removeJointGroup('larm') diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index dceb7e6785b..73d44d1d5c1 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -66,8 +66,8 @@ def getJointLimitTableInfo (table_idx=0): target_jointId = filter( lambda x : x.name == target_joint_name, bodyinfo._get_links())[0].jointId target_llimit=float(limit_table_list[2+table_idx*6]) target_ulimit=float(limit_table_list[3+table_idx*6]) - self_llimits=map(lambda x : float(x), limit_table_list[4+table_idx*6].split(",")) - self_ulimits=map(lambda x : float(x), limit_table_list[5+table_idx*6].split(",")) + self_llimits=[float(x) for x in limit_table_list[4+table_idx*6].split(",")] + self_ulimits=[float(x) for x in limit_table_list[5+table_idx*6].split(",")] return [self_joint_name, target_joint_name, self_jointId, target_jointId, target_llimit, target_ulimit, @@ -86,7 +86,7 @@ def testLimitTables (table_idx=0, debug=True, loop_mod=1): assert(uret) def testOneLimitTable (self_jointId, target_jointId, limit_table, target_llimit, target_ulimit, angle_violation, debug=True, loop_mod=1, thre=1e-2): - tmp_pose=map(lambda x : x, initial_pose) + tmp_pose=[x for x in initial_pose] ret=[] for idx in range(int(target_ulimit-target_llimit+1)): if idx%loop_mod != 0: # skip if loop_mod is specified @@ -194,7 +194,7 @@ def setAndCheckJointVelocityLimit (joint_name, thre=1e-5, dt=0.002): poslist=[] for line in open("/tmp/test-samplerobot-el-vel-check.SampleRobot(Robot)0_q", "r"): poslist.append(float(line.split(" ")[link_info.jointId+1])) - tmp = map(lambda x,y : x-y, poslist[1:], poslist[0:-1]) + tmp = list(map(lambda x,y : x-y, poslist[1:], poslist[0:-1])) max_ret_vel = max(tmp)/dt if is_upper_limit else min(tmp)/dt is_vel_limited = abs(max_ret_vel - vel_limit) < thre # Enable error limit by reverting limit value and reset joint angle @@ -242,7 +242,7 @@ def setAndCheckJointErrorLimit (joint_name, thre=1e-5): refposlist=[] for line in open("/tmp/test-samplerobot-el-err-check.el_q", "r"): refposlist.append(float(line.split(" ")[link_info.jointId+1])) - tmp = map(lambda x,y : x-y, refposlist, poslist) + tmp = list(map(lambda x,y : x-y, refposlist, poslist)) max_ret_err = max(tmp) if is_upper_limit else min(tmp) is_err_limited = abs(max_ret_err - error_limit) < thre # Enable error limit by reverting limit value and reset joint angle diff --git a/sample/SampleRobot/samplerobot_stabilizer.py b/sample/SampleRobot/samplerobot_stabilizer.py index 8fcb821b210..3ec8e9a855e 100755 --- a/sample/SampleRobot/samplerobot_stabilizer.py +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -71,7 +71,7 @@ def demoSetParameter(): OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp_org.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]) + stp_org.eefm_support_polygon_vertices_sequence = [OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x) for x in [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]] stp_org.eefm_leg_inside_margin=tmp_leg_inside_margin stp_org.eefm_leg_outside_margin=tmp_leg_outside_margin stp_org.eefm_leg_front_margin=tmp_leg_front_margin @@ -108,7 +108,7 @@ def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-stabilizer-check-p def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-stabilizer-check-param", save_log=True, rtc_name="SampleRobot(Robot)0"): if save_log: saveLogForCheckParameter(log_fname) - return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + return list(map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])) def checkActualBaseAttitude(thre=5.0): # degree '''Check whether the robot falls down based on actual robot base-link attitude. diff --git a/sample/SampleRobot/samplerobot_virtual_force_sensor.py b/sample/SampleRobot/samplerobot_virtual_force_sensor.py index 4c470c11772..9f0b49c3730 100755 --- a/sample/SampleRobot/samplerobot_virtual_force_sensor.py +++ b/sample/SampleRobot/samplerobot_virtual_force_sensor.py @@ -30,26 +30,26 @@ def demo (): fsensor_names = hcf.getForceSensorNames() # vs check port_names = fsensor_names - if all(map (lambda x : hcf.vs.port(x), fsensor_names)): + if all([hcf.vs.port(x) for x in fsensor_names]): print(hcf.vs.name(), "ports are OK (", port_names, ")") # seq check - port_names = map (lambda x : x+"Ref", fsensor_names) - if all(map (lambda x : hcf.seq.port(x), port_names)): + port_names = [x+"Ref" for x in fsensor_names] + if all([hcf.seq.port(x) for x in port_names]): print(hcf.seq.name(), "ports are OK (", port_names, ")") # sh check - port_names = map (lambda x : x+"Out", fsensor_names) - if all(map (lambda x : hcf.sh.port(x), port_names)): + port_names = [x+"Out" for x in fsensor_names] + if all([hcf.sh.port(x) for x in port_names]): print(hcf.sh.name(), "ports are OK (", port_names, ")") - port_names = map (lambda x : x+"In", fsensor_names) - if all(map (lambda x : hcf.sh.port(x), port_names)): + port_names = [x+"In" for x in fsensor_names] + if all([hcf.sh.port(x) for x in port_names]): print(hcf.sh.name(), "ports are OK (", port_names, ")") # ic check - port_names = map (lambda x : "ref_"+x+"In", fsensor_names) - if all(map (lambda x : hcf.ic.port(x), port_names)): + port_names = ["ref_"+x+"In" for x in fsensor_names] + if all([hcf.ic.port(x) for x in port_names]): print(hcf.ic.name(), "ports are OK (", port_names, ")") # abc check - port_names = map (lambda x : "ref_"+x+"In", fsensor_names) - if all(map (lambda x : hcf.ic.port(x), port_names)): + port_names = ["ref_"+x+"In" for x in fsensor_names] + if all([hcf.ic.port(x) for x in port_names]): print(hcf.ic.name(), "ports are OK (", port_names, ")") # 2. Test impedance controller diff --git a/src/hrpsys/hrpsys_config.py b/src/hrpsys/hrpsys_config.py index 0586bad84ee..7fba9ccb6fa 100755 --- a/src/hrpsys/hrpsys_config.py +++ b/src/hrpsys/hrpsys_config.py @@ -824,15 +824,14 @@ def getSensors(self, url): if url == '': return [] else: - return sum(map(lambda x: x.sensors, - [x for x in self.getBodyInfo(url)._get_links() if len(x.sensors) > 0]), []) # sum is for list flatten + return sum([x.sensors for x in [x for x in self.getBodyInfo(url)._get_links() if len(x.sensors) > 0]], []) # sum is for list flatten # public method to get sensors list def getForceSensorNames(self): '''!@brief Get list of force sensor names. Returns existence force sensors and virtual force sensors. self.sensors and virtual force sensors are assumed. ''' - ret = map (lambda x : x.name, [x for x in self.sensors if x.type == "Force"]) + ret = [x.name for x in [x for x in self.sensors if x.type == "Force"]] if self.vs != None: ret += [x for x in self.vs.ports.keys() if str.find(x, 'v') >= 0 and str.find(x, 'sensor') >= 0] return ret @@ -2039,7 +2038,7 @@ def startAutoBalancer(self, limbs=None): If Groups is not defined or Groups does not have rarm and larm, rleg and lleg by default. ''' if limbs==None: - if self.Groups != None and "rarm" in map (lambda x : x[0], self.Groups) and "larm" in map (lambda x : x[0], self.Groups): + if self.Groups != None and "rarm" in [x[0] for x in self.Groups] and "larm" in [x[0] for x in self.Groups]: limbs=["rleg", "lleg", "rarm", "larm"] else: limbs=["rleg", "lleg"] @@ -2166,13 +2165,13 @@ def startDefaultUnstableControllers (self, ic_limbs=[], abc_limbs=[]): ''' print(self.configurator_name + "Start Default Unstable Controllers") # Check robot setting - is_legged_robot = map(lambda x: x[0], [x for x in self.Groups if re.match(".*leg", x[0])]) + is_legged_robot = [x[0] for x in [x for x in self.Groups if re.match(".*leg", x[0])]] # Select all arms in "Groups" for impedance as a default setting if not ic_limbs: - ic_limbs = map(lambda x: x[0], [x for x in self.Groups if re.match(".*(arm)", x[0])]) + ic_limbs = [x[0] for x in [x for x in self.Groups if re.match(".*(arm)", x[0])]] # Select all arms/legs in "Groups" for autobalancer as a default setting if not abc_limbs: - abc_limbs = map(lambda x: x[0], [x for x in self.Groups if re.match(".*(leg|arm)", x[0])]) + abc_limbs = [x[0] for x in [x for x in self.Groups if re.match(".*(leg|arm)", x[0])]] # Start controllers for limb in ic_limbs: self.ic_svc.startImpedanceControllerNoWait(limb) @@ -2196,10 +2195,10 @@ def stopDefaultUnstableControllers (self, ic_limbs=[]): ''' print(self.configurator_name + "Stop Default Unstable Controllers") # Check robot setting - is_legged_robot = map(lambda x: x[0], [x for x in self.Groups if re.match(".*leg", x[0])]) + is_legged_robot = [x[0] for x in [x for x in self.Groups if re.match(".*leg", x[0])]] # Select all arms in "Groups" for impedance as a default setting if not ic_limbs: - ic_limbs = map(lambda x: x[0], [x for x in self.Groups if re.match(".*(arm)", x[0])]) + ic_limbs = [x[0] for x in [x for x in self.Groups if re.match(".*(arm)", x[0])]] # Stop controllers if is_legged_robot: self.stopStabilizer() From 396d63442fcd46db702de8aea04c0e91b6199e32 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 15:36:57 +0900 Subject: [PATCH 14/26] apply 2to3 -f reduce --- sample/SampleRobot/samplerobot_emergency_stopper.py | 1 + sample/SampleRobot/samplerobot_sequence_player.py | 1 + 2 files changed, 2 insertions(+) diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index 25cd9a5aca1..583d934cd62 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from functools import reduce try: from hrpsys.hrpsys_config import * diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py index fbcdc32daa9..47aa4b82ebb 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -14,6 +14,7 @@ import time from distutils.version import StrictVersion +from functools import reduce def init (): global hcf, hrpsys_version From a8e5417469957910dee495b1415732aaacd87eaf Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 16:48:08 +0900 Subject: [PATCH 15/26] remove double parentheses in print function --- .../sample4legrobot_auto_balancer.py | 2 +- .../sample4legrobot_stabilizer.py | 2 +- .../SampleRobot/samplerobot_auto_balancer.py | 2 +- .../samplerobot_emergency_stopper.py | 2 +- .../samplerobot_impedance_controller.py | 2 +- .../SampleRobot/samplerobot_kalman_filter.py | 2 +- .../samplerobot_reference_force_updater.py | 2 +- .../samplerobot_remove_force_offset.py | 2 +- .../samplerobot_sequence_player.py | 2 +- .../samplerobot_soft_error_limiter.py | 2 +- sample/SampleRobot/samplerobot_stabilizer.py | 2 +- .../samplespecialjointrobot_auto_balancer.py | 2 +- src/hrpsys/rtm.py | 78 +++++++++---------- 13 files changed, 51 insertions(+), 51 deletions(-) diff --git a/sample/Sample4LegRobot/sample4legrobot_auto_balancer.py b/sample/Sample4LegRobot/sample4legrobot_auto_balancer.py index ee80be86e0f..25dacde428b 100755 --- a/sample/Sample4LegRobot/sample4legrobot_auto_balancer.py +++ b/sample/Sample4LegRobot/sample4legrobot_auto_balancer.py @@ -42,7 +42,7 @@ def init (): hcf.abc_svc.setGaitGeneratorParam(ggp) hcf.startAutoBalancer(limbs=['rleg','lleg','rarm','larm']) hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) def demoGaitGeneratorSetFootSteps(print_str="1. setFootSteps"): print(print_str, file=sys.stderr) diff --git a/sample/Sample4LegRobot/sample4legrobot_stabilizer.py b/sample/Sample4LegRobot/sample4legrobot_stabilizer.py index da280866dd7..d7fa5b516fe 100755 --- a/sample/Sample4LegRobot/sample4legrobot_stabilizer.py +++ b/sample/Sample4LegRobot/sample4legrobot_stabilizer.py @@ -36,7 +36,7 @@ def init (): hcf.abc_svc.setGaitGeneratorParam(ggp) hcf.startAutoBalancer(['rleg', 'lleg', 'rarm', 'larm']) hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) def demoSetParameterAndStartST(): print("1. setParameter", file=sys.stderr) diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index 734847e3c3f..34e61466a7a 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -40,7 +40,7 @@ def init (): hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.waitInterpolation() hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) def testPoseList(pose_list, initial_pose): for pose in pose_list: diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index 583d934cd62..d58e03e2230 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -27,7 +27,7 @@ def init (): wrench_command0 = [0.0]*24 wrench_command1 = [1.0]*24 hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) def arrayDistance (angle1, angle2): return sum([abs(i-j) for (i,j) in zip(angle1,angle2)]) diff --git a/sample/SampleRobot/samplerobot_impedance_controller.py b/sample/SampleRobot/samplerobot_impedance_controller.py index 5a37675fd99..e360c06af82 100755 --- a/sample/SampleRobot/samplerobot_impedance_controller.py +++ b/sample/SampleRobot/samplerobot_impedance_controller.py @@ -19,7 +19,7 @@ def init (): hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) # set initial pose from sample/controller/SampleController/etc/Sample.pos initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] hcf.seq_svc.setJointAngles(initial_pose, 2.0) diff --git a/sample/SampleRobot/samplerobot_kalman_filter.py b/sample/SampleRobot/samplerobot_kalman_filter.py index 255aae8e341..0bd05591c50 100755 --- a/sample/SampleRobot/samplerobot_kalman_filter.py +++ b/sample/SampleRobot/samplerobot_kalman_filter.py @@ -57,7 +57,7 @@ def init (): roll_pitch_pose3=[0.463158,0.281851,-0.0701,0.747965,-0.514677,-0.108534,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.486068,0.189331,-0.083976,1.08676,-0.76299,-0.139173,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] roll_pitch_poses = [roll_pitch_pose1, roll_pitch_pose2, roll_pitch_pose3] hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) def test_kf_plot (test_motion_func, optional_out_file_name): # time [s] # Reset KF and store data during motion diff --git a/sample/SampleRobot/samplerobot_reference_force_updater.py b/sample/SampleRobot/samplerobot_reference_force_updater.py index d043a01a667..ba9e33363b5 100755 --- a/sample/SampleRobot/samplerobot_reference_force_updater.py +++ b/sample/SampleRobot/samplerobot_reference_force_updater.py @@ -21,7 +21,7 @@ def init (): hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) if hcf.rfu != None: hcf.connectLoggerPort(hcf.rfu, 'ref_rhsensorOut') hcf.connectLoggerPort(hcf.rfu, 'ref_lhsensorOut') diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py b/sample/SampleRobot/samplerobot_remove_force_offset.py index 8d862a9af3c..abdc5a28de0 100755 --- a/sample/SampleRobot/samplerobot_remove_force_offset.py +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -23,7 +23,7 @@ def init (): hcf.seq_svc.setJointAngles(initial_pose, 2.5) hcf.waitInterpolation() hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-remove-force-offset-check-param"): hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py index 47aa4b82ebb..e2777ce00dd 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -47,7 +47,7 @@ def init (): 'optionaldata':[0,1,0,0,0.1,0.1,0.1,0.1] # non realistic value } hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) hcf.seq_svc.removeJointGroup('larm') hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); hcf.seq_svc.waitInterpolation(); diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index 73d44d1d5c1..a4c5377ade5 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -37,7 +37,7 @@ def init (): hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.seq_svc.waitInterpolation() hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) def demo (): init() diff --git a/sample/SampleRobot/samplerobot_stabilizer.py b/sample/SampleRobot/samplerobot_stabilizer.py index 3ec8e9a855e..88351ac8457 100755 --- a/sample/SampleRobot/samplerobot_stabilizer.py +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -26,7 +26,7 @@ def init (): initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] half_sitting_pose = [-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): # on < 315.5.0 this outputs huge error log message hcf.seq_svc.setJointAngles(initial_pose, 2.0) diff --git a/sample/SampleSpecialJointRobot/samplespecialjointrobot_auto_balancer.py b/sample/SampleSpecialJointRobot/samplespecialjointrobot_auto_balancer.py index 0fe6e8760e3..c2e2613e0f8 100755 --- a/sample/SampleSpecialJointRobot/samplespecialjointrobot_auto_balancer.py +++ b/sample/SampleSpecialJointRobot/samplespecialjointrobot_auto_balancer.py @@ -27,7 +27,7 @@ def init (): hcf.waitInterpolation() hcf.startAutoBalancer() hrpsys_version = hcf.seq.ref.get_component_profile().version - print(("hrpsys_version = %s"%hrpsys_version)) + print("hrpsys_version = %s"%hrpsys_version) def checkActualBaseAttitude(): rpy = rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation diff --git a/src/hrpsys/rtm.py b/src/hrpsys/rtm.py index 22e17cdbd57..dcb331e3673 100644 --- a/src/hrpsys/rtm.py +++ b/src/hrpsys/rtm.py @@ -120,8 +120,8 @@ def start(self, ec=None, timeout=3.0): return True ret = ec.activate_component(self.ref) if ret != RTC.RTC_OK: - print(('[rtm.py] \033[31m Failed to start %s(%s)\033[0m' % \ - (self.name(), ret))) + print('[rtm.py] \033[31m Failed to start %s(%s)\033[0m' % \ + (self.name(), ret)) return False tm = 0 while tm < timeout: @@ -129,8 +129,8 @@ def start(self, ec=None, timeout=3.0): return True time.sleep(0.01) tm += 0.01 - print(('[rtm.py] \033[31m Failed to start %s(timeout)\033[0m' % \ - self.name())) + print('[rtm.py] \033[31m Failed to start %s(timeout)\033[0m' % \ + self.name()) return False ## @@ -147,8 +147,8 @@ def stop(self, ec=None, timeout=3.0): return True ret = ec.deactivate_component(self.ref) if ret != RTC.RTC_OK: - print(('[rtm.py] \033[31m Failed to stop %s(%s)\033[0m' % \ - (self.name(), ret))) + print('[rtm.py] \033[31m Failed to stop %s(%s)\033[0m' % \ + (self.name(), ret)) return False tm = 0 while tm < timeout: @@ -156,8 +156,8 @@ def stop(self, ec=None, timeout=3.0): return True time.sleep(0.01) tm += 0.01 - print(('[rtm.py] \033[31m Failed to stop %s(timeout)\033[0m' % \ - self.name())) + print('[rtm.py] \033[31m Failed to stop %s(timeout)\033[0m' % \ + self.name()) return False ## @@ -236,7 +236,7 @@ def load(self, basename, initfunc=""): try: self.ref.load_module(path, initfunc) except: - print(("failed to load", path)) + print("failed to load", path) ## # \brief create an instance of RT component @@ -248,7 +248,7 @@ def create(self, module, name=None): if name != None: rtc = findRTC(name) if rtc != None: - print(('RTC named "' + name + '" already exists.')) + print('RTC named "' + name + '" already exists.') return rtc args = module if name != None: @@ -334,7 +334,7 @@ def initCORBA(): mc.parseArgs(rtm_argv) if nshost != None: # these values can be set via other script like "import rtm; rtm.nshost=XXX" - print(("\033[34m[rtm.py] nshost already set as " + str(nshost) + "\033[0m")) + print("\033[34m[rtm.py] nshost already set as " + str(nshost) + "\033[0m") else: try: nshost = mc._argprop.getProperty("corba.nameservers").split(":")[0] @@ -342,10 +342,10 @@ def initCORBA(): raise except: nshost = socket.gethostname() # default - print(("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nshost) + " as nshost \033[0m")) + print("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nshost) + " as nshost \033[0m") if nsport != None: - print(("\033[34m[rtm.py] nsport already set as " + str(nsport) + "\033[0m")) + print("\033[34m[rtm.py] nsport already set as " + str(nsport) + "\033[0m") else: try: nsport = int(mc._argprop.getProperty("corba.nameservers").split(":")[1]) @@ -353,15 +353,15 @@ def initCORBA(): raise except: nsport = 15005 # default - print(("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nsport) + " as nsport \033[0m")) + print("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nsport) + " as nsport \033[0m") if mgrhost != None: - print(("\033[34m[rtm.py] mgrhost already set as " + str(mgrhost) + "\033[0m")) + print("\033[34m[rtm.py] mgrhost already set as " + str(mgrhost) + "\033[0m") else: mgrhost = nshost if mgrport != None: - print(("\033[34m[rtm.py] mgrport already set as " + str(mgrport) + "\033[0m")) + print("\033[34m[rtm.py] mgrport already set as " + str(mgrport) + "\033[0m") else: try: mgrport = int(mc._argprop.getProperty("corba.master_manager").split(":")[1]) @@ -369,10 +369,10 @@ def initCORBA(): raise except: mgrport = 2810 # default - print(("\033[34m[rtm.py] Failed to parse corba.master_manager, use " + str(mgrport) + "\033[0m")) + print("\033[34m[rtm.py] Failed to parse corba.master_manager, use " + str(mgrport) + "\033[0m") - print(("\033[34m[rtm.py] configuration ORB with %s:%s\033[0m"%(nshost, nsport))) - print(("\033[34m[rtm.py] configuration RTCManager with %s:%s\033[0m"%(mgrhost, mgrport))) + print("\033[34m[rtm.py] configuration ORB with %s:%s\033[0m"%(nshost, nsport)) + print("\033[34m[rtm.py] configuration RTCManager with %s:%s\033[0m"%(mgrhost, mgrport)) os.environ['ORBInitRef'] = 'NameService=corbaloc:iiop:%s:%s/NameService' % \ (nshost, nsport) @@ -394,7 +394,7 @@ def initCORBA(): 'Make sure the hostname is correct and the Nameserver is running.\n' + str(e)) except Exception: _, e, _ = sys.exc_info() - print((str(e))) + print(str(e)) return None @@ -426,7 +426,7 @@ def findObject(name, kind="", rnc=None): if not rnc: rnc = rootnc if not rnc: - print(("[ERROR] findObject(%r,kind=%r,rnc=%r) rootnc is not found" % (name, kind, rnc))) + print("[ERROR] findObject(%r,kind=%r,rnc=%r) rootnc is not found" % (name, kind, rnc)) return rnc.resolve(path) ## @@ -438,8 +438,8 @@ def findObject(name, kind="", rnc=None): # def findRTCmanager(hostname=None, rnc=None): if not rootnc: - print(("[ERROR] findRTCmanager(hostname=%r,rnc=%r) rootnc is not defined, need to call initCORBA()" % \ - (hostname, rnc))) + print("[ERROR] findRTCmanager(hostname=%r,rnc=%r) rootnc is not defined, need to call initCORBA()" % \ + (hostname, rnc)) if not hostname: hostname = nshost cxt = None @@ -464,7 +464,7 @@ def getManagerFromNS(hostname, mgr=None): def getManagerDirectly(hostname, mgr=None): global orb, mgrport corbaloc = "corbaloc:iiop:" + hostname + ":" + str(mgrport) + "/manager" - print(("\033[34m[rtm.py] trying to findRTCManager on port" + str(mgrport) + "\033[0m")) + print("\033[34m[rtm.py] trying to findRTCManager on port" + str(mgrport) + "\033[0m") try: obj = orb.string_to_object(corbaloc) mgr = RTCmanager(obj._narrow(RTM.Manager)) @@ -546,10 +546,10 @@ def serializeComponents(rtcs, stopEC=True): else: print('error in add_component()') else: - print((rtc.name() + 'is already serialized')) + print(rtc.name() + 'is already serialized') except Exception: _, e, _ = sys.exc_info() - print(("[rtm.py] \033[31m error in serialize %s of %s %s\033[0m" % (rtc.name(), [[r, r.name()] for r in rtcs], str(e)))) + print("[rtm.py] \033[31m error in serialize %s of %s %s\033[0m" % (rtc.name(), [[r, r.name()] for r in rtcs], str(e))) raise e ## @@ -580,7 +580,7 @@ def disconnectPorts(outP, inP): if len(ports) == 2: pname = ports[1].get_port_profile().name if pname == iname: - print(('[rtm.py] Disconnect %s - %s' %(op.name, iname))) + print('[rtm.py] Disconnect %s - %s' %(op.name, iname)) outP.disconnect(con_prof.connector_id) return True return False @@ -611,21 +611,21 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength if not isinstance(inPs, list): inPs = [inPs] if not outP: - print(('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ - (outP, [inP.get_port_profile().name if inP else inP for inP in inPs], inPs))) + print('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ + (outP, [inP.get_port_profile().name if inP else inP for inP in inPs], inPs)) return for inP in inPs: if not inP: - print(('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ - (outP.get_port_profile().name, inP, inPs))) + print('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ + (outP.get_port_profile().name, inP, inPs)) continue if isConnected(outP, inP) == True: - print(('[rtm.py] %s and %s are already connected' % \ - (outP.get_port_profile().name, inP.get_port_profile().name))) + print('[rtm.py] %s and %s are already connected' % \ + (outP.get_port_profile().name, inP.get_port_profile().name)) continue if dataTypeOfPort(outP) != dataTypeOfPort(inP): - print(('[rtm.py] \033[31m %s and %s have different data types\033[0m' % \ - (outP.get_port_profile().name, inP.get_port_profile().name))) + print('[rtm.py] \033[31m %s and %s have different data types\033[0m' % \ + (outP.get_port_profile().name, inP.get_port_profile().name)) continue nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any(interfaceType)) nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any(dataflow)) @@ -636,8 +636,8 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength nv7 = SDOPackage.NameValue("dataport.data_type", any.to_any(dataTypeOfPort(outP))) con_prof = RTC.ConnectorProfile("connector0", "", [outP, inP], [nv1, nv2, nv3, nv4, nv5, nv6, nv7]) - print(('[rtm.py] Connect ' + outP.get_port_profile().name + ' - ' + \ - inP.get_port_profile().name+' (dataflow_type='+dataflow+', subscription_type='+ subscription+', bufferlength='+str(bufferlength)+', push_rate='+str(rate)+', push_policy='+pushpolicy+')')) + print('[rtm.py] Connect ' + outP.get_port_profile().name + ' - ' + \ + inP.get_port_profile().name+' (dataflow_type='+dataflow+', subscription_type='+ subscription+', bufferlength='+str(bufferlength)+', push_rate='+str(rate)+', push_policy='+pushpolicy+')') ret, prof = inP.connect(con_prof) if ret != RTC.RTC_OK: print("failed to connect") @@ -847,7 +847,7 @@ def findService(rtc, port_name, type_name, instance_name): else: p = rtc.port(port_name) if p == None: - print(("can't find a port named" + port_name)) + print("can't find a port named" + port_name) return None else: port_prof = [p.get_port_profile()] @@ -863,7 +863,7 @@ def findService(rtc, port_name, type_name, instance_name): aif.polarity == PROVIDED: port = pp.port_ref if port == None: - print(("can't find a service named", instance_name)) + print("can't find a service named", instance_name) return None con_prof = RTC.ConnectorProfile("noname", "", [port], []) ret, con_prof = port.connect(con_prof) From 601e8f82aececef0e9fbd2cf59e8eee94fbfbda5 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 14:44:26 +0900 Subject: [PATCH 16/26] apply 2to3 -f execfile --- util/simulator/hrpsys-simulator-jython.in | 4 ++-- util/simulator/hrpsys-simulator-python.in | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/util/simulator/hrpsys-simulator-jython.in b/util/simulator/hrpsys-simulator-jython.in index 7899fea6e9a..1c8e008abbc 100755 --- a/util/simulator/hrpsys-simulator-jython.in +++ b/util/simulator/hrpsys-simulator-jython.in @@ -5,7 +5,7 @@ import time import os import thread -execfile("@CMAKE_INSTALL_PREFIX@/share/hrpsys/jython/waitInput.py") +exec(compile(open("@CMAKE_INSTALL_PREFIX@/share/hrpsys/jython/waitInput.py", "rb").read(), "@CMAKE_INSTALL_PREFIX@/share/hrpsys/jython/waitInput.py", 'exec')) sys.path.append(os.getcwd()) @@ -32,7 +32,7 @@ thread.start_new_thread(runSimulator, args) time.sleep(1) if script != None: - execfile(script) + exec(compile(open(script, "rb").read(), script, 'exec')) diff --git a/util/simulator/hrpsys-simulator-python.in b/util/simulator/hrpsys-simulator-python.in index f404a5731e6..1e8dd173ff3 100755 --- a/util/simulator/hrpsys-simulator-python.in +++ b/util/simulator/hrpsys-simulator-python.in @@ -6,7 +6,7 @@ import os sys.path.append("@python_dist_pkg_dir@/hrpsys") import hrpsysext -execfile("@python_dist_pkg_dir@/hrpsys/waitInput.py") +exec(compile(open("@python_dist_pkg_dir@/hrpsys/waitInput.py", "rb").read(), "@python_dist_pkg_dir@/hrpsys/waitInput.py", 'exec')) sys.path.append(os.getcwd()) @@ -53,7 +53,7 @@ else: sim.start(sim.totalTime) if script != None: - execfile(script) + exec(compile(open(script, "rb").read(), script, 'exec')) From eeb22cdcaf3903782f2b9e98062e33c41fb5ac93 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 14:48:28 +0900 Subject: [PATCH 17/26] apply 2to3 -f raw_input --- sample/SampleRobot/samplerobot_emergency_stopper.py | 4 ++-- src/hrpsys/waitInput.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index d58e03e2230..3ab7fec0942 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -96,11 +96,11 @@ def demoEmergencyStopJointAngleWithKeyInteracton (): print(" send angle_vector_sequence of %d [sec]" % (play_time*len(pose_list)), file=sys.stderr) print(" press Enter to stop / release motion", file=sys.stderr) while True: - raw_input() + input() print(" stop motion", file=sys.stderr) hcf.es_svc.stopMotion() if hcf.seq_svc.isEmpty(): break - raw_input() + input() print(" release motion", file=sys.stderr) hcf.es_svc.releaseMotion() if hcf.seq_svc.isEmpty(): break diff --git a/src/hrpsys/waitInput.py b/src/hrpsys/waitInput.py index 1a1eb203fd3..53d4ed1088e 100644 --- a/src/hrpsys/waitInput.py +++ b/src/hrpsys/waitInput.py @@ -14,7 +14,7 @@ def waitInputConfirm(msg): except Exception: _, e, _ = sys.exc_info() if "couldn't connect to display" in str(e): - c = raw_input(msg+' (Enter [Y/y] to proceed) ').lower() + c = input(msg+' (Enter [Y/y] to proceed) ').lower() return c == 'y' or c == '' raise finally: From acca3729ed6c3c011c6a770435ae78b7b020fe49 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 14:52:34 +0900 Subject: [PATCH 18/26] apply 2to3 -f raise --- jython/waitInput.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jython/waitInput.py b/jython/waitInput.py index 807e90f8c32..b456284c9eb 100644 --- a/jython/waitInput.py +++ b/jython/waitInput.py @@ -14,7 +14,7 @@ def waitInputConfirm(msg): ret = JOptionPane.showConfirmDialog(f, msg, "waitInputConfirm", JOptionPane.OK_CANCEL_OPTION) if ret == 2: - raise StandardError, "script is canceled" + raise Exception("script is canceled") return True def waitInputSelect(msg): @@ -29,7 +29,7 @@ def waitInputSelect(msg): elif ret == 1: return False elif ret == 2: - raise StandardError, "script is canceled" + raise Exception("script is canceled") class posFilter(FileFilter): def accept(self, f): From f1ea1807d6b39d21aed0b8fb6ac107338399acb0 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 17:04:23 +0900 Subject: [PATCH 19/26] manually replace filter function, 2to3 could not resolve --- .../SampleRobot/samplerobot_remove_force_offset.py | 4 ++-- sample/SampleRobot/samplerobot_soft_error_limiter.py | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py b/sample/SampleRobot/samplerobot_remove_force_offset.py index abdc5a28de0..9638e7572cd 100755 --- a/sample/SampleRobot/samplerobot_remove_force_offset.py +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -92,9 +92,9 @@ def demoDumpLoadForceMomentOffsetParams(): print(" Dump param as file", file=sys.stderr) ret = hcf.rmfo_svc.dumpForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat") print(" Value check", file=sys.stderr) - data_str=filter(lambda x : x.find("lhsensor") >= 0, open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n"))[0] + data_str=[x for x in open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n") if x.find("lhsensor") >= 0][0] vcheck = list(map(float, data_str.split(" ")[7:10])) == l_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == l_fmop.link_offset_mass - data_str=filter(lambda x : x.find("rhsensor") >= 0, open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n"))[0] + data_str=[x for x in open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n") if x.find("rhsensor") >= 0][0] vcheck = vcheck and list(map(float, data_str.split(" ")[7:10])) == r_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == r_fmop.link_offset_mass import os if ret and os.path.exists("/tmp/test-rmfo-offsets.dat") and vcheck: diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index a4c5377ade5..51ea6400d17 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -28,7 +28,7 @@ def init (): # load joint limit table from conf file HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip() f=open("{}/share/hrpsys/samples/SampleRobot/SampleRobot.500.el.conf".format(HRPSYS_DIR)) - limit_table_str=filter(lambda x : x.find("joint_limit_table") > -1 , f.readlines())[0] + limit_table_str=[x for x in f.readlines() if x.find("joint_limit_table") > -1][0] limit_table_list=limit_table_str.split(":")[1:] f.close() # set bodyinfo @@ -62,8 +62,8 @@ def deg2rad (ang): def getJointLimitTableInfo (table_idx=0): self_joint_name = limit_table_list[0+table_idx*6].replace(' ', '') target_joint_name = limit_table_list[1+table_idx*6].replace(' ', '') - self_jointId = filter( lambda x : x.name == self_joint_name, bodyinfo._get_links())[0].jointId - target_jointId = filter( lambda x : x.name == target_joint_name, bodyinfo._get_links())[0].jointId + self_jointId = [x for x in bodyinfo._get_links() if x.name == self_joint_name][0].jointId + target_jointId = [x for x in bodyinfo._get_links() if x.name == target_joint_name][0].jointId target_llimit=float(limit_table_list[2+table_idx*6]) target_ulimit=float(limit_table_list[3+table_idx*6]) self_llimits=[float(x) for x in limit_table_list[4+table_idx*6].split(",")] @@ -132,7 +132,7 @@ def testOneLimitTable (self_jointId, target_jointId, limit_table, target_llimit, def setAndCheckJointLimit (joint_name): print(" ", joint_name, file=sys.stderr) # ulimit check - link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0] + link_info=[x for x in bodyinfo._get_links() if x.name==joint_name][0] hcf.seq_svc.setJointAngle(joint_name, math.radians(1)+link_info.ulimit[0], 1) hcf.waitInterpolation() # Dummy setJointAngles to wait for command joint angles are static @@ -163,7 +163,7 @@ def demoPositionLimit(): setAndCheckJointLimit('LARM_SHOULDER_P') def setAndCheckJointVelocityLimit (joint_name, thre=1e-5, dt=0.002): - link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0] + link_info=[x for x in bodyinfo._get_links() if x.name==joint_name][0] # lvlimit and uvlimit existence check if not(len(link_info.lvlimit) == 1 and len(link_info.uvlimit) == 1): print(" ", joint_name, " test neglected because no lvlimit and uvlimit are found.", file=sys.stderr) @@ -212,7 +212,7 @@ def demoVelocityLimit(): setAndCheckJointVelocityLimit('LARM_WRIST_P') def setAndCheckJointErrorLimit (joint_name, thre=1e-5): - link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0] + link_info=[x for x in bodyinfo._get_links() if x.name==joint_name][0] for is_upper_limit in [True, False]: # uvlimit or lvlimit print(" ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit", file=sys.stderr) # Disable error limit for checking vel limit From 2e91c257f7f8c8bce7118db6a04db23bf13d9a00 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 19:07:19 +0900 Subject: [PATCH 20/26] fix string and byte error --- sample/SampleRobot/samplerobot_carry_object.py | 2 +- sample/SampleRobot/samplerobot_data_logger.py | 2 +- sample/SampleRobot/samplerobot_soft_error_limiter.py | 4 ++-- sample/SampleRobot/samplerobot_stabilizer.py | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/sample/SampleRobot/samplerobot_carry_object.py b/sample/SampleRobot/samplerobot_carry_object.py index a83c003d510..88df20d30a5 100755 --- a/sample/SampleRobot/samplerobot_carry_object.py +++ b/sample/SampleRobot/samplerobot_carry_object.py @@ -91,7 +91,7 @@ def init (): hcf.ic_svc.setImpedanceControllerParam("larm", icp) hcf.Groups = defJointGroups() hcf.startDefaultUnstableControllers(['rarm', 'larm'], ["rleg", "lleg", "rarm", "larm"]) - HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip() + HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip().decode() hcf.rmfo_svc.loadForceMomentOffsetParams(HRPSYS_DIR+'/share/hrpsys/samples/SampleRobot/ForceSensorOffset_SampleRobot.txt') def demoDualarmCarryup (is_walk=True, auto_detecion = True): diff --git a/sample/SampleRobot/samplerobot_data_logger.py b/sample/SampleRobot/samplerobot_data_logger.py index 60b24c2acb8..1a170be295e 100755 --- a/sample/SampleRobot/samplerobot_data_logger.py +++ b/sample/SampleRobot/samplerobot_data_logger.py @@ -47,7 +47,7 @@ def demoSetMaxLogLength(): hcf.waitInterpolation() hcf.saveLog("/tmp/test-samplerobot-log") from subprocess import check_output - ret = check_output(['wc', '-l', '/tmp/test-samplerobot-log.sh_qOut']).split(" ")[0] == '100' + ret = check_output(['wc', '-l', '/tmp/test-samplerobot-log.sh_qOut']).decode().split(" ")[0] == '100' if ret: print(" maxLength() =>OK", file=sys.stderr) assert(ret is True) diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index 51ea6400d17..177cfa7f9fe 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -26,7 +26,7 @@ def init (): hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] # load joint limit table from conf file - HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip() + HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip().decode() f=open("{}/share/hrpsys/samples/SampleRobot/SampleRobot.500.el.conf".format(HRPSYS_DIR)) limit_table_str=[x for x in f.readlines() if x.find("joint_limit_table") > -1][0] limit_table_list=limit_table_str.split(":")[1:] @@ -50,7 +50,7 @@ def demo (): def demoTestAllLimitTables(): print("1. demo all jointLimitTables", file=sys.stderr) - for table_idx in range(len(limit_table_list)/6): + for table_idx in range(len(limit_table_list)//6): testLimitTables(table_idx, True, 5) def rad2deg (ang): diff --git a/sample/SampleRobot/samplerobot_stabilizer.py b/sample/SampleRobot/samplerobot_stabilizer.py index 88351ac8457..0f723ae106c 100755 --- a/sample/SampleRobot/samplerobot_stabilizer.py +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -158,7 +158,7 @@ def demoSTLoadPattern (): changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQP) hcf.startStabilizer() # Exec loadPattern - HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip() + HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip().decode() hcf.loadPattern(HRPSYS_DIR+'/share/hrpsys/samples/SampleRobot/data/samplerobot-gopos000', 0.0) hcf.waitInterpolation() ret = checkActualBaseAttitude() @@ -432,7 +432,7 @@ def demoSTMimicRouchTerrainWalk (terrain_height_diff = 0.04): def demo(): - OPENHRP3_DIR=check_output(['pkg-config', 'openhrp3.1', '--variable=prefix']).rstrip() + OPENHRP3_DIR=check_output(['pkg-config', 'openhrp3.1', '--variable=prefix']).rstrip().decode() if os.path.exists(OPENHRP3_DIR+"/share/OpenHRP-3.1/sample/model/sample1_bush.wrl"): init() if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): From 50ecb22ad9c6629e7c88565e5480e466f2c123cf Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 4 Apr 2024 19:08:15 +0900 Subject: [PATCH 21/26] fix converting range to list --- sample/SampleRobot/samplerobot_sequence_player.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py index e2777ce00dd..f55ff7af91f 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -42,7 +42,7 @@ def init (): # 'gsens':[0,0,0], 'waist':[-0.092492, -6.260780e-05, 0.6318, -0.000205, 0.348204, 0.000268], 'waist_acc':[0]*3, - 'torque':range(dof), # non realistic value + 'torque':list(range(dof)), # non realistic value 'wrenches':[1]*6+[-2]*6+[3]*6+[-4]*6, # non realistic value 'optionaldata':[0,1,0,0,0.1,0.1,0.1,0.1] # non realistic value } From 8e9e6c3a894906dc7805a00263ba354307ee39d1 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 11 Apr 2024 17:26:43 +0900 Subject: [PATCH 22/26] use packaging instead of distutils --- package.xml | 22 ++++++++++--------- .../SampleRobot/samplerobot_auto_balancer.py | 2 +- .../samplerobot_collision_detector.py | 2 +- .../samplerobot_emergency_stopper.py | 2 +- .../samplerobot_impedance_controller.py | 2 +- .../SampleRobot/samplerobot_kalman_filter.py | 2 +- .../samplerobot_reference_force_updater.py | 2 +- .../samplerobot_remove_force_offset.py | 2 +- .../samplerobot_sequence_player.py | 2 +- .../samplerobot_soft_error_limiter.py | 2 +- sample/SampleRobot/samplerobot_stabilizer.py | 2 +- 11 files changed, 22 insertions(+), 20 deletions(-) diff --git a/package.xml b/package.xml index ca3e8029a52..a142b10a8d7 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,4 @@ - + hrpsys 315.15.0 @@ -42,15 +42,17 @@ python-tk sdl - cv_bridge - glut - libirrlicht-dev - libqhull - libxml2 - libxmu-dev - openhrp3 - python-tk - sdl + cv_bridge + glut + libirrlicht-dev + libqhull + libxml2 + libxmu-dev + openhrp3 + python-tk + sdl + python-packaging + python3-packaging diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index 34e61466a7a..ffd9c081aad 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -772,7 +772,7 @@ def demoStandingPosResetting(): def demo(): start_time = time.time() init() - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): # sample for AutoBalancer mode demoAutoBalancerFixFeet() diff --git a/sample/SampleRobot/samplerobot_collision_detector.py b/sample/SampleRobot/samplerobot_collision_detector.py index 4058e5dc634..e052f0bb57b 100755 --- a/sample/SampleRobot/samplerobot_collision_detector.py +++ b/sample/SampleRobot/samplerobot_collision_detector.py @@ -147,7 +147,7 @@ def demo(): def demo_co_loop(): init() - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) >= StrictVersion('315.10.0'): demoCollisionCheckSafe() demoCollisionCheckFail() diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index 3ab7fec0942..032e4f1aa4a 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -165,7 +165,7 @@ def demoEmergencyStopReleaseWhenDeactivated(): def demo(key_interaction=False): init() - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) >= StrictVersion('315.6.0'): if key_interaction: demoEmergencyStopJointAngleWithKeyInteracton() diff --git a/sample/SampleRobot/samplerobot_impedance_controller.py b/sample/SampleRobot/samplerobot_impedance_controller.py index e360c06af82..4975c8314b6 100755 --- a/sample/SampleRobot/samplerobot_impedance_controller.py +++ b/sample/SampleRobot/samplerobot_impedance_controller.py @@ -219,7 +219,7 @@ def demo(): init() demoStartStopIMP () - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'): return diff --git a/sample/SampleRobot/samplerobot_kalman_filter.py b/sample/SampleRobot/samplerobot_kalman_filter.py index 0bd05591c50..da7b1f64b8b 100755 --- a/sample/SampleRobot/samplerobot_kalman_filter.py +++ b/sample/SampleRobot/samplerobot_kalman_filter.py @@ -152,7 +152,7 @@ def demoSetKalmanFilterParameter(): def demo(): init() - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoGetKalmanFilterParameter() demoSetKalmanFilterParameter() diff --git a/sample/SampleRobot/samplerobot_reference_force_updater.py b/sample/SampleRobot/samplerobot_reference_force_updater.py index ba9e33363b5..22a0fde0b97 100755 --- a/sample/SampleRobot/samplerobot_reference_force_updater.py +++ b/sample/SampleRobot/samplerobot_reference_force_updater.py @@ -158,7 +158,7 @@ def checkDataPortFromLog(port_name, log_fname="/tmp/test-samplerobot-reference-f def demo(): init() - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) >= StrictVersion('315.9.0'): demoGetReferecenForceUpdateParam() demoSetReferecenForceUpdateParam() diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py b/sample/SampleRobot/samplerobot_remove_force_offset.py index 9638e7572cd..0320f726d20 100755 --- a/sample/SampleRobot/samplerobot_remove_force_offset.py +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -131,7 +131,7 @@ def demoRemoveForceSensorOffsetRMFO(): def demo(): import numpy init() - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoGetForceMomentOffsetParam() demoSetForceMomentOffsetParam() diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py index f55ff7af91f..0f2523ebcbc 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -13,7 +13,7 @@ import socket import time -from distutils.version import StrictVersion +from packaging.version import parse as StrictVersion from functools import reduce def init (): diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index 177cfa7f9fe..7b6bf2b8468 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -41,7 +41,7 @@ def init (): def demo (): init() - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoTestAllLimitTables() demoPositionLimit() diff --git a/sample/SampleRobot/samplerobot_stabilizer.py b/sample/SampleRobot/samplerobot_stabilizer.py index 0f723ae106c..2648a0ccfda 100755 --- a/sample/SampleRobot/samplerobot_stabilizer.py +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -15,7 +15,7 @@ import math from subprocess import check_output -from distutils.version import StrictVersion +from packaging.version import parse as StrictVersion def init (): global hcf, initial_pose, half_sitting_pose, hrpsys_version From 65027124e230625993aeeb0b6bba150242454e8c Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 11 Apr 2024 17:29:18 +0900 Subject: [PATCH 23/26] fix invalid return value in KalmanFilter --- rtc/KalmanFilter/KalmanFilter.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rtc/KalmanFilter/KalmanFilter.cpp b/rtc/KalmanFilter/KalmanFilter.cpp index 873fd47b35d..79399c9cece 100644 --- a/rtc/KalmanFilter/KalmanFilter.cpp +++ b/rtc/KalmanFilter/KalmanFilter.cpp @@ -311,6 +311,7 @@ bool KalmanFilter::resetKalmanFilterState() { rpy_kf.resetKalmanFilterState(); ekf_filter.resetKalmanFilterState(); + return true; }; bool KalmanFilter::getKalmanFilterParam(OpenHRP::KalmanFilterService::KalmanFilterParam& i_param) From ae9374864be314bd2774c4ee576161db8f260a84 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 13 Jun 2024 20:08:55 +0900 Subject: [PATCH 24/26] python2 python3 compatible for input() function --- sample/SampleRobot/samplerobot_emergency_stopper.py | 3 +++ src/hrpsys/waitInput.py | 3 +++ 2 files changed, 6 insertions(+) diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index 032e4f1aa4a..d29b3d9de5d 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -14,6 +14,9 @@ import socket import time +if hasattr(__builtins__, 'raw_input'): + input = raw_input + def init (): global hcf, init_pose, reset_pose, wrench_command0, wrench_command1, hrpsys_version hcf = HrpsysConfigurator() diff --git a/src/hrpsys/waitInput.py b/src/hrpsys/waitInput.py index 53d4ed1088e..f41dcbd26e1 100644 --- a/src/hrpsys/waitInput.py +++ b/src/hrpsys/waitInput.py @@ -4,6 +4,9 @@ import builtins import threading +if hasattr(__builtins__, 'raw_input'): + input = raw_input + def waitInputConfirm(msg): root = None try: From bca79321ff68122d023c6f9c95f89c85b4e78bbe Mon Sep 17 00:00:00 2001 From: kirohy Date: Fri, 14 Jun 2024 15:28:24 +0900 Subject: [PATCH 25/26] python2 python3 compatible for parsing hrpsys_version --- .../SampleRobot/samplerobot_auto_balancer.py | 3 ++- sample/SampleRobot/samplerobot_carry_object.py | 1 + .../samplerobot_collision_detector.py | 3 ++- sample/SampleRobot/samplerobot_data_logger.py | 1 + .../samplerobot_emergency_stopper.py | 3 ++- .../samplerobot_impedance_controller.py | 3 ++- .../SampleRobot/samplerobot_kalman_filter.py | 3 ++- .../samplerobot_reference_force_updater.py | 3 ++- .../samplerobot_remove_force_offset.py | 3 ++- .../SampleRobot/samplerobot_sequence_player.py | 3 ++- .../samplerobot_soft_error_limiter.py | 3 ++- sample/SampleRobot/samplerobot_stabilizer.py | 3 ++- src/hrpsys/hrpsys_config.py | 18 +++++++++--------- 13 files changed, 31 insertions(+), 19 deletions(-) diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index ffd9c081aad..15cae4e1a33 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -39,7 +40,7 @@ def init (): pose_list=[half_sitting_pose, root_rot_x_pose, root_rot_y_pose, arm_front_pose, four_legs_mode_pose] hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.waitInterpolation() - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) def testPoseList(pose_list, initial_pose): diff --git a/sample/SampleRobot/samplerobot_carry_object.py b/sample/SampleRobot/samplerobot_carry_object.py index 88df20d30a5..b238fe55db8 100755 --- a/sample/SampleRobot/samplerobot_carry_object.py +++ b/sample/SampleRobot/samplerobot_carry_object.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * diff --git a/sample/SampleRobot/samplerobot_collision_detector.py b/sample/SampleRobot/samplerobot_collision_detector.py index e052f0bb57b..6d696159043 100755 --- a/sample/SampleRobot/samplerobot_collision_detector.py +++ b/sample/SampleRobot/samplerobot_collision_detector.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -30,7 +31,7 @@ def init (): init_pose = [0]*29 col_safe_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,0.15708,-0.113446,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0] col_fail_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.845363,0.03992,0.250074,-1.32816,0.167513,0.016204,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0] - hrpsys_version = hcf.co.ref.get_component_profile().version + hrpsys_version = hcf.co.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version, file=sys.stderr) # demo functions diff --git a/sample/SampleRobot/samplerobot_data_logger.py b/sample/SampleRobot/samplerobot_data_logger.py index 1a170be295e..c1f6602b5c5 100755 --- a/sample/SampleRobot/samplerobot_data_logger.py +++ b/sample/SampleRobot/samplerobot_data_logger.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index d29b3d9de5d..f27326c014f 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function from functools import reduce try: @@ -29,7 +30,7 @@ def init (): reset_pose = [0.0,-0.772215,0.0,1.8338,-1.06158,0.0,0.523599,0.0,0.0,-2.44346,0.15708,-0.113446,0.637045,0.0,-0.772215,0.0,1.8338,-1.06158,0.0,0.523599,0.0,0.0,-2.44346,-0.15708,-0.113446,-0.637045,0.0,0.0,0.0] wrench_command0 = [0.0]*24 wrench_command1 = [1.0]*24 - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) def arrayDistance (angle1, angle2): diff --git a/sample/SampleRobot/samplerobot_impedance_controller.py b/sample/SampleRobot/samplerobot_impedance_controller.py index 4975c8314b6..f07c1bd658f 100755 --- a/sample/SampleRobot/samplerobot_impedance_controller.py +++ b/sample/SampleRobot/samplerobot_impedance_controller.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -18,7 +19,7 @@ def init (): hcf = HrpsysConfigurator() hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) # set initial pose from sample/controller/SampleController/etc/Sample.pos initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] diff --git a/sample/SampleRobot/samplerobot_kalman_filter.py b/sample/SampleRobot/samplerobot_kalman_filter.py index da7b1f64b8b..6b620199eb8 100755 --- a/sample/SampleRobot/samplerobot_kalman_filter.py +++ b/sample/SampleRobot/samplerobot_kalman_filter.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -56,7 +57,7 @@ def init (): roll_pitch_pose2=[-0.486326,-1.18821,-0.026531,0.908889,-0.267927,0.130916,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.430362,-0.964194,0.009303,0.590166,-0.173131,0.103544,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] roll_pitch_pose3=[0.463158,0.281851,-0.0701,0.747965,-0.514677,-0.108534,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.486068,0.189331,-0.083976,1.08676,-0.76299,-0.139173,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] roll_pitch_poses = [roll_pitch_pose1, roll_pitch_pose2, roll_pitch_pose3] - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) def test_kf_plot (test_motion_func, optional_out_file_name): # time [s] diff --git a/sample/SampleRobot/samplerobot_reference_force_updater.py b/sample/SampleRobot/samplerobot_reference_force_updater.py index 22a0fde0b97..33419db0697 100755 --- a/sample/SampleRobot/samplerobot_reference_force_updater.py +++ b/sample/SampleRobot/samplerobot_reference_force_updater.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -20,7 +21,7 @@ def init (): hcf = HrpsysConfigurator() hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) if hcf.rfu != None: hcf.connectLoggerPort(hcf.rfu, 'ref_rhsensorOut') diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py b/sample/SampleRobot/samplerobot_remove_force_offset.py index 0320f726d20..0eaebb07111 100755 --- a/sample/SampleRobot/samplerobot_remove_force_offset.py +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -22,7 +23,7 @@ def init (): initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0.637045, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, -0.637045, 0, 0, 0] hcf.seq_svc.setJointAngles(initial_pose, 2.5) hcf.waitInterpolation() - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-remove-force-offset-check-param"): diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py index 0f2523ebcbc..3d880c4e386 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -46,7 +47,7 @@ def init (): 'wrenches':[1]*6+[-2]*6+[3]*6+[-4]*6, # non realistic value 'optionaldata':[0,1,0,0,0.1,0.1,0.1,0.1] # non realistic value } - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) hcf.seq_svc.removeJointGroup('larm') hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index 7b6bf2b8468..473924fe0d6 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -36,7 +37,7 @@ def init (): # set initial pose from sample/controller/SampleController/etc/Sample.pos hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.seq_svc.waitInterpolation() - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) def demo (): diff --git a/sample/SampleRobot/samplerobot_stabilizer.py b/sample/SampleRobot/samplerobot_stabilizer.py index 2648a0ccfda..5065bcfcbdb 100755 --- a/sample/SampleRobot/samplerobot_stabilizer.py +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +from __future__ import print_function try: from hrpsys.hrpsys_config import * @@ -25,7 +26,7 @@ def init (): # set initial pose from sample/controller/SampleController/etc/Sample.pos initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] half_sitting_pose = [-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] - hrpsys_version = hcf.seq.ref.get_component_profile().version + hrpsys_version = hcf.seq.ref.get_component_profile().version.strip('"') print("hrpsys_version = %s"%hrpsys_version) if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): # on < 315.5.0 this outputs huge error log message diff --git a/src/hrpsys/hrpsys_config.py b/src/hrpsys/hrpsys_config.py index 7fba9ccb6fa..4f069850e67 100755 --- a/src/hrpsys/hrpsys_config.py +++ b/src/hrpsys/hrpsys_config.py @@ -367,14 +367,14 @@ def connectComps(self): connectPorts(self.seq.port("basePos"), self.sh.port("basePosIn")) connectPorts(self.seq.port("baseRpy"), self.sh.port("baseRpyIn")) connectPorts(self.seq.port("zmpRef"), self.sh.port("zmpIn")) - if StrictVersion(self.seq_version) >= StrictVersion('315.2.6'): + if StrictVersion(self.seq_version.strip('"')) >= StrictVersion('315.2.6'): connectPorts(self.seq.port("optionalData"), self.sh.port("optionalDataIn")) connectPorts(self.sh.port("basePosOut"), [self.seq.port("basePosInit"), self.fk.port("basePosRef")]) connectPorts(self.sh.port("baseRpyOut"), [self.seq.port("baseRpyInit"), self.fk.port("baseRpyRef")]) connectPorts(self.sh.port("qOut"), self.seq.port("qInit")) - if StrictVersion(self.seq_version) >= StrictVersion('315.2.0'): + if StrictVersion(self.seq_version.strip('"')) >= StrictVersion('315.2.0'): connectPorts(self.sh.port("zmpOut"), self.seq.port("zmpRefInit")) for sen in self.getForceSensorNames(): connectPorts(self.seq.port(sen + "Ref"), @@ -466,14 +466,14 @@ def connectComps(self): # connection for ic if self.ic: connectPorts(self.rh.port("q"), self.ic.port("qCurrent")) - if StrictVersion(self.seq_version) >= StrictVersion('315.3.0'): + if StrictVersion(self.seq_version.strip('"')) >= StrictVersion('315.3.0'): connectPorts(self.sh.port("basePosOut"), self.ic.port("basePosIn")) connectPorts(self.sh.port("baseRpyOut"), self.ic.port("baseRpyIn")) # connection for rfu if self.rfu: if self.es: connectPorts(self.es.port("q"), self.rfu.port("qRef")) - if StrictVersion(self.seq_version) >= StrictVersion('315.3.0'): + if StrictVersion(self.seq_version.strip('"')) >= StrictVersion('315.3.0'): connectPorts(self.sh.port("basePosOut"), self.rfu.port("basePosIn")) connectPorts(self.sh.port("baseRpyOut"), self.rfu.port("baseRpyIn")) # connection for tf @@ -812,7 +812,7 @@ def getBodyInfo(self, url): mdlldr = obj._narrow(ModelLoader_idl._0_OpenHRP__POA.ModelLoader) url = self.parseUrl(url) print(self.configurator_name + " bodyinfo URL = file://" + url) - return mdlldr.getBodyInfo("file://" + url) + return mdlldr.getBodyInfo(str("file://" + url)) # public method to get sensors list def getSensors(self, url): @@ -1263,7 +1263,7 @@ def getCurrentPose(self, lname=None, frame_name=None): raise RuntimeError("need to specify joint name") if frame_name: lname = lname + ':' + frame_name - if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname: + if StrictVersion(self.fk_version.strip('"')) < StrictVersion('315.2.5') and ':' in lname: raise RuntimeError('frame_name ('+lname+') is not supported') pose = self.fk_svc.getCurrentPose(lname) if not pose[0]: @@ -1363,7 +1363,7 @@ def getReferencePose(self, lname, frame_name=None): raise RuntimeError("need to specify joint name") if frame_name: lname = lname + ':' + frame_name - if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname: + if StrictVersion(self.fk_version.strip('"')) < StrictVersion('315.2.5') and ':' in lname: raise RuntimeError('frame_name ('+lname+') is not supported') pose = self.fk_svc.getReferencePose(lname) if not pose[0]: @@ -2137,13 +2137,13 @@ def startImpedance(self, arm, **kwargs): (which you can find by "self.hrpsys_version" command). For instance, if your hrpsys is 315.10.1, refer to "startImpedance_315_4" method. ''' - if self.hrpsys_version and StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'): + if self.hrpsys_version and StrictVersion(self.hrpsys_version.strip('"')) < StrictVersion('315.2.0'): print(self.configurator_name + '\033[31mstartImpedance: Try to connect unsupported RTC' + str(self.hrpsys_version) + '\033[0m') else: self.startImpedance_315_4(arm, **kwargs) def stopImpedance(self, arm): - if self.hrpsys_version and StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'): + if self.hrpsys_version and StrictVersion(self.hrpsys_version.strip('"')) < StrictVersion('315.2.0'): print(self.configurator_name + '\033[31mstopImpedance: Try to connect unsupported RTC' + str(self.hrpsys_version) + '\033[0m') else: self.stopImpedance_315_4(arm) From 67f4fc237ebefd7a0b4941ea5681289528316508 Mon Sep 17 00:00:00 2001 From: kirohy Date: Fri, 14 Jun 2024 15:31:12 +0900 Subject: [PATCH 26/26] python2 python3 compatible for boost-python --- util/simulator/CMakeLists.txt | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/util/simulator/CMakeLists.txt b/util/simulator/CMakeLists.txt index b87c2c2168f..1f5307fb837 100644 --- a/util/simulator/CMakeLists.txt +++ b/util/simulator/CMakeLists.txt @@ -32,12 +32,21 @@ add_library(hrpsysext SHARED PyShape.cpp ) -find_package(Boost REQUIRED COMPONENTS python38) -target_link_libraries(hrpsysext - Boost::python38 - hrpsysUtil - ${PYTHON_LIBRARIES} +if("$ENV{ROS_DISTRO}" STRGREATER "melodic") + find_package(Boost REQUIRED COMPONENTS python38) + target_link_libraries(hrpsysext + Boost::python38 + hrpsysUtil + ${PYTHON_LIBRARIES} + ) +else() + find_package(Boost REQUIRED COMPONENTS python) + target_link_libraries(hrpsysext + Boost::python + hrpsysUtil + ${PYTHON_LIBRARIES} ) +endif() set_target_properties(hrpsysext PROPERTIES PREFIX "") set_target_properties(hrpsysext PROPERTIES SUFFIX ".so")