samplerobot_soft_error_limiter.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 try:
00004     from hrpsys.hrpsys_config import *
00005     import OpenHRP
00006 except:
00007     print "import without hrpsys"
00008     import rtm
00009     from rtm import *
00010     from OpenHRP import *
00011     import waitInput
00012     from waitInput import *
00013     import socket
00014     import time
00015 
00016 from subprocess import check_output
00017 
00018 # Tempolarily remove tc which is limit position range
00019 def getRTCList ():
00020     return filter(lambda x : x[0]!='tc', hcf.getRTCListUnstable())
00021 
00022 def init ():
00023     global hcf, initial_pose, limit_table_list, bodyinfo, hrpsys_version
00024     hcf = HrpsysConfigurator()
00025     hcf.getRTCList = getRTCList
00026     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00027     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]
00028     # load joint limit table from conf file
00029     HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip()
00030     f=open("{}/share/hrpsys/samples/SampleRobot/SampleRobot.500.el.conf".format(HRPSYS_DIR))
00031     limit_table_str=filter(lambda x : x.find("joint_limit_table") > -1 , f.readlines())[0]
00032     limit_table_list=limit_table_str.split(":")[1:]
00033     f.close()
00034     # set bodyinfo
00035     bodyinfo=hcf.getBodyInfo("$(PROJECT_DIR)/../model/sample1.wrl")
00036     # set initial pose from sample/controller/SampleController/etc/Sample.pos
00037     hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00038     hcf.seq_svc.waitInterpolation()
00039     hrpsys_version = hcf.seq.ref.get_component_profile().version
00040     print("hrpsys_version = %s"%hrpsys_version)
00041 
00042 def demo ():
00043     init()
00044     from distutils.version import StrictVersion
00045     if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
00046         demoTestAllLimitTables()
00047         demoPositionLimit()
00048         demoVelocityLimit()
00049         demoErrorLimit()
00050 
00051 def demoTestAllLimitTables():
00052     print >> sys.stderr, "1. demo all jointLimitTables"
00053     for table_idx in range(len(limit_table_list)/6):
00054         testLimitTables(table_idx, True, 5)
00055 
00056 def rad2deg (ang):
00057     return 180.0*ang/3.14159
00058 
00059 def deg2rad (ang):
00060     return 3.14159*ang/180.0
00061 
00062 def getJointLimitTableInfo (table_idx=0):
00063     self_joint_name = limit_table_list[0+table_idx*6].replace(' ', '')
00064     target_joint_name = limit_table_list[1+table_idx*6].replace(' ', '')
00065     self_jointId = filter( lambda x : x.name == self_joint_name, bodyinfo._get_links())[0].jointId
00066     target_jointId = filter( lambda x : x.name == target_joint_name, bodyinfo._get_links())[0].jointId
00067     target_llimit=float(limit_table_list[2+table_idx*6])
00068     target_ulimit=float(limit_table_list[3+table_idx*6])
00069     self_llimits=map(lambda x : float(x), limit_table_list[4+table_idx*6].split(","))
00070     self_ulimits=map(lambda x : float(x), limit_table_list[5+table_idx*6].split(","))
00071     return [self_joint_name, target_joint_name,
00072             self_jointId, target_jointId,
00073             target_llimit, target_ulimit,
00074             self_llimits, self_ulimits]
00075 
00076 def testLimitTables (table_idx=0, debug=True, loop_mod=1):
00077     [self_joint_name, target_joint_name,
00078      self_jointId, target_jointId,
00079      target_llimit, target_ulimit,
00080      self_llimits, self_ulimits] = getJointLimitTableInfo(table_idx)
00081     lret = testOneLimitTable(self_jointId, target_jointId, self_llimits, target_llimit, target_ulimit, -1, debug, loop_mod)
00082     uret = testOneLimitTable(self_jointId, target_jointId, self_ulimits, target_llimit, target_ulimit, 1, debug, loop_mod)
00083     print >> sys.stderr, "lower limit check(", self_joint_name, ",", target_joint_name,")=", lret
00084     print >> sys.stderr, "upper limit check(", self_joint_name, ",", target_joint_name,")=", uret
00085     assert(lret)
00086     assert(uret)
00087 
00088 def testOneLimitTable (self_jointId, target_jointId, limit_table, target_llimit, target_ulimit, angle_violation, debug=True, loop_mod=1, thre=1e-2):
00089     tmp_pose=map(lambda x : x, initial_pose)
00090     ret=[]
00091     for idx in range(int(target_ulimit-target_llimit+1)):
00092         if idx%loop_mod != 0: # skip if loop_mod is specified
00093             continue
00094         if debug:
00095             print "idx=",idx,
00096         # A-1. set safe joint
00097         tmp_pose[target_jointId]=deg2rad(target_llimit + idx);
00098         tmp_pose[self_jointId]=deg2rad(limit_table[idx]);
00099         if idx == 0:
00100             hcf.seq_svc.setJointAngles(tmp_pose, 1);
00101         else:
00102             hcf.seq_svc.setJointAngles(tmp_pose, 0.01);
00103         hcf.seq_svc.waitInterpolation()
00104         # A-2. check joint limit is not violated
00105         #   Dummy setJointAngles to wait for command joint angles are static
00106         hcf.seq_svc.setJointAngles(tmp_pose, 0.01);hcf.seq_svc.waitInterpolation();
00107         #   Use RobotHardware's command as SoftErrorLimiter joint angle output
00108         el_out1 = hcf.getActualState().command
00109         ret1 = abs(rad2deg(el_out1[self_jointId])-limit_table[idx]) < thre and abs(rad2deg(el_out1[target_jointId])- (target_llimit + idx)) < thre
00110         # B-1. set violated joint
00111         tmp_pose[self_jointId]=deg2rad(limit_table[idx]+angle_violation);
00112         hcf.seq_svc.setJointAngles(tmp_pose, 0.01);
00113         hcf.seq_svc.waitInterpolation()
00114         # B-2. check joint limit is not violated
00115         #   Dummy setJointAngles to wait for command joint angles are static
00116         hcf.seq_svc.setJointAngles(tmp_pose, 0.01);hcf.seq_svc.waitInterpolation()
00117         #   Use RobotHardware's command as SoftErrorLimiter joint angle output
00118         el_out2 = hcf.getActualState().command
00119         ret2 = abs(rad2deg(el_out2[self_jointId]) - limit_table[int(round(rad2deg(el_out2[target_jointId])-target_llimit))]) < thre # Check self and target is on limit table
00120         ret2 = ret2 and abs(el_out2[self_jointId] - (limit_table[idx]+angle_violation)) > thre # Check result value is not violated value
00121         # C. results
00122         if debug:
00123             print " ret = (", ret1, ",", ret2,")"
00124             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]"
00125         ret.append(ret1);
00126         ret.append(ret2);
00127         hcf.seq_svc.waitInterpolation()
00128     hcf.seq_svc.setJointAngles(initial_pose, 1);
00129     hcf.seq_svc.waitInterpolation()
00130     return all(ret)
00131 
00132 def setAndCheckJointLimit (joint_name):
00133     print >> sys.stderr, "  ", joint_name
00134     # ulimit check
00135     link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
00136     hcf.seq_svc.setJointAngle(joint_name, math.radians(1)+link_info.ulimit[0], 1)
00137     hcf.waitInterpolation()
00138     #   Dummy setJointAngles to wait for command joint angles are static
00139     hcf.seq_svc.setJointAngle(joint_name, math.radians(1)+link_info.ulimit[0], 0.01);hcf.waitInterpolation()
00140     #   Use RobotHardware's command as SoftErrorLimiter joint angle output
00141     tmppose = hcf.getActualState().command
00142     ret = tmppose[link_info.jointId] <= link_info.ulimit[0]
00143     print >> sys.stderr, "    ulimit = ", ret, "(elout=", tmppose[link_info.jointId], ", limit=", link_info.ulimit[0], ")"
00144     assert(ret)
00145     # llimit check
00146     hcf.seq_svc.setJointAngle(joint_name, math.radians(-1)+link_info.llimit[0], 1)
00147     hcf.waitInterpolation()
00148     #   Dummy setJointAngles to wait for command joint angles are static
00149     hcf.seq_svc.setJointAngle(joint_name, math.radians(-1)+link_info.llimit[0], 0.01);hcf.waitInterpolation()
00150     #   Use RobotHardware's command as SoftErrorLimiter joint angle output
00151     tmppose = hcf.getActualState().command
00152     ret = tmppose[link_info.jointId] >= link_info.llimit[0]
00153     print >> sys.stderr, "    llimit = ", ret, "(elout=", tmppose[link_info.jointId], ", limit=", link_info.llimit[0], ")"
00154     assert(ret)
00155     # go to initial
00156     hcf.seq_svc.setJointAngles(initial_pose, 1.0)
00157     hcf.waitInterpolation()
00158 
00159 def demoPositionLimit():
00160     print >> sys.stderr, "2. Check Position limit"
00161     setAndCheckJointLimit('LARM_WRIST_Y')
00162     setAndCheckJointLimit('LARM_WRIST_P')
00163     setAndCheckJointLimit('LARM_SHOULDER_P')
00164 
00165 def setAndCheckJointVelocityLimit (joint_name, thre=1e-5, dt=0.002):
00166     link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
00167     # lvlimit and uvlimit existence check
00168     if not(len(link_info.lvlimit) == 1 and len(link_info.uvlimit) == 1):
00169         print >> sys.stderr, "  ", joint_name, " test neglected because no lvlimit and uvlimit are found."
00170         return
00171     for is_upper_limit in [True, False]: # uvlimit or lvlimit
00172         print >> sys.stderr, "  ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit"
00173         # Disable error limit for checking vel limit
00174         hcf.el_svc.setServoErrorLimit("all", 100000)
00175         # Test motion and logging
00176         hcf.clearLog()
00177         target_angle = (math.degrees( link_info.ulimit[0] if is_upper_limit else link_info.llimit[0] )*0.99) # 0.99 is margin
00178         vel_limit = link_info.uvlimit[0] if is_upper_limit else link_info.lvlimit[0]
00179         wait_time = abs(target_angle/math.degrees(vel_limit) * 1.1) # 1.1 is margin
00180         hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.1)
00181         hcf.waitInterpolation()
00182         hcf.setJointAngle(joint_name, target_angle, 0.002)
00183         hcf.waitInterpolation()
00184         hcf.setJointAngle(joint_name, target_angle, wait_time) # Wait for finishing of joint motion
00185         hcf.waitInterpolation()
00186         #   Dummy setJointAngles to wait for command joint angles are static
00187         hcf.setJointAngle(joint_name, target_angle, 0.01);hcf.waitInterpolation()
00188         hcf.saveLog("/tmp/test-samplerobot-el-vel-check")
00189         # Check whether joint angle is reached
00190         #   Use RobotHardware's command as SoftErrorLimiter joint angle output
00191         reach_angle = math.degrees(hcf.getActualState().command[link_info.jointId])
00192         is_reached = abs(reach_angle - target_angle) < thre
00193         # Check actual velocity from Datalogger log
00194         poslist=[]
00195         for line in open("/tmp/test-samplerobot-el-vel-check.SampleRobot(Robot)0_q", "r"):
00196             poslist.append(float(line.split(" ")[link_info.jointId+1]))
00197         tmp = map(lambda x,y : x-y, poslist[1:], poslist[0:-1])
00198         max_ret_vel = max(tmp)/dt if is_upper_limit else min(tmp)/dt
00199         is_vel_limited = abs(max_ret_vel - vel_limit) < thre
00200         # Enable error limit by reverting limit value and reset joint angle
00201         hcf.el_svc.setServoErrorLimit("all", (0.2-0.02))
00202         hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5)
00203         hcf.waitInterpolation()
00204         # Check flags and print
00205         print >> sys.stderr, "    is_reached =", is_reached, ", is_vel_limited =", is_vel_limited,
00206         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]"
00207         assert(is_reached and is_vel_limited)
00208 
00209 def demoVelocityLimit():
00210     print >> sys.stderr, "3. Check Velocity limit"
00211     setAndCheckJointVelocityLimit('LARM_WRIST_Y')
00212     setAndCheckJointVelocityLimit('LARM_WRIST_P')
00213 
00214 def setAndCheckJointErrorLimit (joint_name, thre=1e-5):
00215     link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
00216     for is_upper_limit in [True, False]: # uvlimit or lvlimit
00217         print >> sys.stderr, "  ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit"
00218         # Disable error limit for checking vel limit
00219         error_limit = 0.002 if is_upper_limit else -0.002 # [rad]
00220         hcf.el_svc.setServoErrorLimit("all", abs(error_limit))
00221         # Test motion and logging
00222         hcf.clearLog()
00223         target_angle = 3.0 if is_upper_limit else -3.0 # [deg]
00224         wait_time = 0.2
00225         hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.1)
00226         hcf.waitInterpolation()
00227         hcf.setJointAngle(joint_name, target_angle, 0.002)
00228         hcf.waitInterpolation()
00229         hcf.setJointAngle(joint_name, target_angle, wait_time) # Wait for finishing of joint motion
00230         hcf.waitInterpolation()
00231         #   Dummy setJointAngles to wait for command joint angles are static
00232         hcf.setJointAngle(joint_name, target_angle, 0.01);hcf.waitInterpolation()
00233         hcf.saveLog("/tmp/test-samplerobot-el-err-check")
00234         # Check whether joint angle is reached
00235         #   Use RobotHardware's command as SoftErrorLimiter joint angle output
00236         reach_angle = math.degrees(hcf.getActualState().command[link_info.jointId])
00237         is_reached = abs(reach_angle - target_angle) < thre
00238         # Check actual velocity from Datalogger log
00239         poslist=[]
00240         for line in open("/tmp/test-samplerobot-el-err-check.SampleRobot(Robot)0_q", "r"):
00241             poslist.append(float(line.split(" ")[link_info.jointId+1]))
00242         refposlist=[]
00243         for line in open("/tmp/test-samplerobot-el-err-check.el_q", "r"):
00244             refposlist.append(float(line.split(" ")[link_info.jointId+1]))
00245         tmp = map(lambda x,y : x-y, refposlist, poslist)
00246         max_ret_err = max(tmp) if is_upper_limit else min(tmp)
00247         is_err_limited = abs(max_ret_err - error_limit) < thre
00248         # Enable error limit by reverting limit value and reset joint angle
00249         hcf.el_svc.setServoErrorLimit("all", (0.2-0.02))
00250         hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5)
00251         hcf.waitInterpolation()
00252         # Check flags and print
00253         print >> sys.stderr, "    is_reached =", is_reached, ", is_err_limited =", is_err_limited,
00254         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]"
00255         assert(is_reached and is_err_limited)
00256 
00257 def demoErrorLimit():
00258     print >> sys.stderr, "4. Check Error limit"
00259     setAndCheckJointErrorLimit('LARM_WRIST_Y')
00260     setAndCheckJointErrorLimit('LARM_WRIST_P')
00261 
00262 
00263 if __name__ == '__main__':
00264     demo()


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56