PA10.py
Go to the documentation of this file.
1 import rtm
2 from hrpsys import OpenHRP
3 
4 from rtm import *
5 from OpenHRP import *
6 
7 import waitInput
8 from waitInput import *
9 
10 import socket
11 import time
12 
14  connectPorts(rh.port("q"), sh.port("currentQIn"))
15  #
16  connectPorts(seq.port("qRef"), sh.port("qIn"))
17  #
18  connectPorts(sh.port("qOut"), [seq.port("qInit"), qRefPort])
19  #
20 
22  rtm.serializeComponents([rh, seq, sh, log])
23  rh.start()
24  seq.start()
25  sh.start()
26  log.start()
27 
29  global seq, seq_svc, sh, sh_svc, tk_svc, log, log_svc
30 
31  ms.load("SequencePlayer")
32  seq = ms.create("SequencePlayer", "seq")
33  seq_svc = narrow(seq.service("service0"), "SequencePlayerService")
34 
35  ms.load("StateHolder")
36  sh = ms.create("StateHolder", "StateHolder0")
37  sh_svc = narrow(sh.service("service0"), "StateHolderService")
38  tk_svc = narrow(sh.service("service1"), "TimeKeeperService")
39 
40  ms.load("DataLogger")
41  log = ms.create("DataLogger", "log")
42  log_svc = narrow(log.service("service0"), "DataLoggerService")
43 
44 def init():
45  global ms, rh, rh_svc, ep_svc, simulation_mode, qRefPort
46 
47  ms = rtm.findRTCmanager()
48  rh = rtm.findRTC("RobotHardware0")
49  if rh:
50  rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
51  ep_svc = narrow(rh.ec, "ExecutionProfileService")
52  qRefPort = rh.port("qRef")
53  else:
54  rh = rtm.findRTC("PA10Controller(Robot)0")
55  qRefPort = rtm.findRTC("HGcontroller0").port("qIn")
56  simulation_mode = 1
57  simulation_mode = 0
58 
59  ms = rtm.findRTCmanager()
60 
61  print "creating components"
62  createComps()
63 
64  print "connecting components"
65  connectComps()
66 
67  print "activating components"
69  print "initialized successfully"
70 
71 def goInitial(tm=3.0):
72  seq_svc.setJointAngles([0]*9, tm)
73 
74 def goActual():
75  sh_svc.goActual()
76 
77 def servoOn(joint="all"):
78  rh_svc.servo(joint, SwitchStatus.SWITCH_ON)
79 
80 def powerOn(joint="all"):
81  rh_svc.power(joint, SwitchStatus.SWITCH_ON)
82 
83 def powerOff(joint="all"):
84  rh_svc.power(joint, SwitchStatus.SWITCH_OFF)
85 
86 def servoOff(joint="all"):
87  rh_svc.servo(joint, SwitchStatus.SWITCH_OFF)
88 
89 def loadPattern(basename, tm=3.0):
90  seq_svc.loadPattern(basename, tm)
91 
93  log_svc.add("TimedDoubleSeq", "pos")
94  log_svc.add("TimedLongSeq", "servoState")
95  # log.owned_ecs[0].start()
96  # log.start(log.owned_ecs[0])
97 
98 def demo():
99  init()
100  setupLogger()
101  seq_svc.setJointAngles([0,0.7,0,2.5,0,1.5,0,0,0], 3.0)
102  seq_svc.waitInterpolation()
103  goInitial(3.0)
104  seq_svc.waitInterpolation()
105  log_svc.save("/tmp/demo")
106 
107 
108 if __name__ == '__main__':
109  initCORBA()
110  demo()
111 
def findRTC(name, rnc=None)
get RT component
Definition: jython/rtm.py:341
def powerOn(joint="all")
Definition: PA10.py:80
def servoOn(joint="all")
Definition: PA10.py:77
def narrow(ior, klass, package="OpenHRP")
narrow ior
Definition: jython/rtm.py:709
def setupLogger()
Definition: PA10.py:92
def init()
Definition: PA10.py:44
def serializeComponents(rtcs, stopEC=True)
set up execution context of the first RTC so that RTCs are executed sequentially
Definition: jython/rtm.py:373
def servoOff(joint="all")
Definition: PA10.py:86
def findRTCmanager(hostname=None, rnc=None)
get RTCmanager
Definition: jython/rtm.py:321
def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength=1, rate=1000)
connect ports
Definition: jython/rtm.py:433
def loadPattern(basename, tm=3.0)
Definition: PA10.py:89
def powerOff(joint="all")
Definition: PA10.py:83
def activateComps()
Definition: PA10.py:21
def goInitial(tm=3.0)
Definition: PA10.py:71
def connectComps()
Definition: PA10.py:13
def goActual()
Definition: PA10.py:74
def createComps()
Definition: PA10.py:28
def initCORBA()
initialize ORB
Definition: jython/rtm.py:272
def demo()
Definition: PA10.py:98


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20