samplerobot_emergency_stopper.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 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 # demo functions
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()


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:18