samplerobot_virtual_force_sensor.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 try:
4  from hrpsys.hrpsys_config import *
5  import OpenHRP
6 except:
7  print "import without hrpsys"
8  import rtm
9  from rtm import *
10  from OpenHRP import *
11  import waitInput
12  from waitInput import *
13  import socket
14  import time
15 
16 def init ():
17  global hcf
18  hcf = HrpsysConfigurator()
19  hcf.getRTCList = hcf.getRTCListUnstable
20  hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
21 
22 def demo ():
23  init()
24  # set initial pose from sample/controller/SampleController/etc/Sample.pos
25  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]
26 
27  # 1. Check wrench data ports existence (real force sensor + virtual force sensor)
28  hcf.seq_svc.setJointAngles(initial_pose, 2.0)
29  hcf.seq_svc.waitInterpolation()
30  fsensor_names = hcf.getForceSensorNames()
31  # vs check
32  port_names = fsensor_names
33  if all(map (lambda x : hcf.vs.port(x), fsensor_names)):
34  print hcf.vs.name(), "ports are OK (", port_names, ")"
35  # seq check
36  port_names = map (lambda x : x+"Ref", fsensor_names)
37  if all(map (lambda x : hcf.seq.port(x), port_names)):
38  print hcf.seq.name(), "ports are OK (", port_names, ")"
39  # sh check
40  port_names = map (lambda x : x+"Out", fsensor_names)
41  if all(map (lambda x : hcf.sh.port(x), port_names)):
42  print hcf.sh.name(), "ports are OK (", port_names, ")"
43  port_names = map (lambda x : x+"In", fsensor_names)
44  if all(map (lambda x : hcf.sh.port(x), port_names)):
45  print hcf.sh.name(), "ports are OK (", port_names, ")"
46  # ic check
47  port_names = map (lambda x : "ref_"+x+"In", fsensor_names)
48  if all(map (lambda x : hcf.ic.port(x), port_names)):
49  print hcf.ic.name(), "ports are OK (", port_names, ")"
50  # abc check
51  port_names = map (lambda x : "ref_"+x+"In", fsensor_names)
52  if all(map (lambda x : hcf.ic.port(x), port_names)):
53  print hcf.ic.name(), "ports are OK (", port_names, ")"
54 
55  # 2. Test impedance controller
56  ret1=hcf.ic_svc.getImpedanceControllerParam("vrhsensor")
57  ret1[1].base_name="CHEST"
58  ret1[1].target_name="RARM_WRIST_P"
59  ret1[1].K_r=1.0
60  ret1[1].D_r=2.0
61  ret1[1].M_r=0.2
62  ret2=hcf.ic_svc.setImpedanceControllerParam("vrhsensor", ret1[1])
63  hcf.seq_svc.setWrenches([0,0,0,0,0,0,
64  0,0,0,0,0,0,
65  0,0,0,0,0,0,
66  0,0,0,0,0,0,
67  0,0,0,0,0,0,
68  10,10,-10,0,0,0,], 2.0);
69  hcf.seq_svc.waitInterpolation();
70  hcf.seq_svc.setWrenches([0,0,0,0,0,0,
71  0,0,0,0,0,0,
72  0,0,0,0,0,0,
73  0,0,0,0,0,0,
74  0,0,0,0,0,0,
75  0,0,0,0,0,0,], 2.0);
76  hcf.seq_svc.waitInterpolation();
77  hcf.ic_svc.deleteImpedanceController("vrhsensor")
78  print "test ImpedanceController for virtual force sensor => OK"
79 
80 if __name__ == '__main__':
81  demo()


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51