00001
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 def init ():
00017 global hcf, init_pose, reset_pose, wrench_command0, wrench_command1, hrpsys_version
00018 hcf = HrpsysConfigurator()
00019 hcf.getRTCList = hcf.getRTCListUnstable
00020 hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00021 if hcf.es != None:
00022 for sen in filter(lambda x: x.type == "Force", hcf.sensors):
00023 hcf.connectLoggerPort(hcf.es, sen.name+"Out")
00024 init_pose = [0]*29
00025 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]
00026 wrench_command0 = [0.0]*24
00027 wrench_command1 = [1.0]*24
00028 hrpsys_version = hcf.seq.ref.get_component_profile().version
00029 print("hrpsys_version = %s"%hrpsys_version)
00030
00031 def arrayDistance (angle1, angle2):
00032 return sum([abs(i-j) for (i,j) in zip(angle1,angle2)])
00033
00034 def arrayApproxEqual (angle1, angle2, thre=1e-3):
00035 return arrayDistance(angle1, angle2) < thre
00036
00037 def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-emergency-stopper-check-param"):
00038 hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname)
00039
00040 def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-emergency-stopper-check-param", save_log=True, rtc_name="es"):
00041 if save_log:
00042 saveLogForCheckParameter(log_fname)
00043 return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])
00044
00045 def getWrenchArray ():
00046 saveLogForCheckParameter()
00047 return reduce(lambda x,y: x+y, (map(lambda fs : checkParameterFromLog(fs+"Out", save_log=False), ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])))
00048
00049
00050 def demoEmergencyStopJointAngle ():
00051 print >> sys.stderr, "1. test stopMotion and releaseMotion for joint angle"
00052 hcf.es_svc.releaseMotion()
00053 hcf.seq_svc.setJointAngles(init_pose, 1.0)
00054 hcf.waitInterpolation()
00055 time.sleep(0.1)
00056 tmp_angle1 = hcf.getActualState().angle
00057 play_time = 10
00058 hcf.seq_svc.setJointAngles(reset_pose, play_time)
00059 print >> sys.stderr, " send angle_vector of %d [sec]" % play_time
00060 time.sleep(4)
00061 print >> sys.stderr, " check whether robot pose is changing"
00062 tmp_angle2 = hcf.getActualState().angle
00063 if arrayApproxEqual(init_pose, tmp_angle1) and not(arrayApproxEqual(tmp_angle1, tmp_angle2)):
00064 print >> sys.stderr, " => robot is moving."
00065 assert (arrayApproxEqual(init_pose, tmp_angle1) and not(arrayApproxEqual(tmp_angle1, tmp_angle2)))
00066 print >> sys.stderr, " stop motion"
00067 hcf.es_svc.stopMotion()
00068 time.sleep(0.1)
00069 print >> sys.stderr, " check whether robot pose remained still"
00070 tmp_angle1 = hcf.getActualState().angle
00071 time.sleep(3)
00072 tmp_angle2 = hcf.getActualState().angle
00073 if arrayApproxEqual(tmp_angle1, tmp_angle2):
00074 print >> sys.stderr, " => robot is not moving. stopMotion is working succesfully."
00075 assert (arrayApproxEqual(tmp_angle1, tmp_angle2))
00076 print >> sys.stderr, " release motion"
00077 hcf.es_svc.releaseMotion()
00078 print >> sys.stderr, " check whether robot pose changed"
00079 tmp_angle1 = hcf.getActualState().angle
00080 hcf.waitInterpolation()
00081 time.sleep(0.1)
00082 tmp_angle2 = hcf.getActualState().angle
00083 if (not(arrayApproxEqual(tmp_angle1, tmp_angle2)) and arrayApproxEqual(tmp_angle2, reset_pose)):
00084 print >> sys.stderr, " => robot is moving. releaseMotion is working succesfully."
00085 assert(not(arrayApproxEqual(tmp_angle1, tmp_angle2)) and arrayApproxEqual(tmp_angle2, reset_pose))
00086 hcf.es_svc.releaseMotion()
00087 hcf.seq_svc.setJointAngles(init_pose, 1.0)
00088 hcf.waitInterpolation()
00089
00090 def demoEmergencyStopJointAngleWithKeyInteracton ():
00091 print >> sys.stderr, "1. test stopMotion and releaseMotion with key interaction for joint angle"
00092 pose_list = [reset_pose, init_pose] * 4
00093 play_time = 5
00094 hcf.seq_svc.playPattern(pose_list, [[0,0,0]]*len(pose_list), [[0,0,0]]*len(pose_list), [play_time]*len(pose_list))
00095 print >> sys.stderr, " send angle_vector_sequence of %d [sec]" % (play_time*len(pose_list))
00096 print >> sys.stderr, " press Enter to stop / release motion"
00097 while True:
00098 raw_input()
00099 print >> sys.stderr, " stop motion"
00100 hcf.es_svc.stopMotion()
00101 if hcf.seq_svc.isEmpty(): break
00102 raw_input()
00103 print >> sys.stderr, " release motion"
00104 hcf.es_svc.releaseMotion()
00105 if hcf.seq_svc.isEmpty(): break
00106 hcf.es_svc.releaseMotion()
00107
00108 def demoEmergencyStopWrench ():
00109 print >> sys.stderr, "2. test stopMotion and releaseMotion for wrench"
00110 hcf.es_svc.releaseMotion()
00111 hcf.seq_svc.setJointAngles(init_pose, 1.0)
00112 hcf.seq_svc.setWrenches(wrench_command0, 1.0)
00113 hcf.waitInterpolation()
00114 time.sleep(0.1)
00115 tmp_wrench1 = getWrenchArray()
00116 play_time = 10
00117 hcf.seq_svc.setWrenches(wrench_command1, play_time)
00118 print >> sys.stderr, " send wrench command of %d [sec]" % play_time
00119 time.sleep(4)
00120 print >> sys.stderr, " check whether wrench is changing"
00121 tmp_wrench2 = getWrenchArray()
00122 if arrayApproxEqual(wrench_command0, tmp_wrench1) and not(arrayApproxEqual(tmp_wrench1, tmp_wrench2)):
00123 print >> sys.stderr, " => wrench is changing."
00124 assert (arrayApproxEqual(wrench_command0, tmp_wrench1) and not(arrayApproxEqual(tmp_wrench1, tmp_wrench2)))
00125 print >> sys.stderr, " stop motion"
00126 hcf.es_svc.stopMotion()
00127 time.sleep(0.1)
00128 print >> sys.stderr, " check whether wrench remained still"
00129 tmp_wrench1 = getWrenchArray()
00130 time.sleep(3)
00131 tmp_wrench2 = getWrenchArray()
00132 if arrayApproxEqual(tmp_wrench1, tmp_wrench2):
00133 print >> sys.stderr, " => wrench is not changing. stopMotion is working succesfully."
00134 assert (arrayApproxEqual(tmp_wrench1, tmp_wrench2))
00135 print >> sys.stderr, " release motion"
00136 hcf.es_svc.releaseMotion()
00137 print >> sys.stderr, " check whether wrench changed"
00138 tmp_wrench1 = getWrenchArray()
00139 hcf.waitInterpolation()
00140 time.sleep(1.0)
00141 tmp_wrench2 = getWrenchArray()
00142 if (not(arrayApproxEqual(tmp_wrench1, tmp_wrench2)) and arrayApproxEqual(tmp_wrench2, wrench_command1)):
00143 print >> sys.stderr, " => wrench is changing. releaseMotion is working succesfully."
00144 assert(not(arrayApproxEqual(tmp_wrench1, tmp_wrench2)) and arrayApproxEqual(tmp_wrench2, wrench_command1))
00145 hcf.es_svc.releaseMotion()
00146 hcf.seq_svc.setWrenches(wrench_command0, 1.0)
00147 hcf.waitInterpolation()
00148
00149 def demoEmergencyStopReleaseWhenDeactivated():
00150 print >> sys.stderr, "3. test transition to release mode when deactivated"
00151 print >> sys.stderr, " stop motion"
00152 hcf.es_svc.stopMotion()
00153 hcf.hes_svc.stopMotion()
00154 if((hcf.es_svc.getEmergencyStopperParam()[1].is_stop_mode == True) and (hcf.hes_svc.getEmergencyStopperParam()[1].is_stop_mode == True)):
00155 print >> sys.stderr, " emergency stopper become stop mode succesfully"
00156 print >> sys.stderr, " deactivate and activate es and hes"
00157 hcf.es.stop()
00158 hcf.hes.stop()
00159 hcf.es.start()
00160 hcf.hes.start()
00161 if((hcf.es_svc.getEmergencyStopperParam()[1].is_stop_mode == False) and (hcf.hes_svc.getEmergencyStopperParam()[1].is_stop_mode == False)):
00162 print >> sys.stderr, " emergency stopper become release mode succesfully"
00163 assert((hcf.es_svc.getEmergencyStopperParam()[1].is_stop_mode == False) and (hcf.hes_svc.getEmergencyStopperParam()[1].is_stop_mode == False))
00164
00165 def demo(key_interaction=False):
00166 init()
00167 from distutils.version import StrictVersion
00168 if StrictVersion(hrpsys_version) >= StrictVersion('315.6.0'):
00169 if key_interaction:
00170 demoEmergencyStopJointAngleWithKeyInteracton()
00171 else:
00172 demoEmergencyStopJointAngle()
00173 demoEmergencyStopWrench()
00174 demoEmergencyStopReleaseWhenDeactivated()
00175
00176 if __name__ == '__main__':
00177 demo()