hrpsys: samplerobot_kalman_filter.py Source File

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 math
00017 
00018 def init ():
00019     global hcf, pitch_poses, roll_poses, yaw_poses, roll_pitch_poses, initial_pose, hrpsys_version
00020     hcf = HrpsysConfigurator()
00021     hcf.getRTCList = hcf.getRTCListUnstable
00022     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00023     hcf.connectLoggerPort(hcf.kf, 'baseRpyCurrent')
00024     # initialize poses
00025     # pose1 = [0]*29
00026     # pose2 = [0]*29
00027     # pose2[3] = 20*3.14159/180.0
00028     # pose2[3+7+6] = 20*3.14159/180.0
00029     # pose2[4] = -10*3.14159/180.0
00030     # pose2[4+7+6] = -10*3.14159/180.0
00031     # pose3 = [0]*29
00032     # pose3[3] = -20*3.14159/180.0
00033     # pose3[3+7+6] = -20*3.14159/180.0
00034     # pose3[4] = 10*3.14159/180.0
00035     # pose3[4+7+6] = 10*3.14159/180.0
00036     # hcf.seq_svc.setJointAngles(pose1, 2.5)
00037     # hcf.seq_svc.waitInterpolation()
00038     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]
00039     # reset-pose+ 50 legup + 30deg pitch
00040     pitch_pose1=[0.000112,-0.553688,-0.00028,1.15739,-0.602847,-1.330313e-05,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.000112,-0.553703,-0.00028,1.15739,-0.602833,-1.330597e-05,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00041     pitch_pose2=[8.200401e-05,-0.980787,-0.000375,0.598284,-0.140235,-9.749026e-05,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,8.206224e-05,-0.980716,-0.000375,0.598026,-0.140049,-9.749664e-05,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00042     pitch_pose3=[0.000229,0.313052,-0.000276,0.779751,-0.568344,-3.437126e-05,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.000229,0.313082,-0.000276,0.779709,-0.568332,-3.436731e-05,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00043     pitch_poses = [pitch_pose1, pitch_pose2, pitch_pose3]
00044     # roll
00045     roll_pose1=[0.000112,-0.553688,-0.00028,1.15739,-0.602847,-1.330313e-05,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.000112,-0.553703,-0.00028,1.15739,-0.602833,-1.330597e-05,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00046     roll_pose2=[-0.475317,-0.568231,0.022036,1.1549,-0.595553,0.138268,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.447984,-0.378428,0.020283,0.753855,-0.383755,0.106576,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00047     roll_pose3=[0.44835,-0.377793,-0.020814,0.752451,-0.38309,-0.106633,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.475616,-0.567819,-0.022616,1.15389,-0.595074,-0.138377,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00048     roll_poses = [roll_pose1, roll_pose2, roll_pose3]
00049     # yaw
00050     yaw_pose1=[-0.000113, -0.523213, -0.000225, 1.15585, -0.631772, 0.000245, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, -0.000113, -0.523213, -0.000225, 1.15585, -0.631772, 0.000245, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
00051     yaw_pose2=[-0.000113, -0.523213, -0.000225, 1.15585, -0.631772, 0.000245, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, -0.000113, -0.523213, -0.000225, 1.15585, -0.631772, 0.000245, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.785398]
00052     yaw_pose3=[-0.000113, -0.523213, -0.000225, 1.15585, -0.631772, 0.000245, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, -0.000113, -0.523213, -0.000225, 1.15585, -0.631772, 0.000245, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 1.5708]
00053     yaw_poses = [yaw_pose1, yaw_pose2, yaw_pose3]
00054     # roll + pitch
00055     roll_pitch_pose1=[0.000111,-0.681981,-0.000333,1.41142,-0.728577,-7.604916e-05,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.000111,-0.682003,-0.000333,1.41142,-0.728553,-7.605437e-05,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00056     roll_pitch_pose2=[-0.486326,-1.18821,-0.026531,0.908889,-0.267927,0.130916,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.430362,-0.964194,0.009303,0.590166,-0.173131,0.103544,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00057     roll_pitch_pose3=[0.463158,0.281851,-0.0701,0.747965,-0.514677,-0.108534,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,0.486068,0.189331,-0.083976,1.08676,-0.76299,-0.139173,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00058     roll_pitch_poses = [roll_pitch_pose1, roll_pitch_pose2, roll_pitch_pose3]
00059     hrpsys_version = hcf.seq.ref.get_component_profile().version
00060     print("hrpsys_version = %s"%hrpsys_version)
00061 
00062 def test_kf_plot (test_motion_func, optional_out_file_name): # time [s]
00063     # Reset KF and store data during motion
00064     hcf.log_svc.clear()
00065     test_motion_func()
00066     hcf.log_svc.save("/tmp/test-kf-samplerobot-{0}".format(optional_out_file_name))
00067     # Parse data
00068     #  Estimated base link rpy from KF : time, roll, pitch, yaw
00069     estimated_base_rpy_ret=[]
00070     for line in open("/tmp/test-kf-samplerobot-{0}.kf_baseRpyCurrent".format(optional_out_file_name), "r"):
00071         estimated_base_rpy_ret.append(line.split(" ")[0:-1])
00072     #  Actual rpy from simualtor : time, posx, posy, posz, roll, pitch, yaw
00073     act_rpy_ret=[]
00074     for line in open("/tmp/test-kf-samplerobot-{0}.SampleRobot(Robot)0_WAIST".format(optional_out_file_name), "r"):
00075         act_rpy_ret.append(line.split(" ")[0:-1])
00076     #  Get time list
00077     initial_sec=int(act_rpy_ret[0][0].split(".")[0])
00078     tm_list=map (lambda x : int(x[0].split(".")[0])-initial_sec + float(x[0].split(".")[1]) * 1e-6, act_rpy_ret)
00079     # Plotting
00080     try:
00081         import matplotlib.pyplot as plt
00082         plt.clf()
00083         color_list = ['r', 'g', 'b']
00084         for idx in range(3):
00085             plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+3+idx]) / math.pi, act_rpy_ret), color=color_list[idx])
00086             plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+idx]) / math.pi, estimated_base_rpy_ret), "--", color=color_list[idx])
00087         plt.xlabel("Time [s]")
00088         plt.ylabel("Angle [deg]")
00089         plt.title("KF actual-estimated data (motion time = {0})".format(optional_out_file_name))
00090         plt.legend(("Actual roll",  "Estimated base roll",
00091                     "Actual pitch", "Estimated base pitch",
00092                     "Actual yaw",   "Estimated base yaw"))
00093         plt.savefig("/tmp/test-kf-samplerobot-data-{0}.eps".format(optional_out_file_name))
00094     except:
00095         print >> sys.stderr, "No plot"
00096 
00097 def test_bending_common (time, poses):
00098     hcf.seq_svc.setJointAngles(poses[1], time*0.25)
00099     hcf.seq_svc.waitInterpolation()
00100     hcf.seq_svc.setJointAngles(poses[2], time*0.5)
00101     hcf.seq_svc.waitInterpolation()
00102     hcf.seq_svc.setJointAngles(poses[0], time*0.25)
00103     hcf.seq_svc.waitInterpolation()
00104 
00105 def test_pitch_bending_4s ():
00106     test_bending_common(4.0, pitch_poses)
00107 
00108 def test_pitch_bending_2s ():
00109     test_bending_common(2.0, pitch_poses)
00110 
00111 def test_roll_bending_4s ():
00112     test_bending_common(4.0, roll_poses)
00113 
00114 def test_roll_bending_2s ():
00115     test_bending_common(2.0, roll_poses)
00116 
00117 def test_yaw_bending_4s ():
00118     test_bending_common(4.0, yaw_poses)
00119 
00120 def test_yaw_bending_2s ():
00121     test_bending_common(2.0, yaw_poses)
00122 
00123 def test_roll_pitch_bending_4s ():
00124     test_bending_common(4.0, roll_pitch_poses)
00125 
00126 def test_roll_pitch_bending_2s ():
00127     test_bending_common(2.0, roll_pitch_poses)
00128 
00129 def test_walk ():
00130     hcf.abc_svc.goPos(0.1,0,0)
00131     hcf.abc_svc.waitFootSteps()
00132 
00133 def demoGetKalmanFilterParameter():
00134     print >> sys.stderr, "1. getParameter"
00135     ret=hcf.kf_svc.getKalmanFilterParam()
00136     if ret[0]:
00137         print >> sys.stderr, "  getKalmanFilterParam() => OK"
00138     assert(ret[0])
00139 
00140 def demoSetKalmanFilterParameter():
00141     print >> sys.stderr, "2. setParameter"
00142     kfp=hcf.kf_svc.getKalmanFilterParam()[1]
00143     kfp.Q_angle = 0.001;
00144     kfp.Q_rate = 0.003;
00145     kfp.R_angle = 100;
00146     ret=hcf.kf_svc.setKalmanFilterParam(kfp)
00147     kfp2=hcf.kf_svc.getKalmanFilterParam()[1]
00148     ret2 = ret and kfp.Q_angle == kfp2.Q_angle and kfp.Q_rate == kfp2.Q_rate and kfp.R_angle == kfp2.R_angle
00149     if ret2:
00150         print >> sys.stderr, "  setKalmanFilterParam() => OK"
00151     assert(ret2)
00152 
00153 def demo():
00154     init()
00155     from distutils.version import StrictVersion
00156     if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
00157         demoGetKalmanFilterParameter()
00158         demoSetKalmanFilterParameter()
00159 
00160         # 3. check log and plot
00161         hcf.abc_svc.startAutoBalancer(["rleg", "lleg"])
00162 
00163         for alg in [OpenHRP.KalmanFilterService.RPYKalmanFilter, OpenHRP.KalmanFilterService.QuaternionExtendedKalmanFilter]:
00164             kfp=hcf.kf_svc.getKalmanFilterParam()[1]
00165             kfp.kf_algorithm = alg
00166             hcf.kf_svc.setKalmanFilterParam(kfp)
00167 
00168             hcf.kf_svc.resetKalmanFilterState()
00169             hcf.seq_svc.setJointAngles(pitch_poses[0], 1.0)
00170             hcf.seq_svc.waitInterpolation()
00171             test_kf_plot(test_pitch_bending_2s, "pitch-bending-2s"+str(alg))
00172 
00173             #hcf.kf_svc.resetKalmanFilterState()
00174             hcf.seq_svc.setJointAngles(roll_poses[0], 1.0)
00175             hcf.seq_svc.waitInterpolation()
00176             test_kf_plot(test_roll_bending_2s, "roll-bending-2s"+str(alg))
00177 
00178             #hcf.kf_svc.resetKalmanFilterState()
00179             hcf.seq_svc.setJointAngles(yaw_poses[0], 1.0)
00180             hcf.seq_svc.waitInterpolation()
00181             test_kf_plot(test_yaw_bending_2s, "yaw-bending-4s"+str(alg))
00182 
00183             #hcf.kf_svc.resetKalmanFilterState()
00184             hcf.seq_svc.setJointAngles(roll_pitch_poses[0], 1.0)
00185             hcf.seq_svc.waitInterpolation()
00186             test_kf_plot(test_roll_pitch_bending_4s, "roll-pitch-bending-4s"+str(alg))
00187 
00188             hcf.seq_svc.setJointAngles(initial_pose, 1.0)
00189             #hcf.seq_svc.waitInterpolation()
00190             #hcf.kf_svc.resetKalmanFilterState()
00191             #hcf.seq_svc.setJointAngles(initial_pose, 1.0)
00192             hcf.seq_svc.waitInterpolation()
00193             test_kf_plot(test_walk, "test_walk"+str(alg))
00194 
00195 if __name__ == '__main__':
00196     demo()