hrpsys: samplerobot_virtual_force

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
00018     hcf = HrpsysConfigurator()
00019     hcf.getRTCList = hcf.getRTCListUnstable
00020     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00021 
00022 def demo ():
00023     init()
00024     # set initial pose from sample/controller/SampleController/etc/Sample.pos
00025     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]
00026 
00027     # 1. Check wrench data ports existence (real force sensor + virtual force sensor)
00028     hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00029     hcf.seq_svc.waitInterpolation()
00030     fsensor_names = hcf.getForceSensorNames()
00031     # vs check
00032     port_names = fsensor_names
00033     if all(map (lambda x : hcf.vs.port(x), fsensor_names)):
00034         print hcf.vs.name(), "ports are OK (", port_names, ")"
00035     # seq check
00036     port_names = map (lambda x : x+"Ref", fsensor_names)
00037     if all(map (lambda x : hcf.seq.port(x), port_names)):
00038         print hcf.seq.name(), "ports are OK (", port_names, ")"
00039     # sh check
00040     port_names = map (lambda x : x+"Out", fsensor_names)
00041     if all(map (lambda x : hcf.sh.port(x), port_names)):
00042         print hcf.sh.name(), "ports are OK (", port_names, ")"
00043     port_names = map (lambda x : x+"In", fsensor_names)
00044     if all(map (lambda x : hcf.sh.port(x), port_names)):
00045         print hcf.sh.name(), "ports are OK (", port_names, ")"
00046     # ic check
00047     port_names = map (lambda x : "ref_"+x+"In", fsensor_names)
00048     if all(map (lambda x : hcf.ic.port(x), port_names)):
00049         print hcf.ic.name(), "ports are OK (", port_names, ")"
00050     # abc check
00051     port_names = map (lambda x : "ref_"+x+"In", fsensor_names)
00052     if all(map (lambda x : hcf.ic.port(x), port_names)):
00053         print hcf.ic.name(), "ports are OK (", port_names, ")"
00054 
00055     # 2. Test impedance controller
00056     ret1=hcf.ic_svc.getImpedanceControllerParam("vrhsensor")
00057     ret1[1].base_name="CHEST"
00058     ret1[1].target_name="RARM_WRIST_P"
00059     ret1[1].K_r=1.0
00060     ret1[1].D_r=2.0
00061     ret1[1].M_r=0.2
00062     ret2=hcf.ic_svc.setImpedanceControllerParam("vrhsensor", ret1[1])
00063     hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00064                              0,0,0,0,0,0,
00065                              0,0,0,0,0,0,
00066                              0,0,0,0,0,0,
00067                              0,0,0,0,0,0,
00068                              10,10,-10,0,0,0,], 2.0);
00069     hcf.seq_svc.waitInterpolation();
00070     hcf.seq_svc.setWrenches([0,0,0,0,0,0,
00071                              0,0,0,0,0,0,
00072                              0,0,0,0,0,0,
00073                              0,0,0,0,0,0,
00074                              0,0,0,0,0,0,
00075                              0,0,0,0,0,0,], 2.0);
00076     hcf.seq_svc.waitInterpolation();
00077     hcf.ic_svc.deleteImpedanceController("vrhsensor")
00078     print "test ImpedanceController for virtual force sensor => OK"
00079 
00080 if __name__ == '__main__':
00081     demo()