diff --git a/CMakeLists.txt b/CMakeLists.txt index 59f2933b317..9e333cab9ec 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) @@ -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) @@ -217,14 +217,12 @@ 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{_}") 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/doc/CMakeLists.txt b/doc/CMakeLists.txt index 989740266d1..82d88203cd9 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 @@ -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) 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/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): 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/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/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) 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..25dacde428b 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 * @@ -45,7 +45,7 @@ def init (): 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..d7fa5b516fe 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 * @@ -39,7 +39,7 @@ def init (): 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] @@ -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 @@ -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/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in b/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in index bf687b60ef1..0dd55c715cb 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 * @@ -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)) @@ -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/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index 606ded99173..15cae4e1a33 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -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): @@ -55,7 +56,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. @@ -64,7 +65,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 +98,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 @@ -109,7 +110,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,21 +123,21 @@ 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 >> 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 +145,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 +156,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 +207,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 +223,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 +240,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 +292,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 +315,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 +334,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 +344,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 +355,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 +372,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 +386,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 +404,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 +424,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 +498,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 +518,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 = ", [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 >> 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 +548,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 +558,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 +568,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 +585,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 +607,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 +625,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 +639,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 +692,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 +703,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 +726,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 +734,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 +759,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 @@ -772,7 +773,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() @@ -806,7 +807,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..b238fe55db8 100755 --- a/sample/SampleRobot/samplerobot_carry_object.py +++ b/sample/SampleRobot/samplerobot_carry_object.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -37,7 +38,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 @@ -91,54 +92,54 @@ 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): - 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 +148,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 +175,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 +197,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 +211,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..6d696159043 100755 --- a/sample/SampleRobot/samplerobot_collision_detector.py +++ b/sample/SampleRobot/samplerobot_collision_detector.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -30,12 +31,12 @@ 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 - print >> sys.stderr, "hrpsys_version = %s"%hrpsys_version + hrpsys_version = hcf.co.ref.get_component_profile().version.strip('"') + 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 +46,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 +123,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(): @@ -147,7 +148,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_data_logger.py b/sample/SampleRobot/samplerobot_data_logger.py index 7f76d04b385..c1f6602b5c5 100755 --- a/sample/SampleRobot/samplerobot_data_logger.py +++ b/sample/SampleRobot/samplerobot_data_logger.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -25,31 +26,31 @@ 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() 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 >> 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..f27326c014f 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -1,10 +1,12 @@ #!/usr/bin/env python +from __future__ import print_function +from functools import reduce 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 * @@ -13,19 +15,22 @@ 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() 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] 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): @@ -40,15 +45,15 @@ 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 (): - 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 +61,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" + input() + print(" stop motion", file=sys.stderr) hcf.es_svc.stopMotion() if hcf.seq_svc.isEmpty(): break - raw_input() - print >> sys.stderr, " release motion" + input() + 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,56 +120,56 @@ 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): 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 8ef9b096823..f07c1bd658f 100755 --- a/sample/SampleRobot/samplerobot_impedance_controller.py +++ b/sample/SampleRobot/samplerobot_impedance_controller.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -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] @@ -27,7 +28,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 +36,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 +57,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 +79,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 +113,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 +127,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 +143,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 +158,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 +196,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,17 +210,17 @@ 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(): 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 0b9405cf37d..6b620199eb8 100755 --- a/sample/SampleRobot/samplerobot_kalman_filter.py +++ b/sample/SampleRobot/samplerobot_kalman_filter.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -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] @@ -75,15 +76,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)) @@ -92,7 +93,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 +132,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,12 +148,12 @@ 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(): 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 d4249cc7b2f..33419db0697 100755 --- a/sample/SampleRobot/samplerobot_reference_force_updater.py +++ b/sample/SampleRobot/samplerobot_reference_force_updater.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -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') @@ -28,28 +29,28 @@ def init (): # 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 +69,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 +115,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 ..." - assert(ret and (map (lambda x,y : abs(x-y)<1e-5, hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].motion_dir, tmp_value))) + print(" motion_dir ...", file=sys.stderr) + 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 >> 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) @@ -154,11 +155,11 @@ 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() - 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 35b7213e9c0..0eaebb07111 100755 --- a/sample/SampleRobot/samplerobot_remove_force_offset.py +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -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"): @@ -31,26 +32,26 @@ 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 >> 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 +65,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 +90,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" - 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 + print(" Value check", file=sys.stderr) + 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=[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: - 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,29 +110,29 @@ 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(): 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 b43db9a155d..3d880c4e386 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -13,7 +14,8 @@ import socket import time -from distutils.version import StrictVersion +from packaging.version import parse as StrictVersion +from functools import reduce def init (): global hcf, hrpsys_version @@ -41,11 +43,11 @@ 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 } - 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); @@ -76,7 +78,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) @@ -85,7 +87,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 +97,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 + 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) 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 +148,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 +156,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 +165,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 +180,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 +222,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 +235,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 +244,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 +255,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 +264,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 +282,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 +292,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 +302,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 +313,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 +331,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 +339,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 +347,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 +373,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 +398,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 +417,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 +445,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(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) 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(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) - 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 +483,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 766c9dfc59c..473924fe0d6 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -17,7 +18,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 @@ -26,9 +27,9 @@ 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=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 @@ -36,12 +37,12 @@ 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 (): init() - from distutils.version import StrictVersion + from packaging.version import parse as StrictVersion if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoTestAllLimitTables() demoPositionLimit() @@ -49,8 +50,8 @@ def demo (): demoErrorLimit() def demoTestAllLimitTables(): - print >> sys.stderr, "1. demo all jointLimitTables" - for table_idx in range(len(limit_table_list)/6): + print("1. demo all jointLimitTables", file=sys.stderr) + for table_idx in range(len(limit_table_list)//6): testLimitTables(table_idx, True, 5) def rad2deg (ang): @@ -62,12 +63,12 @@ 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=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, @@ -80,19 +81,19 @@ 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) 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 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 +121,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,9 +131,9 @@ 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] + 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 @@ -140,7 +141,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,26 +151,26 @@ 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') 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 >> 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 @@ -194,7 +195,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 @@ -202,19 +203,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] + 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 >> 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)) @@ -242,7 +243,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 @@ -250,12 +251,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..5065bcfcbdb 100755 --- a/sample/SampleRobot/samplerobot_stabilizer.py +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -1,10 +1,11 @@ #!/usr/bin/env python +from __future__ import print_function 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 * @@ -15,7 +16,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 @@ -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 @@ -45,12 +46,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] @@ -71,7 +72,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 @@ -88,7 +89,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): @@ -108,21 +109,21 @@ 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. ''' 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 +137,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 @@ -158,18 +159,18 @@ 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() 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 +180,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 +196,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 +242,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 +297,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 +324,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 +340,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 +366,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 +386,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,14 +426,14 @@ 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(): - 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'): @@ -448,7 +449,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..9f0b49c3730 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 * @@ -30,27 +30,27 @@ def demo (): fsensor_names = hcf.getForceSensorNames() # 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, ")" + 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)): - print hcf.seq.name(), "ports are OK (", 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)): - 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, ")" + 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 = [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)): - print hcf.ic.name(), "ports are OK (", 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)): - print hcf.ic.name(), "ports are OK (", 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 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..c2e2613e0f8 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 * @@ -32,19 +32,19 @@ def init (): 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/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) + 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 97% rename from python/hrpsys_config.py rename to src/hrpsys/hrpsys_config.py index 9038aa40369..4f069850e67 100755 --- a/python/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 @@ -335,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")) @@ -366,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"), @@ -446,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), @@ -458,21 +459,21 @@ 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)) # 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 @@ -487,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: @@ -500,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 @@ -562,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: @@ -769,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([c for c in controller_list if c != None]) # only return existing controllers def getRTCInstanceList(self, verbose=True): '''!@brief @@ -795,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 @@ -809,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): @@ -821,18 +824,16 @@ def getSensors(self, url): if 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 + 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, filter(lambda x: x.type == "Force", self.sensors)) + ret = [x.name for x in [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): @@ -913,13 +914,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") @@ -1262,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]: @@ -1362,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]: @@ -2037,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"] @@ -2136,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) @@ -2164,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], filter(lambda x : re.match(".*leg", x[0]), self.Groups)) + 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], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups)) + 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], filter(lambda x : re.match(".*(leg|arm)", x[0]), self.Groups)) + 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) @@ -2194,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], filter(lambda x : re.match(".*leg", x[0]), self.Groups)) + 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], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups)) + 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() 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 98% rename from python/rtm.py rename to src/hrpsys/rtm.py index 2ac1a7fc175..dcb331e3673 100644 --- a/python/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 # @@ -118,7 +120,7 @@ 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' % \ + print('[rtm.py] \033[31m Failed to start %s(%s)\033[0m' % \ (self.name(), ret)) return False tm = 0 @@ -127,7 +129,7 @@ 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' % \ + print('[rtm.py] \033[31m Failed to start %s(timeout)\033[0m' % \ self.name()) return False @@ -145,7 +147,7 @@ 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' % \ + print('[rtm.py] \033[31m Failed to stop %s(%s)\033[0m' % \ (self.name(), ret)) return False tm = 0 @@ -154,7 +156,7 @@ 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' % \ + print('[rtm.py] \033[31m Failed to stop %s(timeout)\033[0m' % \ self.name()) return False diff --git a/python/waitInput.py b/src/hrpsys/waitInput.py similarity index 96% rename from python/waitInput.py rename to src/hrpsys/waitInput.py index 1019e67992f..f41dcbd26e1 100644 --- a/python/waitInput.py +++ b/src/hrpsys/waitInput.py @@ -1,9 +1,12 @@ -from Tkinter import * -from tkMessageBox import * +from tkinter import * +from tkinter.messagebox import * import datetime -import __builtin__ +import builtins import threading +if hasattr(__builtins__, 'raw_input'): + input = raw_input + def waitInputConfirm(msg): root = None try: @@ -14,7 +17,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: @@ -244,7 +247,3 @@ def waitInputMenu(menu): thr = threading.Thread(target=waitInputMenuMain, args=(menu,)) thr.start() return thr - -__builtin__.waitInputConfirm = waitInputConfirm -__builtin__.waitInputSelect = waitInputSelect -__builtin__.waitInputMenu = waitInputMenu 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) diff --git a/util/simulator/CMakeLists.txt b/util/simulator/CMakeLists.txt index beb53389d08..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 python) -target_link_libraries(hrpsysext - Boost::python - 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") diff --git a/util/simulator/hrpsys-simulator-jython.in b/util/simulator/hrpsys-simulator-jython.in index b1873295f89..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()) @@ -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": @@ -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 d051680295a..1e8dd173ff3 100755 --- a/util/simulator/hrpsys-simulator-python.in +++ b/util/simulator/hrpsys-simulator-python.in @@ -6,14 +6,14 @@ 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()) 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] @@ -53,7 +53,7 @@ else: sim.start(sim.totalTime) if script != None: - execfile(script) + exec(compile(open(script, "rb").read(), script, 'exec'))