PA10.py
Go to the documentation of this file.
00001 import rtm
00002 from hrpsys import OpenHRP
00003 
00004 from rtm import *
00005 from OpenHRP import *
00006 
00007 import waitInput
00008 from waitInput import *
00009 
00010 import socket
00011 import time
00012 
00013 def connectComps():
00014     connectPorts(rh.port("q"), sh.port("currentQIn"))
00015     #
00016     connectPorts(seq.port("qRef"), sh.port("qIn"))
00017     #
00018     connectPorts(sh.port("qOut"),  [seq.port("qInit"), qRefPort])
00019     #
00020 
00021 def activateComps():
00022     rtm.serializeComponents([rh, seq, sh, log])
00023     rh.start()
00024     seq.start()
00025     sh.start()
00026     log.start()
00027 
00028 def createComps():
00029     global seq, seq_svc, sh, sh_svc, tk_svc, log, log_svc
00030 
00031     ms.load("SequencePlayer")
00032     seq = ms.create("SequencePlayer", "seq")
00033     seq_svc = narrow(seq.service("service0"), "SequencePlayerService")
00034 
00035     ms.load("StateHolder")
00036     sh = ms.create("StateHolder", "StateHolder0")
00037     sh_svc = narrow(sh.service("service0"), "StateHolderService")
00038     tk_svc = narrow(sh.service("service1"), "TimeKeeperService")
00039 
00040     ms.load("DataLogger")
00041     log = ms.create("DataLogger", "log")
00042     log_svc = narrow(log.service("service0"), "DataLoggerService")
00043     
00044 def init():
00045     global ms, rh, rh_svc, ep_svc, simulation_mode, qRefPort
00046 
00047     ms = rtm.findRTCmanager()
00048     rh = rtm.findRTC("RobotHardware0")
00049     if rh:
00050         rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
00051         ep_svc = narrow(rh.ec, "ExecutionProfileService")
00052         qRefPort = rh.port("qRef")
00053     else:
00054         rh = rtm.findRTC("PA10Controller(Robot)0")
00055         qRefPort = rtm.findRTC("HGcontroller0").port("qIn")
00056         simulation_mode = 1
00057     simulation_mode = 0
00058 
00059     ms = rtm.findRTCmanager()
00060 
00061     print "creating components"
00062     createComps()
00063       
00064     print "connecting components"
00065     connectComps()
00066 
00067     print "activating components"
00068     activateComps()
00069     print "initialized successfully"
00070 
00071 def goInitial(tm=3.0):
00072     seq_svc.setJointAngles([0]*9, tm)
00073 
00074 def goActual():
00075     sh_svc.goActual()
00076 
00077 def servoOn(joint="all"):
00078     rh_svc.servo(joint, SwitchStatus.SWITCH_ON)
00079 
00080 def powerOn(joint="all"):
00081     rh_svc.power(joint, SwitchStatus.SWITCH_ON)
00082 
00083 def powerOff(joint="all"):
00084     rh_svc.power(joint, SwitchStatus.SWITCH_OFF)
00085 
00086 def servoOff(joint="all"):
00087     rh_svc.servo(joint, SwitchStatus.SWITCH_OFF)
00088 
00089 def loadPattern(basename, tm=3.0):
00090     seq_svc.loadPattern(basename, tm)
00091 
00092 def setupLogger():
00093     log_svc.add("TimedDoubleSeq", "pos")
00094     log_svc.add("TimedLongSeq", "servoState")
00095     # log.owned_ecs[0].start()
00096     # log.start(log.owned_ecs[0])
00097 
00098 def demo():
00099     init()
00100     setupLogger()
00101     seq_svc.setJointAngles([0,0.7,0,2.5,0,1.5,0,0,0], 3.0)
00102     seq_svc.waitInterpolation()
00103     goInitial(3.0)
00104     seq_svc.waitInterpolation()
00105     log_svc.save("/tmp/demo")
00106 
00107 
00108 if __name__ == '__main__':
00109     initCORBA()
00110     demo()
00111 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55