hrpsys: samplerobot_impedance_controller.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 def init ():
00017     global hcf, hrpsys_version
00018     hcf = HrpsysConfigurator()
00019     hcf.getRTCList = hcf.getRTCListUnstable
00020     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00021     hrpsys_version = hcf.seq.ref.get_component_profile().version
00022     print("hrpsys_version = %s"%hrpsys_version)
00023     # set initial pose from sample/controller/SampleController/etc/Sample.pos
00024     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]
00025     hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00026     hcf.seq_svc.waitInterpolation()
00027 
00028 def demoStartStopIMP ():
00029     # 0. startImpedance + stopImpedance python interface
00030     print >> sys.stderr, "0. startImpedance + stopImpedance python interface"
00031     hcf.startImpedance("rarm")
00032     hcf.startImpedance("larm")
00033     hcf.stopImpedance("larm")
00034     hcf.stopImpedance("rarm")
00035 
00036 def demoGetImpedanceControllerParam ():
00037     # 1. Getter check
00038     print >> sys.stderr, "1. Getter check"
00039     all_get_ret = []
00040     for limb in ["rarm", "larm"]:
00041         all_get_ret.append(hcf.ic_svc.getImpedanceControllerParam(limb)[0])
00042     print >> sys.stderr, "  all_get_ret = ", all_get_ret
00043     assert(all(all_get_ret))
00044     print >> sys.stderr, "  getImpedanceControllerParam => OK"
00045 
00046 def demoSetImpedanceControllerParam ():
00047     # 2. Setter check
00048     print >> sys.stderr, "2. Setter check"
00049     all_set_ret = []
00050     all_value_ret = []
00051     for limb in ["rarm", "larm"]:
00052         [ret1, icp1]=hcf.ic_svc.getImpedanceControllerParam(limb)
00053         icp1.K_r=1.0
00054         icp1.D_r=2.0
00055         icp1.M_r=0.2
00056         all_set_ret.append(hcf.ic_svc.setImpedanceControllerParam(limb, icp1))
00057         [ret2, icp2]=hcf.ic_svc.getImpedanceControllerParam(limb)
00058         all_value_ret.append((icp1.M_r == icp2.M_r) and (icp1.D_r == icp2.D_r) and (icp1.K_r == icp2.K_r))
00059     print >> sys.stderr, "  all_set_ret = ", all_set_ret, ", all_value_ret = ", all_value_ret
00060     assert(all(all_set_ret) and all(all_value_ret))
00061     print >> sys.stderr, "  setImpedanceControllerParam => OK"
00062 
00063 def demoStartImpedanceController ():
00064     # 3. Start impedance
00065     print >> sys.stderr, "3. Start impedance"
00066     all_start_ret = []
00067     all_mode_ret = []
00068     # start
00069     for limb in ["rarm", "larm"]:
00070         [ret, icp]=hcf.ic_svc.getImpedanceControllerParam(limb)
00071         all_start_ret.append(hcf.ic_svc.startImpedanceControllerNoWait(limb))
00072         all_mode_ret.append(icp.controller_mode == OpenHRP.ImpedanceControllerService.MODE_IDLE)
00073     # wait and check
00074     for limb in ["rarm", "larm"]:
00075         hcf.ic_svc.waitImpedanceControllerTransition(limb)
00076         [ret, icp]=hcf.ic_svc.getImpedanceControllerParam(limb)
00077         all_mode_ret.append(icp.controller_mode == OpenHRP.ImpedanceControllerService.MODE_IMP)
00078     # "already start" check
00079     for limb in ["rarm", "larm"]:
00080         all_start_ret.append(not hcf.ic_svc.startImpedanceControllerNoWait(limb))
00081     print >> sys.stderr, "  all_start_ret = ", all_start_ret, ", all_mode_ret = ", all_mode_ret
00082     assert(all(all_start_ret) and all(all_mode_ret))
00083     print >> sys.stderr, "  startImpedanceController => OK"
00084 
00085 def demoSetRefForce ():
00086     # 4. Set ref force and moment
00087     print >> sys.stderr, "4. Set ref force and moment"
00088     hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00089                              0,0,0,0,0,0,
00090                              0,0,0,0,0,0,
00091                              20,10,-10,0,0,0,], 1.0);
00092     hcf.seq_svc.waitInterpolation();
00093     time.sleep(2)
00094     hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00095                              0,0,0,0,0,0,
00096                              0,0,0,0,0,0,
00097                              0,0,0,0,0,0,], 1.0);
00098     hcf.seq_svc.waitInterpolation();
00099     time.sleep(2)
00100     hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00101                              0,0,0,0,0,0,
00102                              0,0,0,0,0,0,
00103                              0,0,0,0.1,-0.1,0.1], 1.0);
00104     hcf.seq_svc.waitInterpolation();
00105     time.sleep(2)
00106     hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00107                              0,0,0,0,0,0,
00108                              0,0,0,0,0,0,
00109                              0,0,0,0,0,0], 1.0);
00110     hcf.seq_svc.waitInterpolation();
00111     time.sleep(2)
00112 
00113 def demoStopImpedanceController ():
00114     # 5. Stop impedance
00115     print >> sys.stderr, "5. Stop impedance"
00116     all_stop_ret = []
00117     all_mode_ret = []
00118     # stop
00119     for limb in ["rarm", "larm"]:
00120         all_stop_ret.append(hcf.ic_svc.stopImpedanceControllerNoWait(limb))
00121     # wait and check
00122     for limb in ["rarm", "larm"]:
00123         hcf.ic_svc.waitImpedanceControllerTransition(limb)
00124         [ret, icp]=hcf.ic_svc.getImpedanceControllerParam(limb)
00125         all_mode_ret.append(icp.controller_mode == OpenHRP.ImpedanceControllerService.MODE_IDLE)
00126     # "already stop" check
00127     for limb in ["rarm", "larm"]:
00128         all_stop_ret.append(not hcf.ic_svc.stopImpedanceControllerNoWait(limb))
00129     print >> sys.stderr, "  all_stop_ret = ", all_stop_ret, ", all_mode_ret = ", all_mode_ret
00130     assert(all(all_stop_ret) and all(all_mode_ret))
00131     print >> sys.stderr, "  stopImpedanceController => OK"
00132 
00133 def demoArmTrackingCheck ():
00134     # 6. Arm tracking check
00135     print >> sys.stderr, "6. Arm tracking check"
00136     hcf.ic_svc.startImpedanceController("rarm")
00137     hcf.setJointAngle("RARM_ELBOW", -40.0, 0.5);
00138     hcf.waitInterpolation()
00139     hcf.setJointAngle("RARM_ELBOW", -70.0, 0.5);
00140     hcf.waitInterpolation()
00141 
00142 def demoWorldFrameCheck ():
00143     # 7. World frame check
00144     if hcf.kinematics_only_mode:
00145         print >> sys.stderr, "7. World frame check"
00146         # tempolarily set use_sh_base_pos_rpy
00147         icp=hcf.ic_svc.getImpedanceControllerParam("rarm")[1]
00148         icp.use_sh_base_pos_rpy = True
00149         hcf.ic_svc.setImpedanceControllerParam("rarm", icp)
00150         # test
00151         hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00152         hcf.seq_svc.waitInterpolation()
00153         hcf.setJointAngle("RLEG_ANKLE_P",40, 1);
00154         hcf.waitInterpolation()
00155         hcf.setJointAngle("RLEG_ANKLE_P",-40, 1);
00156         hcf.waitInterpolation()
00157         hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00158         hcf.seq_svc.waitInterpolation()
00159     else:
00160         print >> sys.stderr, "7. World frame check is not executed in non-kinematics-only-mode"
00161 
00162 def demoWorldFrameRefForceCheck ():
00163     # 8. World frame ref-force check
00164     if hcf.kinematics_only_mode:
00165         print >> sys.stderr, "8. World frame ref-force check"
00166         # tempolarily set use_sh_base_pos_rpy
00167         icp=hcf.ic_svc.getImpedanceControllerParam("rarm")[1]
00168         icp.use_sh_base_pos_rpy = True
00169         hcf.ic_svc.setImpedanceControllerParam("rarm", icp)
00170         # test
00171         hcf.seq_svc.setBaseRpy([0,0,0], 1.0);
00172         hcf.seq_svc.waitInterpolation();
00173         hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00174                                  0,0,0,0,0,0,
00175                                  0,0,0,0,0,0,
00176                                  -40,0,0,0,0,0], 1.0);
00177         hcf.seq_svc.waitInterpolation();
00178         hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00179                                  0,0,0,0,0,0,
00180                                  0,0,0,0,0,0,
00181                                  0,0,0,0,0,0], 1.0);
00182         hcf.seq_svc.waitInterpolation();
00183         hcf.seq_svc.setBaseRpy([0,45*3.14159/180,0], 1.0);
00184         hcf.seq_svc.waitInterpolation();
00185         hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00186                                  0,0,0,0,0,0,
00187                                  0,0,0,0,0,0,
00188                                  -40,0,0,0,0,0], 1.0);
00189         hcf.seq_svc.waitInterpolation();
00190         hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00191                                  0,0,0,0,0,0,
00192                                  0,0,0,0,0,0,
00193                                  0,0,0,0,0,0], 1.0);
00194         hcf.seq_svc.waitInterpolation();
00195         hcf.seq_svc.setBaseRpy([0,0,0], 1.0);
00196         hcf.seq_svc.waitInterpolation();
00197     else:
00198         print >> sys.stderr, "8. World frame ref-force check is not executed in non-kinematics-only-mode"
00199 
00200 def demoOCTDCheck ():
00201     # 1. Object Contact Turnaround Detector set param check
00202     print >> sys.stderr, "1. Object Contact Turnaround Detector set param check"
00203     ret9 = True
00204     detect_time_thre = 0.3
00205     start_time_thre=0.3
00206     for number_disturbance in [0, 1e-5, -1e-5]: # 1e-5 is smaller than dt
00207         octdp=hcf.octd_svc.getObjectContactTurnaroundDetectorParam()[1];
00208         octdp.detect_time_thre = detect_time_thre + number_disturbance
00209         octdp.start_time_thre = start_time_thre + number_disturbance
00210         hcf.octd_svc.setObjectContactTurnaroundDetectorParam(octdp);
00211         octdp2=hcf.octd_svc.getObjectContactTurnaroundDetectorParam()[1];
00212         print >> sys.stderr, "  ", octdp2
00213         ret9 = ret9 and (octdp2.detect_time_thre == detect_time_thre and octdp2.start_time_thre == start_time_thre)
00214     assert(ret9)
00215     print >> sys.stderr, "  => OK"
00216 
00217 
00218 def demo():
00219     init()
00220 
00221     demoStartStopIMP ()
00222     from distutils.version import StrictVersion
00223     if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'):
00224         return
00225 
00226     demoGetImpedanceControllerParam()
00227     demoSetImpedanceControllerParam()
00228     demoStartImpedanceController()
00229     demoSetRefForce()
00230     demoStopImpedanceController()
00231     demoArmTrackingCheck()
00232     demoWorldFrameCheck()
00233     demoWorldFrameRefForceCheck()
00234     demoOCTDCheck()
00235 
00236 if __name__ == '__main__':
00237     demo()