samplerobot_remove_force_offset.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, initial_pose, hrpsys_version
00018     hcf = HrpsysConfigurator()
00019     hcf.getRTCList = hcf.getRTCListUnstable
00020     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00021     # set initial pose from sample/controller/SampleController/etc/Sample.pos
00022     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]
00023     hcf.seq_svc.setJointAngles(initial_pose, 2.5)
00024     hcf.waitInterpolation()
00025     hrpsys_version = hcf.seq.ref.get_component_profile().version
00026     print("hrpsys_version = %s"%hrpsys_version)
00027 
00028 def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-remove-force-offset-check-param"):
00029     hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname)
00030 
00031 def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-remove-force-offset-check-param", save_log=True, rtc_name="rmfo"):
00032     if save_log:
00033         saveLogForCheckParameter(log_fname)
00034     return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])
00035 
00036 def demoGetForceMomentOffsetParam ():
00037     print >> sys.stderr, "1. GetForceMomentOffsetParam"
00038     for fs_name in ["rhsensor", "lhsensor"]:
00039         ret = hcf.rmfo_svc.getForceMomentOffsetParam(fs_name)
00040         if ret[0]:
00041             print >> sys.stderr, "    getForceMomentOffsetParam('", fs_name,"') => OK"
00042         assert(ret[0] is True)
00043 
00044 def demoSetForceMomentOffsetParam ():
00045     print >> sys.stderr, "2. SetForceMomentOffsetParam"
00046     print >> sys.stderr, "  Force and moment are large because of link offsets"
00047     saveLogForCheckParameter()
00048     for fs_name in ["rhsensor", "lhsensor"]:
00049         fm = numpy.linalg.norm(checkParameterFromLog("off_"+fs_name, save_log=False))
00050         vret = fm > 5e-2
00051         print >> sys.stderr, "    no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret
00052         assert(vret)
00053     print >> sys.stderr, "  Set link offsets (link_offset_centroid and link_offset_mass are identified value)."
00054     # Get param
00055     r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1]
00056     r_fmop.link_offset_centroid = [0,0.0368,-0.076271]
00057     r_fmop.link_offset_mass = 0.80011
00058     l_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1]
00059     l_fmop.link_offset_centroid = [0,-0.0368,-0.076271]
00060     l_fmop.link_offset_mass = 0.80011
00061     # Set param
00062     hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop)
00063     hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop)
00064     # Check values
00065     ret = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")
00066     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:
00067         print >> sys.stderr, "    getForceMomentOffsetParam('rhsensor') => OK"
00068     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))
00069     ret = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")
00070     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:
00071         print >> sys.stderr, "    getForceMomentOffsetParam('lhsensor') => OK"
00072     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))
00073     print >> sys.stderr, "  Force and moment are reduced"
00074     saveLogForCheckParameter()
00075     for fs_name in ["rhsensor", "lhsensor"]:
00076         fm = numpy.linalg.norm(checkParameterFromLog("off_"+fs_name, save_log=False))
00077         vret = fm < 5e-2
00078         print >> sys.stderr, "    no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret
00079         assert(vret)
00080 
00081 def demoDumpLoadForceMomentOffsetParams():
00082     print >> sys.stderr, "3. Dump and load parameter file"
00083     print >> sys.stderr, "  Get and set param"
00084     r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1]
00085     r_fmop.link_offset_centroid = [0,0.0368,-0.076271]
00086     r_fmop.link_offset_mass = 0.80011
00087     l_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1]
00088     l_fmop.link_offset_centroid = [0,-0.0368,-0.076271]
00089     l_fmop.link_offset_mass = 0.80011
00090     hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop)
00091     hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop)
00092     print >> sys.stderr, "  Dump param as file"
00093     ret = hcf.rmfo_svc.dumpForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat")
00094     print >> sys.stderr, "  Value check"
00095     data_str=filter(lambda x : x.find("lhsensor") >= 0, open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n"))[0]
00096     vcheck = map(float, data_str.split(" ")[7:10]) == l_fmop.link_offset_centroid and float(data_str.split(" ")[10]) == l_fmop.link_offset_mass
00097     data_str=filter(lambda x : x.find("rhsensor") >= 0, open("/tmp/test-rmfo-offsets.dat", "r").read().split("\n"))[0]
00098     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
00099     import os
00100     if ret and os.path.exists("/tmp/test-rmfo-offsets.dat") and vcheck:
00101         print >> sys.stderr, "    dumpForceMomentOffsetParams => OK"
00102     assert((ret and os.path.exists("/tmp/test-rmfo-offsets.dat") and vcheck))
00103     print >> sys.stderr, "  Resetting values"
00104     r_fmop2 = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1]
00105     r_fmop2.link_offset_centroid = [0,0,0]
00106     r_fmop2.link_offset_mass = 0
00107     l_fmop2 = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1]
00108     l_fmop2.link_offset_centroid = [0,0,0]
00109     l_fmop2.link_offset_mass = 0
00110     hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop2)
00111     hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop2)
00112     print >> sys.stderr, "  Load from file"
00113     ret = hcf.rmfo_svc.loadForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat")
00114     r_fmop3 = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1]
00115     l_fmop3 = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1]
00116     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
00117     if ret and vcheck:
00118         print >> sys.stderr, "    loadForceMomentOffsetParams => OK"
00119     assert((ret and vcheck))
00120 
00121 def demoRemoveForceSensorOffsetRMFO():
00122     print >> sys.stderr, "4. remove force sensor offset"
00123     print >> sys.stderr, "  Test valid calibration"
00124     ret = hcf.removeForceSensorOffsetRMFO(tm=1.0) # all sensors by default
00125     print >> sys.stderr, "  Test invalid calibration"
00126     ret = ret and not hcf.removeForceSensorOffsetRMFO(["testtest"], 1.0) # invalid sensor name
00127     if ret:
00128         print >> sys.stderr, "    removeforcesensorlinkoffset => OK"
00129     assert(ret)
00130 
00131 def demo():
00132     import numpy
00133     init()
00134     from distutils.version import StrictVersion
00135     if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
00136         demoGetForceMomentOffsetParam()
00137         demoSetForceMomentOffsetParam()
00138         demoDumpLoadForceMomentOffsetParams()
00139         demoRemoveForceSensorOffsetRMFO()
00140 
00141 if __name__ == '__main__':
00142     demo()


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