samplerobot_reference_force_updater.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 import sys
00017 
00018 def init ():
00019     global hcf, init_pose, reset_pose, wrench_command0, wrench_command1, hrpsys_version
00020     hcf = HrpsysConfigurator()
00021     hcf.getRTCList = hcf.getRTCListUnstable
00022     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00023     hrpsys_version = hcf.seq.ref.get_component_profile().version
00024     print("hrpsys_version = %s"%hrpsys_version)
00025     if hcf.rfu != None:
00026         hcf.connectLoggerPort(hcf.rfu, 'ref_rhsensorOut')
00027         hcf.connectLoggerPort(hcf.rfu, 'ref_lhsensorOut')
00028 
00029 # demo functions
00030 def demoGetReferecenForceUpdateParam ():
00031     print >> sys.stderr, "1. getParam"
00032     for limb_name in ['rarm', 'larm']:
00033         [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam(limb_name)
00034         assert(ret)
00035     [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm2') # Invalid name
00036     assert(not ret)
00037     print >> sys.stderr, "  =>OK"
00038 
00039 def demoSetReferecenForceUpdateParam ():
00040     print >> sys.stderr, "2. setParam"
00041     print >> sys.stderr, "  Valid limb access"
00042     for limb_name in ['rarm', 'larm']:
00043         [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam(limb_name)
00044         ret = hcf.rfu_svc.setReferenceForceUpdaterParam(limb_name, rfup)
00045         assert(ret)
00046     print >> sys.stderr, "  Invalid limb access"
00047     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm2', rfup) # Invalid name
00048     assert(not ret)
00049     print >> sys.stderr, "  =>OK"
00050 
00051 def demoReferenceForceUpdater ():
00052     print >> sys.stderr, "3. Reference Force Update"
00053     import numpy as np
00054     i=1;
00055     #print >> sys.stderr, i,". get param";i+=1
00056     p = hcf.rfu_svc.getReferenceForceUpdaterParam('larm')[1]
00057     p.update_freq=100
00058     p.p_gain=0.1
00059     # set rmfo
00060     r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1]
00061     r_fmop.link_offset_centroid = [0,0.0368,-0.076271]
00062     r_fmop.link_offset_mass = 0.80011
00063     l_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1]
00064     l_fmop.link_offset_centroid = [0,-0.0368,-0.076271]
00065     l_fmop.link_offset_mass = 0.80011
00066     # Set param
00067     hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop)
00068     hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop)
00069     for armName,portName in zip(['rarm', 'larm'],['ref_rhsensorOut','ref_lhsensorOut']):
00070         hcf.rfu_svc.setReferenceForceUpdaterParam(armName,p)
00071         print >> sys.stderr, " 3."+str(i)+". set ref_force from seq [10,0,0]";i+=1
00072         # set ref_force from seq
00073         hcf.seq_svc.setWrenches([0]*12+[10,0,0,0,0,0]*2,1);time.sleep(1)
00074         portData=checkDataPortFromLog(portName)
00075         print >> sys.stderr, portName,portData[0:3]
00076         ret = np.linalg.norm(portData) > 9.9;
00077         assert (ret)
00078         # start/stop rfu
00079         print >> sys.stderr, " 3."+str(i)+". start/stop param for " + armName; i+=1
00080         ##start rfu
00081         hcf.rfu_svc.startReferenceForceUpdater(armName);time.sleep(1)
00082         portData=checkDataPortFromLog(portName)
00083         print >> sys.stderr, portName,portData[0:3]
00084         ret = np.linalg.norm(portData) < 0.1;
00085         assert (ret)
00086         ##stop rfu
00087         hcf.rfu_svc.stopReferenceForceUpdater(armName);time.sleep(1)
00088         portData=checkDataPortFromLog(portName)
00089         print >> sys.stderr, portName,portData[0:3]
00090         ret = np.linalg.norm(portData) > 9.9;
00091         assert (ret)
00092         # reset ref_force from seq
00093         print >> sys.stderr, " 3."+str(i)+". set ref_force from seq [0,0,0]";i+=1
00094         hcf.seq_svc.setWrenches([0]*24,1);time.sleep(1)
00095 
00096 def demoSetReferecenForceUpdateParamWhileActive ():
00097     print >> sys.stderr, "4. setParam while active"
00098     print >> sys.stderr, " 4.0 Start"
00099     hcf.rfu_svc.startReferenceForceUpdater('rarm')
00100     print >> sys.stderr, " 4.1 Check setParam without any changes"
00101     [ret, rfup_org] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')
00102     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup_org)
00103     assert(ret)
00104     print >> sys.stderr, " 4.2 Check setParam which cannot be changed while active"
00105     [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')
00106     rfup.frame = 'world'
00107     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup)
00108     assert(not ret)
00109     [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')
00110     rfup.update_freq = rfup.update_freq*10
00111     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup)
00112     assert(not ret)
00113     [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')
00114     rfup.update_time_ratio = rfup.update_time_ratio*10
00115     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup)
00116     assert(not ret)
00117     print >> sys.stderr, " 4.3 Check setParam which can be changed while active"
00118     [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')
00119     rfup.motion_dir = tmp_value = [0,0,-1]
00120     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup)
00121     print >> sys.stderr, "   motion_dir ..."
00122     assert(ret and (map (lambda x,y : abs(x-y)<1e-5, hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].motion_dir, tmp_value)))
00123     rfup.p_gain = tmp_value = rfup.p_gain*0.1
00124     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup)
00125     print >> sys.stderr, "   p_gain ..."
00126     assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].p_gain-tmp_value) < 1e-5))
00127     rfup.d_gain = tmp_value = rfup.d_gain*0.1
00128     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup)
00129     print >> sys.stderr, "   d_gain ..."
00130     assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].d_gain-tmp_value) < 1e-5))
00131     rfup.i_gain = rfup.i_gain*0.1
00132     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup)
00133     print >> sys.stderr, "   i_gain ..."
00134     assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].i_gain-tmp_value) < 1e-5))
00135     rfup.is_hold_value = tmp_value = not rfup.is_hold_value
00136     ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup)
00137     print >> sys.stderr, "   is_hold_value ..."
00138     assert(ret and hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].is_hold_value == tmp_value)
00139     print >> sys.stderr, " 4.4 Stop"
00140     hcf.rfu_svc.stopReferenceForceUpdater('rarm')
00141     hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup_org)
00142     print >> sys.stderr, "  =>OK"
00143 
00144 def demoReferecenForceUpdateParamFootOriginExtMoment ():
00145     print >> sys.stderr, "5. FootOriginExtMoment"
00146     ret = hcf.rfu_svc.startReferenceForceUpdater('footoriginextmoment')
00147     ret = hcf.rfu_svc.stopReferenceForceUpdater('footoriginextmoment') and ret
00148     assert(ret)
00149     print >> sys.stderr, "  =>OK"
00150 
00151 def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-reference-force-updater-check-port"):
00152     hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname)
00153 
00154 def checkDataPortFromLog(port_name, log_fname="/tmp/test-samplerobot-reference-force-updater-check-port",save_log=True, rtc_name="rfu"):
00155     if save_log:
00156         saveLogForCheckParameter(log_fname)
00157     return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])
00158 
00159 def demo():
00160     init()
00161     from distutils.version import StrictVersion
00162     if StrictVersion(hrpsys_version) >= StrictVersion('315.9.0'):
00163         demoGetReferecenForceUpdateParam()
00164         demoSetReferecenForceUpdateParam()
00165         demoReferenceForceUpdater()
00166         if StrictVersion(hrpsys_version) >= StrictVersion('315.13.0'):
00167             demoSetReferecenForceUpdateParamWhileActive()
00168             demoReferecenForceUpdateParamFootOriginExtMoment()
00169 
00170 if __name__ == '__main__':
00171     demo()


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