def init(host='localhost'): global vs, vs_svc, cvp, cvp_svc global vs_head, vs_head_svc, cvp_head, cvp_head_svc sample.init(host) # for hand vs = rtm.findRTC("VideoStream0_hand") if vs != None: vs_svc = Img.CameraCaptureServiceHelper.narrow(vs.service('service0')) vs.start() cvp = rtm.findRTC("CvProcessor0_hand") cvp_svc = OpenHRP.CvProcessorServiceHelper.narrow(cvp.service('service0')) cvp.start() rtm.connectPorts(vs.port("MultiCameraImages"), cvp.port("MultiCameraImage")) vs_svc.take_one_frame() # for head vs_head = rtm.findRTC("VideoStream0_head") if vs_head != None: vs_head_svc = Img.CameraCaptureServiceHelper.narrow(vs_head.service('service0')) vs_head.start() cvp_head = rtm.findRTC("CvProcessor0_head") cvp_head_svc = OpenHRP.CvProcessorServiceHelper.narrow(cvp_head.service('service0')) cvp_head.start() rtm.connectPorts(vs_head.port("MultiCameraImages"), cvp_head.port("MultiCameraImage")) vs_head_svc.take_one_frame() time.sleep(1)
def total(): global vc, rg2, ri, je, jd, civ mgr = rtm.findRTCmanager() mgr.load("VideoCapture") mgr.load("RGB2Gray") mgr.load("ResizeImage") mgr.load("JpegEncoder") mgr.load("JpegDecoder") mgr.load("CameraImageViewer") vc = mgr.create("VideoCapture") r2g = mgr.create("RGB2Gray") ri = mgr.create("ResizeImage") je = mgr.create("JpegEncoder") jd = mgr.create("JpegDecoder") civ = mgr.create("CameraImageViewer") ri.setProperty("scale", "0.5") rtm.connectPorts(vc.port("CameraImage"), r2g.port("rgb")) rtm.connectPorts(r2g.port("gray"), ri.port("original")) rtm.connectPorts(ri.port("resized"), je.port("decoded")) rtm.connectPorts(je.port("encoded"), jd.port("encoded")) rtm.connectPorts(jd.port("decoded"), civ.port("imageIn")) rtm.serializeComponents([vc, r2g, ri, je, jd, civ]) vc.start() r2g.start() ri.start() je.start() jd.start() civ.start()
def connectMCControl(): connectPorts(rh.port("q"), mc.port("qIn")) connectPorts(rh.port("wristsensorA"), mc.port("@SENSOR_PORT_A@")) connectPorts(rh.port("wristsensorB"), mc.port("@SENSOR_PORT_B@")) if use_udp: connectPorts(mc_ctrl.port("qOut"), sh.port("qIn")) else: connectPorts(mc.port("qOut"), sh.port("qIn"))
def connectComps(): connectPorts(rh.port("q"), [sh.port("currentQIn")]) # for the kinematics mode if kinematics_mode == 1: connectPorts(sh.port("qOut"), rh.port("qIn")) connectPorts(sh.port("baseTformOut"), rh.port("baseTformIn")) # if not rh.port("baseTform"): connectPorts(sh.port("qOut"), servo.port("angleRef")) else: connectPorts(sh.port("baseTformOut"), rh.port("baseTform"))
def init(): global vs_svc, vs_port, md_port vs = rtm.findRTC('VideoStream0') vs.start() vs_svc = Img.CameraCaptureServiceHelper.narrow(vs.service('service0')) vs_port = vs.port('MultiCameraImages') # if you setup MultiDisp RTC (OpenVGR) md = rtm.findRTC('MultiDisp0') if md != None: md_port = md.port('images') rtm.connectPorts(vs_port, md_port) md.start()
def capture(): global vc, civ, ccs mgr = rtm.findRTCmanager() mgr.load("VideoCapture") mgr.load("CameraImageViewer") vc = mgr.create("VideoCapture") civ = mgr.create("CameraImageViewer") ccs = rtm.narrow(vc.service("service0"), "CameraCaptureService", "Img") rtm.connectPorts(vc.port("CameraImage"), civ.port("imageIn")) rtm.serializeComponents([vc, civ]) vc.start() civ.start()
def rgb2gray(): mgr = rtm.findRTCmanager() mgr.load("VideoCapture") mgr.load("RGB2Gray") mgr.load("CameraImageViewer") vc = mgr.create("VideoCapture") r2g = mgr.create("RGB2Gray") civ = mgr.create("CameraImageViewer") rtm.connectPorts(vc.port("CameraImage"), r2g.port("rgb")) rtm.connectPorts(r2g.port("gray"), civ.port("imageIn")) rtm.serializeComponents([vc, r2g, civ]) vc.start() r2g.start() civ.start()
def resize(): global vc, ri, civ mgr = rtm.findRTCmanager() mgr.load("VideoCapture") mgr.load("ResizeImage") mgr.load("CameraImageViewer") vc = mgr.create("VideoCapture") ri = mgr.create("ResizeImage") civ = mgr.create("CameraImageViewer") ri.setProperty("scale", "0.5") rtm.connectPorts(vc.port("CameraImage"), ri.port("original")) rtm.connectPorts(ri.port("resized"), civ.port("imageIn")) rtm.serializeComponents([vc, ri, civ]) vc.start() ri.start() civ.start()
def init(host='localhost', refSuffix=""): global vs, vs_svc, cvp, cvp_svc if robotHost != None: print 'robot host = '+robotHost java.lang.System.setProperty('NS_OPT', '-ORBInitRef NameService=corbaloc:iiop:'+robotHost+':2809/NameService') rtm.initCORBA() if refSuffix == None: refSuffix = '' elif refSuffix != '': refSuffix = '_'+refSuffix print 'suffix = '+refSuffix vs = rtm.findRTC("VideoStream0"+refSuffix) vs_svc = Img.CameraCaptureServiceHelper.narrow(vs.service('service0')) vs.start() cvp = rtm.findRTC("CvProcessor0"+refSuffix) cvp_svc = OpenHRP.CvProcessorServiceHelper.narrow(cvp.service('service0')) cvp.start() rtm.connectPorts(vs.port("MultiCameraImages"), cvp.port("MultiCameraImage"))
def jpeg(): global vc, je, jd, civ mgr = rtm.findRTCmanager() mgr.load("VideoCapture") mgr.load("JpegEncoder") mgr.load("JpegDecoder") mgr.load("CameraImageViewer") vc = mgr.create("VideoCapture") je = mgr.create("JpegEncoder") jd = mgr.create("JpegDecoder") civ = mgr.create("CameraImageViewer") rtm.connectPorts(vc.port("CameraImage"), je.port("decoded")) rtm.connectPorts(je.port("encoded"), jd.port("encoded")) rtm.connectPorts(jd.port("decoded"), civ.port("imageIn")) rtm.serializeComponents([vc, je, jd, civ]) vc.start() je.start() jd.start() civ.start() time.sleep(3) print "jpeg quality 95 -> 30" je.setProperty("quality", "30")
def connectComps(): rtm.connectPorts(rh.port("q"), sh.port("currentQIn")) rtm.connectPorts(sh.port("qOut"), seq.port("qInit")) rtm.connectPorts(sh.port("basePosOut"), seq.port("basePosInit")) rtm.connectPorts(sh.port("baseRpyOut"), seq.port("baseRpyInit")) rtm.connectPorts(seq.port("qRef"), sh.port("qIn")) rtm.connectPorts(seq.port("baseRpy"), sh.port("baseRpyIn")) if servo != None: rtm.connectPorts(sh.port("qOut"), servo.port("qRef")) else: rtm.connectPorts(sh.port("qOut"), rh.port("qIn"))
rtm.rootnc.rebind([CosNaming.NameComponent('MIDJAXON', 'rtc')], midjaxon.ref) rtm.rootnc.rebind([CosNaming.NameComponent('PDcontrollerMIDJAXON0', 'rtc')], rh.ref) rtm.rootnc.rebind([CosNaming.NameComponent('seq', 'rtc')], seq.ref) rtm.rootnc.rebind([CosNaming.NameComponent('sh', 'rtc')], sh.ref) rtm.rootnc.rebind([CosNaming.NameComponent('fk', 'rtc')], fk.ref) js = mgr.create('Joystick', 'js') js.setProperty('device', '/dev/input/js0') #js.setProperty('debugLevel', '1') midc = mgr2.create('MidJaxonController', 'midc') #midc.setProperty('debugLevel', '1') midc.setProperty('autobalance', '1') rtm.rootnc.rebind([CosNaming.NameComponent('midc', 'rtc')], midc.ref) rtm.connectPorts(sh.port("qOut"), rh.port("angleRef")) rtm.connectPorts(midjaxon.port("q"), [sh.port("currentQIn"), fk.port("q"), midc.port("q")]) # connection for actual joint angles rtm.connectPorts(sh.port("qOut"), fk.port("qRef")) rtm.connectPorts(seq.port("qRef"), midc.port("qUpstream")) rtm.connectPorts(midc.port("qRef"), sh.port("qIn")) #rtm.connectPorts(seq.port("qRef"), sh.port("qIn")) rtm.connectPorts(seq.port("zmpRef"), sh.port("zmpIn")) rtm.connectPorts(seq.port("optionalData"), sh.port("optionalDataIn")) rtm.connectPorts(sh.port("basePosOut"), [seq.port("basePosInit"), fk.port("basePosRef")]) rtm.connectPorts(sh.port("baseRpyOut"), [seq.port("baseRpyInit"), fk.port("baseRpyRef")]) rtm.connectPorts(sh.port("qOut"), seq.port("qInit")) rtm.connectPorts(sh.port("zmpOut"), seq.port("zmpRefInit"))
def connectComps(): rtm.connectPorts(rh.port("qCur"), sh.port("qCur")) rtm.connectPorts(sh.port("qOut"), servo.port("qRef")) rtm.connectPorts(rh.port("gyrometer"), kf.port("rate")) rtm.connectPorts(rh.port("gsensor"), kf.port("acc")) rtm.connectPorts(sh.port("qOut"), seq.port("qInit")) rtm.connectPorts(sh.port("basePosOut"), seq.port("basePosInit")) rtm.connectPorts(sh.port("baseRpyOut"), seq.port("baseRpyInit")) rtm.connectPorts(sh.port("zmpRefOut"), seq.port("zmpRefInit")) rtm.connectPorts(seq.port("qRef"), sh.port("qIn")) rtm.connectPorts(seq.port("basePos"), sh.port("basePosIn")) rtm.connectPorts(seq.port("baseRpy"), sh.port("baseRpyIn")) rtm.connectPorts(seq.port("zmpRef"), sh.port("zmpRefIn"))
def connectComps(): #rtm.connectPorts(user.port("refq"), servo.port("qRefIn")) #rtm.connectPorts(user.port("refq"), user.port("mc"))#tempepory #rtm.connectPorts(rh.port("q"), user.port("mc"))#tempepory rtm.connectPorts(rh.port("rfsensor"), user.port("rfsensor")) rtm.connectPorts(rh.port("lfsensor"), user.port("lfsensor")) rtm.connectPorts(rh.port("rhsensor"), user.port("rhsensor")) rtm.connectPorts(rh.port("lhsensor"), user.port("lhsensor")) rtm.connectPorts(user.port("light"), rh.port("light")) if joystick!=None: print "connect to gamepad" rtm.connectPorts(joystick.port("axes"),user.port("axes")) rtm.connectPorts(joystick.port("buttons"),user.port("buttons")) if kf != None: rtm.connectPorts(rh.port("gsensor"), kf.port("acc")) rtm.connectPorts(rh.port("gyrometer"), kf.port("rate")) if st!= None: rtm.connectPorts(kf.port("rpy"), st.port("rpy")) rtm.connectPorts(rh.port("rfsensor"), st.port("forceR")) rtm.connectPorts(rh.port("lfsensor"), st.port("forceL")) rtm.connectPorts(rh.port("q"), st.port("qCurrent")) rtm.connectPorts(user.port("refq"), st.port("qRef")) rtm.connectPorts(user.port("rzmp"), st.port("zmpRef")) rtm.connectPorts(user.port("basePosOut"), st.port("basePosIn")) rtm.connectPorts(user.port("baseRpyOut"), st.port("baseRpyIn")) rtm.connectPorts(user.port("contactStates"), st.port("contactStates")) rtm.connectPorts(user.port("localEEpos"), st.port("localEEpos")) rtm.connectPorts(st.port("q"), servo.port("qRefIn")) rtm.connectPorts(st.port("q"), user.port("mc"))
rtm.initCORBA() setupRTM() # Set NameServer to this host. viewer = findRTC("ImageViewer0") # Find for existing viewer. cpanel = findRTC("ControlPanel0") # Find for existing control panel. vision_pc = setupNameserver() # Set NameServer to control PC. robohw = findRTC("RobotHardware0") # Find for existing robot hardware. mgr = rtm.findRTCmanager(vision_pc) # Find RTC manager on vision PC. if mgr == None: raise Exception("Failed to find manager on {}.".format(vision_pc)) camera = createRTC(mgr, "V4L2CameraRTC", "v4l2") syncer = createRTC(mgr, "VideoSynchronizerRTC", "sync") syncer.setProperty("verbose", "1") rtm.connectPorts(camera.port("TimedCameraImage"), syncer.port("primary")) rtm.connectPorts(robohw.port("q"), syncer.port("secondary")) rtm.connectPorts(cpanel.port("Command"), camera.port("Command")) rtm.connectPorts(syncer.port("primaryOut"), viewer.port("images")) cpanel.start() viewer.start() robohw.start() syncer.start() camera.start() except Exception as err: print(err)
def connectComps(): rtm.connectPorts(rh.port("jointDatOut"), seq.port("jointDatIn")) rtm.connectPorts(rh.port("jointDatOut"), armR.port("jointDatIn")) rtm.connectPorts(rh.port("jointDatOut"), armL.port("jointDatIn")) rtm.connectPorts( seq.port("jointDatOut"), sa.port("jointDatIn0")) rtm.connectPorts( armR.port("jointDatOut"), sa.port("jointDatIn1")) rtm.connectPorts( armL.port("jointDatOut"), sa.port("jointDatIn2")) rtm.connectPorts( sa.port("jointDatOut"), grsp.port("jointDatIn")) rtm.connectPorts( grsp.port("jointDatOut"), gf.port("jointDatIn")) rtm.connectPorts( gf.port("jointDatOut"), ca.port("jointDatIn")) rtm.connectPorts( ca.port("jointDatOut"), rh.port("jointDatIn"))
def setupLogger(): global log_svc log_svc = OpenHRP.DataLoggerServiceHelper.narrow(log.service("service0")) log_svc.add("TimedDoubleSeq", "q") log_svc.add("TimedDoubleSeq", "qRefst") log_svc.add("TimedDoubleSeq", "qOutsh") log_svc.add("TimedDoubleSeq", "qRefsot") log_svc.add("TimedDoubleSeq", "zmpRefSeq") log_svc.add("TimedDoubleSeq", "zmpRefKpg") log_svc.add("TimedDoubleSeq", "basePos") log_svc.add("TimedDoubleSeq", "rpy") log_svc.add("TimedDoubleSeq", "forceL") log_svc.add("TimedDoubleSeq", "forceR") rtm.connectPorts(rh.port("q"), log.port("q")) rtm.connectPorts(rh.port("rfsensor"), log.port("forceR")) rtm.connectPorts(rh.port("lfsensor"), log.port("forceL")) rtm.connectPorts(sh.port("qOut"), log.port("qOutsh")) rtm.connectPorts(sot.port("qRef"), log.port("qRefsot")) if useStabilizerComp: rtm.connectPorts(st.port("qRefOut"), log.port("qRefst")) rtm.connectPorts(kf.port("rpy"), log.port("rpy")) rtm.connectPorts(seq.port("zmpRef"), log.port("zmpRefSeq")) rtm.connectPorts(sot.port("zmpRef"), log.port("zmpRefKpg")) rtm.connectPorts(sh.port("basePosOut"), log.port("basePos"))
import rtm rtm.nshost="localhost" rtm.nsport=2809 rtm.initCORBA() mgr = rtm.findRTCmanager() mgr.load("CameraImageLoader") loader = mgr.create("CameraImageLoader") mgr.load("ColorExtractor") extractor = mgr.create("ColorExtractor") extractor.setProperty("minPixels","10") extractor.setProperty("rgbRegion","100,0,0,255,100,100") mgr.load("CameraImageViewer") viewer = mgr.create("CameraImageViewer") #rtm.serializeComponents([loader, extractor, viewer]) rtm.connectPorts(loader.port("image"), extractor.port("original")) rtm.connectPorts(extractor.port("result"), viewer.port("imageIn")) viewer.start() extractor.start() loader.start()
def setupLogger(): log_svc.add("TimedJointData", "jointDatServo") log_svc.add("TimedJointData", "jointDatSeq") rtm.connectPorts( rh.port("jointDatOut"), log.port("jointDatServo")) rtm.connectPorts( seq.port("jointDatOut"), log.port("jointDatSeq"))
def setupLogger(): global log_svc log_svc = OpenHRP.DataLoggerServiceHelper.narrow(log.service("service0")) log_svc.add("TimedDoubleSeq", "q") log_svc.add("TimedDoubleSeq", "qRef") log_svc.add("TimedDoubleSeq", "zmpRefSeq") log_svc.add("TimedDoubleSeq", "zmpRefKpg") log_svc.add("TimedDoubleSeq", "basePos") log_svc.add("TimedDoubleSeq", "rpy") rtm.connectPorts(rh.port("q"), log.port("q")) rtm.connectPorts(st.port("qRefOut"), log.port("qRef")) rtm.connectPorts(seq.port("zmpRef"), log.port("zmpRefSeq")) rtm.connectPorts(kpg.port("zmpRef"), log.port("zmpRefKpg")) rtm.connectPorts(sh.port("basePosOut"), log.port("basePos")) rtm.connectPorts(kf.port("rpy"), log.port("rpy"))
def connectComps(): if useStabilizerComp: rtm.connectPorts(rh.port("q"), [sh.port("currentQIn"), st.port("q")]) else: rtm.connectPorts(rh.port("q"), sh.port("currentQIn")) if servo != None: if useStabilizerComp: rtm.connectPorts(rh.port("rfsensor"), st.port("forceR")) rtm.connectPorts(rh.port("lfsensor"), st.port("forceL")) rtm.connectPorts(rh.port("gyrometer"), [st.port("rate"), kf.port("rate")]) rtm.connectPorts(rh.port("gsensor"), [kf.port("acc"), st.port("acc")]) rtm.connectPorts(rh.port("rfsensor"), sot.port("forcesRF")) rtm.connectPorts(rh.port("lfsensor"), sot.port("forcesLF")) rtm.connectPorts(rh.port("rhsensor"), sot.port("forcesRH")) rtm.connectPorts(rh.port("lhsensor"), sot.port("forcesLH")) # rtm.connectPorts(seq.port("qRef"), sh.port("qIn")) rtm.connectPorts(seq.port("basePos"), sh.port("basePosIn")) rtm.connectPorts(seq.port("baseRpy"), sh.port("baseRpyIn")) if useStabilizerComp: rtm.connectPorts(seq.port("accRef"), kf.port("accRef")) rtm.connectPorts(seq.port("zmpRef"), st.port("zmpRef")) rtm.connectPorts(sot.port("accRef"), kf.port("accRef")) rtm.connectPorts(sot.port("zmpRef"), st.port("zmpRef")) rtm.connectPorts(sh.port("qOut"), st.port("qRefIn")) rtm.connectPorts(sh.port("baseRpyOut"), st.port("rpyRef")) rtm.connectPorts(sot.port("qRef"), sh.port("qIn")) rtm.connectPorts(sot.port("pRef"), sh.port("basePosIn")) rtm.connectPorts(sot.port("rpyRef"), sh.port("baseRpyIn")) # rtm.connectPorts(sh.port("qOut"), [seq.port("qInit"), sot.port("qInit")]) rtm.connectPorts(sh.port("basePosOut"), [seq.port("basePosInit"), sot.port("pInit")]) rtm.connectPorts(sh.port("baseRpyOut"), [seq.port("baseRpyInit"), sot.port("rpyInit")]) # if servo: # simulation_mode with dynamics or real robot mode if useStabilizerComp: rtm.connectPorts(st.port("qRefOut"), servo.port("qRef")) else: rtm.connectPorts(sh.port("qOut"), servo.port("qRef")) else: # simulation mode without dynamics rtm.connectPorts(sh.port("qOut"), rh.port("qIn"))
def connectComps(): connectPorts(rh.port("q"), [sh.port("currentQIn")]) connectPorts(rh.port("gyrometer"), [kf.port("rate")]) connectPorts(rh.port("gsensor"), [kf.port("acc")]) # for the kinematics mode if kinematics_mode == 1: connectPorts(sh.port("qOut"), rh.port("qIn")) connectPorts(sh.port("baseTformOut"), rh.port("baseTformIn")) # if not rh.port("baseTform"): # connectPorts(st.port("qRefOut"), servo.port("qRef")) connectPorts(sh.port("qOut"), servo.port("angleRef")) else: # connectPorts(st.port("qRefOut"), rh.port("qRef")) connectPorts(sh.port("baseTformOut"), rh.port("baseTform"))
def connectComps(): rtm.connectPorts(rh.port("q"), sh.port("qCur")) rtm.connectPorts(sh.port("qOut"), servo.port("qRef")) rtm.connectPorts(rh.port("gyrometer"), kf.port("rate")) rtm.connectPorts(rh.port("gsensor"), kf.port("acc")) rtm.connectPorts(sh.port("qOut"), seq.port("qInit")) rtm.connectPorts(sh.port("basePosOut"), seq.port("basePosInit")) rtm.connectPorts(sh.port("baseRpyOut"), seq.port("baseRpyInit")) rtm.connectPorts(sh.port("zmpRefOut"), seq.port("zmpRefInit")) rtm.connectPorts(seq.port("qRef"), sh.port("qIn")) rtm.connectPorts(seq.port("basePos"), sh.port("basePosIn")) rtm.connectPorts(seq.port("baseRpy"), sh.port("baseRpyIn")) rtm.connectPorts(seq.port("zmpRef"), sh.port("zmpRefIn")) rtm.connectPorts(rh.port("q"), servoC.port("qIn")) rtm.connectPorts(kf.port("rpy"), servoC.port("baseRpy")) rtm.connectPorts(sh.port("basePosOut"), servoC.port("basePos")) rtm.connectPorts(sh.port("qOut"), servoC.port("qRef")) rtm.connectPorts(sh.port("baseRpyOut"), servoC.port("baseRpyRef")) rtm.connectPorts(sh.port("zmpRefOut"), servoC.port("zmpRef")) rtm.connectPorts(rh.port("rfsensor"), servoC.port("rfsensor")) rtm.connectPorts(rh.port("lfsensor"), servoC.port("lfsensor")) rtm.connectPorts(rh.port("rhsensor"), servoC.port("rhsensor")) rtm.connectPorts(rh.port("lhsensor"), servoC.port("lhsensor")) if wpg != None: rtm.connectPorts(rh.port("rfsensor"), wpg.port("rfsensor")) rtm.connectPorts(rh.port("lfsensor"), wpg.port("lfsensor")) rtm.connectPorts(rh.port("rhsensor"), wpg.port("rhsensor")) rtm.connectPorts(rh.port("lhsensor"), wpg.port("lhsensor")) rtm.connectPorts(sh.port("qOut"), wpg.port("mc")) rtm.connectPorts(wpg.port("refq"), sh.port("qIn")) rtm.connectPorts(wpg.port("rzmp"), sh.port("zmpRefIn")) rtm.connectPorts(wpg.port("basePosOut"), sh.port("basePosIn")) rtm.connectPorts(wpg.port("baseRpyOut"), sh.port("baseRpyIn")) if st!= None: rtm.connectPorts(kf.port("rpy"), st.port("rpy")) rtm.connectPorts(rh.port("rfsensor"), st.port("forceR")) rtm.connectPorts(rh.port("lfsensor"), st.port("forceL")) rtm.connectPorts(rh.port("q"), st.port("qCurrent")) rtm.connectPorts(sh.port("qOut"), st.port("qRef")) rtm.connectPorts(sh.port("zmpRefOut"), st.port("zmpRef")) rtm.connectPorts(sh.port("basePosOut"), st.port("basePosIn")) rtm.connectPorts(sh.port("baseRpyOut"), st.port("baseRpyIn")) rtm.connectPorts(wpg.port("contactStates"), st.port("contactStates")) #rtm.connectPorts(wpg.port("localEEpos"), st.port("localEEpos")) rtm.connectPorts(st.port("q"), servo.port("qRef")) #rtm.connectPorts(st.port("q"), wpg.port("mc")) else: rtm.connectPorts(sh.port("qOut"), servo.port("qRef")) if camera != None: rtm.connectPorts(rh.port("rcamera"), camera.port("rcamera")) rtm.connectPorts(rh.port("lcamera"), camera.port("lcamera")) rtm.connectPorts(rh.port("rhcamera"), camera.port("rhcamera")) rtm.connectPorts(rh.port("lhcamera"), camera.port("lhcamera")) rtm.connectPorts(servoC.port("rcameraPose"), camera.port("rcameraPose")) rtm.connectPorts(servoC.port("lcameraPose"), camera.port("lcameraPose")) rtm.connectPorts(servoC.port("rhcameraPose"), camera.port("rhcameraPose")) rtm.connectPorts(servoC.port("lhcameraPose"), camera.port("lhcameraPose")) if stick != None: rtm.connectPorts(stick.port("axes"), wpg.port("axes")) rtm.connectPorts(stick.port("buttons"), wpg.port("buttons")) rtm.connectPorts(stick.port("axes"), pcl.port("axes")) rtm.connectPorts(stick.port("buttons"), pcl.port("buttons")) if pcl != None: rtm.connectPorts(rh.port("ranger"), pcl.port("ranger")) rtm.connectPorts(rh.port("q"), pcl.port("qCur")) rtm.connectPorts(kf.port("rpy"), pcl.port("baseRpyAct")) #rtm.connectPorts(sh.port("baseRpyOut"), pcl.port("baseRpyAct")) rtm.connectPorts(sh.port("baseRpyOut"), pcl.port("baseRpy")) rtm.connectPorts(sh.port("basePosOut"), pcl.port("basePos")) rtm.connectPorts(pcl.port("baseRpyOut"), wpg.port("baseRpyInit")) rtm.connectPorts(pcl.port("basePosOut"), wpg.port("basePosInit")) rtm.connectPorts(pcl.port("baseRpyOut"), sh.port("baseRpyIn")) rtm.connectPorts(pcl.port("basePosOut"), sh.port("basePosIn")) if arm != None: rtm.connectPorts(sh.port("qOut"), arm.port("qCur")) rtm.connectPorts(sh.port("basePosOut"), arm.port("basePos")) rtm.connectPorts(sh.port("baseRpyOut"), arm.port("baseRpy")) rtm.connectPorts(stick.port("axes"), arm.port("axes")) rtm.connectPorts(stick.port("buttons"), arm.port("buttons")) rtm.connectPorts(arm.port("qRef"), sh.port("qIn"))
def connectComps(): rtm.connectPorts(rh.port("q"), sh.port("qCur")) rtm.connectPorts(sh.port("qOut"), servo.port("qRef")) rtm.connectPorts(rh.port("gyrometer"), kf.port("rate")) rtm.connectPorts(rh.port("gsensor"), kf.port("acc")) rtm.connectPorts(sh.port("qOut"), seq.port("qInit")) rtm.connectPorts(sh.port("basePosOut"), seq.port("basePosInit")) rtm.connectPorts(sh.port("baseRpyOut"), seq.port("baseRpyInit")) rtm.connectPorts(sh.port("zmpRefOut"), seq.port("zmpRefInit")) rtm.connectPorts(seq.port("qRef"), sh.port("qIn")) rtm.connectPorts(seq.port("basePos"), sh.port("basePosIn")) rtm.connectPorts(seq.port("baseRpy"), sh.port("baseRpyIn")) rtm.connectPorts(seq.port("zmpRef"), sh.port("zmpRefIn")) rtm.connectPorts(rh.port("q"), servoC.port("qIn")) rtm.connectPorts(kf.port("rpy"), servoC.port("baseRpy")) rtm.connectPorts(sh.port("basePosOut"), servoC.port("basePos")) if camera != None: rtm.connectPorts(rh.port("rcamera"), camera.port("rcamera")) rtm.connectPorts(rh.port("lcamera"), camera.port("lcamera")) rtm.connectPorts(rh.port("rhcamera"), camera.port("rhcamera")) rtm.connectPorts(rh.port("lhcamera"), camera.port("lhcamera")) rtm.connectPorts(servoC.port("rcameraPose"), camera.port("rcameraPose")) rtm.connectPorts(servoC.port("lcameraPose"), camera.port("lcameraPose")) rtm.connectPorts(servoC.port("rhcameraPose"), camera.port("rhcameraPose")) rtm.connectPorts(servoC.port("lhcameraPose"), camera.port("lhcameraPose"))
import rtm rtm.nsport = 2809 rtm.initCORBA() mgr = rtm.findRTCmanager() mgr.load("PCDLoader") mgr.load("PointCloudViewer") pcl = mgr.create("PCDLoader") pcv = mgr.create("PointCloudViewer") rtm.connectPorts(pcl.port("cloud"), pcv.port("cloud")) pcl.setProperty("fields", "XYZRGB") pcl.start() pcv.start()
import rtm import commands mgr = rtm.findRTCmanager() mgr.load("VirtualCamera") mgr.load("OccupancyGridMap3D") mgr.load("OGMap3DViewer") vc = mgr.create("VirtualCamera") ogmap = mgr.create("OccupancyGridMap3D") viewer = mgr.create("OGMap3DViewer") openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1") project = "file://" + openhrp_dir + "/share/OpenHRP-3.1/sample/project/SampleRobot_inHouse.xml" vc.setProperty("project", project) vc.setProperty("camera", "Robot:VISION_SENSOR1") vc.setProperty("generateRange", "0") vc.setProperty("generatePointCloud", "1") vc.setProperty("generatePointCloudStep", "5") ogmap.setProperty("resolution", "0.05") rtm.connectPorts(vc.port("cloud"), ogmap.port("cloud")) rtm.connectPorts(vc.port("poseSensor"), ogmap.port("pose")) rtm.connectPorts(ogmap.port("OGMap3DService"), viewer.port("OGMap3DService")) rtm.serializeComponents([vc, ogmap]) vc.start() ogmap.start() viewer.start()
# -*- coding: utf-8 -*- import rtm global mgr, rtc1, rtc2 # Managerへの参照を取得します mgr = rtm.findRTCmanager() # NullComponent.soをロードします mgr.load("NullComponent") # NullComponentのインスタンスを2つ作ります rtc1 = mgr.create("NullComponent", "np1") rtc2 = mgr.create("NullComponent", "np2") # 2つのコンポーネントを接続します rtm.connectPorts(rtc1.port("dataOut"), rtc2.port("dataIn")) rtcs = [rtc1, rtc2] # 2つのコンポーネントが同期実行されるように設定します rtm.serializeComponents(rtcs) # 2つのコンポーネントをアクティベートします rtc1.start() rtc2.start()
def connectMCControl(): connectPorts(rh.port("q"), mc.port("qIn")) connectPorts(rh.port("tauOut"), mc.port("taucIn")) connectPorts(rh.port("gyrometer"), mc.port("rateIn")) connectPorts(rh.port("gsensor"), mc.port("accIn")) connectPorts(rh.port("rfsensor"), mc.port("RightFootForceSensor")) connectPorts(rh.port("lfsensor"), mc.port("LeftFootForceSensor")) connectPorts(rh.port("rhsensor"), mc.port("RightHandForceSensor")) connectPorts(rh.port("lhsensor"), mc.port("LeftHandForceSensor")) connectPorts(sh.port("basePosOut"), mc.port("pIn")) connectPorts(kf.port("rpy"), mc.port("rpyIn")) connectPorts(mc.port("qOut"), sh.port("qIn")) connectPorts(rh.port("waistAbsTransform"), mc.port("basePoseIn")) connectPorts(rh.port("waistAbsVelocity"), mc.port("baseVelIn")) connectPorts(rh.port("waistAbsAcceleration"), mc.port("baseAccIn"))
import rtm rtm.nsport=2809 rtm.initCORBA() mgr = rtm.findRTCmanager() mgr.load("PCDLoader") mgr.load("PointCloudViewer") pcl = mgr.create("PCDLoader") pcv = mgr.create("PointCloudViewer") rtm.connectPorts(pcl.port("cloud"), pcv.port("cloud")) pcl.setProperty("fields", "XYZRGB") pcl.start() pcv.start()
# # test script for Simulator and Viewer components # # start SimulatorComp and ViewerComp before running this script # import rtm import commands openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1") project = "file://" + openhrp_dir + "/share/OpenHRP-3.1/sample/project/SamplePD.xml" sim = rtm.findRTC("Simulator0") print "sim:", sim sim.setProperty("project", project) vwr = rtm.findRTC("Viewer0") print "vwr:", vwr vwr.setProperty("project", project) rtm.connectPorts(sim.port("state"), vwr.port("state")) vwr.start() sim.start()
def connectComps(): if useStabilizerComp: rtm.connectPorts(rh.port("q"), [sh.port("currentQIn"), st.port("q")]) else: rtm.connectPorts(rh.port("q"), sh.port("currentQIn")) if servo != None: if useStabilizerComp: rtm.connectPorts(rh.port("rfsensor"), st.port("forceR")) rtm.connectPorts(rh.port("lfsensor"), st.port("forceL")) rtm.connectPorts( rh.port("gyrometer"), [st.port("rate"), kf.port("rate")]) rtm.connectPorts(rh.port("gsensor"), [kf.port("acc"), st.port("acc")]) rtm.connectPorts(rh.port("rfsensor"), sot.port("forcesRF")) rtm.connectPorts(rh.port("lfsensor"), sot.port("forcesLF")) rtm.connectPorts(rh.port("rhsensor"), sot.port("forcesRH")) rtm.connectPorts(rh.port("lhsensor"), sot.port("forcesLH")) # rtm.connectPorts(seq.port("qRef"), sh.port("qIn")) rtm.connectPorts(seq.port("basePos"), sh.port("basePosIn")) rtm.connectPorts(seq.port("baseRpy"), sh.port("baseRpyIn")) if useStabilizerComp: rtm.connectPorts(seq.port("accRef"), kf.port("accRef")) rtm.connectPorts(seq.port("zmpRef"), st.port("zmpRef")) rtm.connectPorts(sot.port("accRef"), kf.port("accRef")) rtm.connectPorts(sot.port("zmpRef"), st.port("zmpRef")) rtm.connectPorts(sh.port("qOut"), st.port("qRefIn")) rtm.connectPorts(sh.port("baseRpyOut"), st.port("rpyRef")) rtm.connectPorts(sot.port("qRef"), sh.port("qIn")) rtm.connectPorts(sot.port("pRef"), sh.port("basePosIn")) rtm.connectPorts(sot.port("rpyRef"), sh.port("baseRpyIn")) # rtm.connectPorts(sh.port("qOut"), [seq.port("qInit"), sot.port("qInit")]) rtm.connectPorts( sh.port("basePosOut"), [seq.port("basePosInit"), sot.port("pInit")]) rtm.connectPorts( sh.port("baseRpyOut"), [seq.port("baseRpyInit"), sot.port("rpyInit")]) # if servo: # simulation_mode with dynamics or real robot mode if useStabilizerComp: rtm.connectPorts(st.port("qRefOut"), servo.port("qRef")) else: rtm.connectPorts(sh.port("qOut"), servo.port("qRef")) else: # simulation mode without dynamics rtm.connectPorts(sh.port("qOut"), rh.port("qIn"))