예제 #1
0
def createComps(hostname=socket.gethostname()):
    global ms, adm_svc, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo, kpg, kpg_svc, st, kf, log, simulation_mode
    ms = rtm.findRTCmanager(hostname)
    rh = rtm.findRTC("RobotHardware0")
    if rh != None:
        simulation_mode = 0
        rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
        servo = rh
        adm = rtm.findRTC("SystemAdmin0")
        if adm != None:
          adm.start()
          adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
    else:
        simulation_mode = 1
        rh = rtm.findRTC(bodyinfo.modelName+"Controller(Robot)0")
        servo = rtm.findRTC("PDservo0")
    seq = initRTC("SequencePlayer", "seq")
    seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
    sh  = initRTC("StateHolder", "StateHolder0")
    sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))

    kf  = initRTC("KalmanFilter", "kf")
    kpg = initRTC("WalkGenerator", "kpg")
    kpg_svc = OpenHRP.WalkGeneratorServiceHelper.narrow(kpg.service("service0"))
    st  = initRTC("Stabilizer", "st")
    st.setConfiguration(bodyinfo.hstConfig)

    log = initRTC("DataLogger", "log")

    return [rh, seq, kf, kpg, sh, st, log]
예제 #2
0
def demoDualarmPush (auto_detecion = True):
    print >> sys.stderr, "3. Dual-arm push demo."
    print >> sys.stderr, "  Move to"
    hcf.abc_svc.goPos(-0.45,0,0);
    hcf.abc_svc.waitFootSteps();
    hcf.abc_svc.goPos(0,0,(math.degrees(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.orientation.y-rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y)));
    hcf.abc_svc.waitFootSteps();
    print >> sys.stderr, "  Reaching"
    #hcf.abc_svc.goPos(0.25, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
    hcf.abc_svc.goPos(0.1, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
    hcf.abc_svc.waitFootSteps();
    hcf.seq_svc.setJointAngles(dualarm_via_pose, 1.0)
    hcf.waitInterpolation()
    hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
    hcf.waitInterpolation()
    print >> sys.stderr, "  Increase operational force"
    if auto_detecion:
        objectTurnaroundDetection(axis=[-1,0,0],max_ref_force=100, max_time=2)
    else:
        hcf.seq_svc.setWrenches([0]*6+[0]*6+[-40,0,0,0,0,0]*2, 2.0)
        hcf.waitInterpolation()
    print >> sys.stderr, "  Push forward"
    abcp=hcf.abc_svc.getAutoBalancerParam()[1]
    abcp.is_hand_fix_mode = True
    hcf.abc_svc.setAutoBalancerParam(abcp)
    hcf.abc_svc.goPos(0.5,0,0)
    hcf.abc_svc.waitFootSteps();
    hcf.seq_svc.setWrenches([0]*24, 2.0)
    hcf.waitInterpolation()
예제 #3
0
def init(hostname=socket.gethostname()):
    global ms, simulation_mode, kinematics_mode

    ms = rtm.findRTCmanager()

    rh = rtm.findRTC("RobotHardware0")
    if rh is not None:
        rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
        servo = rh
        ep_svc = narrow(rh.ec, "ExecutionProfileService")
        simulation_mode = 0
    else:
        rh = rtm.findRTC("JVRC1Controller(Robot)0")
        servo = rtm.findRTC("PDcontroller0")
        simulation_mode = 1
        if rh.port("baseTformIn"):
            kinematics_mode = 1
        else:
            kinematics_mode = 0

    print("creating components")
    createComps()

    print("connecting components")
    connectComps()

    print("activating components")
    activateComps()

    print("initialized successfully")
def demoDualarmPush (auto_detecion = True):
    print >> sys.stderr, "3. Dual-arm push demo."
    print >> sys.stderr, "  Move to"
    hcf.abc_svc.goPos(-0.45,0,0);
    hcf.abc_svc.waitFootSteps();
    hcf.abc_svc.goPos(0,0,(math.degrees(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.orientation.y-rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y)));
    hcf.abc_svc.waitFootSteps();
    print >> sys.stderr, "  Reaching"
    #hcf.abc_svc.goPos(0.25, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
    hcf.abc_svc.goPos(0.1, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
    hcf.abc_svc.waitFootSteps();
    hcf.seq_svc.setJointAngles(dualarm_via_pose, 1.0)
    hcf.waitInterpolation()
    hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
    hcf.waitInterpolation()
    print >> sys.stderr, "  Increase operational force"
    if auto_detecion:
        objectTurnaroundDetection(axis=[-1,0,0],max_ref_force=100, max_time=2)
    else:
        hcf.seq_svc.setWrenches([0]*6+[0]*6+[-40,0,0,0,0,0]*2, 2.0)
        hcf.waitInterpolation()
    print >> sys.stderr, "  Push forward"
    hcf.abc_svc.goPos(0.5,0,0)
    hcf.abc_svc.waitFootSteps();
    hcf.seq_svc.setWrenches([0]*24, 2.0)
    hcf.waitInterpolation()
예제 #5
0
def createComps():
    global ms, rh, rh_svc, sh, sh_svc, tk_svc, st, kf, log, log_svc, servo
    global ep_svc, mc, mc_svc

    rh = rtm.findRTC("RobotHardware0")
    if rh is not None:
        rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
        servo = rh
        ep_svc = narrow(rh.ec, "ExecutionProfileService")
    else:
        rh = rtm.findRTC("JVRC1Controller(Robot)0")
        servo = rtm.findRTC("PDcontroller0")

    ms = rtm.findRTCmanager()

    ms.load("KalmanFilter")
    kf = ms.create("KalmanFilter")

    ms.load("StateHolder")
    sh = ms.create("StateHolder")
    sh_svc = narrow(sh.service("service0"), "StateHolderService")
    tk_svc = narrow(sh.service("service1"), "TimeKeeperService")

    ms.load("DataLogger")
    log = ms.create("DataLogger")
    log_svc = narrow(log.service("service0"), "DataLoggerService")

    ms.load("MCControl")
    mc = ms.create("MCControl")
예제 #6
0
def createComps():
    global ms, rh, rh_svc, sh, sh_svc, tk_svc, st, kf, log, log_svc, servo
    global ep_svc, mc, mc_ctrl

    rh = rtm.findRTC("pandaController(Robot)0")
    servo = rtm.findRTC("PDcontroller0")

    ms = rtm.findRTCmanager()

    ms.load("KalmanFilter")
    kf = ms.create("KalmanFilter")

    ms.load("StateHolder")
    sh = ms.create("StateHolder")
    sh_svc = narrow(sh.service("service0"), "StateHolderService")
    tk_svc = narrow(sh.service("service1"), "TimeKeeperService")

    ms.load("DataLogger")
    log = ms.create("DataLogger")
    log_svc = narrow(log.service("service0"), "DataLoggerService")

    if use_udp:
      ms.load("MCUDPSensors")
      mc = ms.create("MCUDPSensors")

      ms.load("MCUDPControl")
      mc_ctrl = ms.create("MCUDPControl")
    else:
      ms.load("MCControl")
      mc = ms.create("MCControl")
예제 #7
0
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)
예제 #8
0
def init():
    global ms, rh, rh_svc, ep_svc, simulation_mode, qRefPort

    ms = rtm.findRTCmanager()
    rh = rtm.findRTC("RobotHardware0")
    if rh:
        rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
        ep_svc = narrow(rh.ec, "ExecutionProfileService")
        qRefPort = rh.port("qRef")
    else:
        rh = rtm.findRTC("PA10Controller(Robot)0")
        qRefPort = rtm.findRTC("HGcontroller0").port("qIn")
        simulation_mode = 1
    simulation_mode = 0

    ms = rtm.findRTCmanager()

    print "creating components"
    createComps()

    print "connecting components"
    connectComps()

    print "activating components"
    activateComps()
    print "initialized successfully"
예제 #9
0
def init():
    global ms, rh, rh_svc, ep_svc, simulation_mode, qRefPort

    ms = rtm.findRTCmanager()
    rh = rtm.findRTC("RobotHardware0")
    if rh:
        rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
        ep_svc = narrow(rh.ec, "ExecutionProfileService")
        qRefPort = rh.port("qRef")
    else:
        rh = rtm.findRTC("PA10Controller(Robot)0")
        qRefPort = rtm.findRTC("HGcontroller0").port("qIn")
        simulation_mode = 1
    simulation_mode = 0

    ms = rtm.findRTCmanager()

    print "creating components"
    createComps()
      
    print "connecting components"
    connectComps()

    print "activating components"
    activateComps()
    print "initialized successfully"
예제 #10
0
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()
예제 #11
0
def createComps(hostname=socket.gethostname()):
    global ms, user, user_svc, log, rh, servo, joystick, kf, st, st_svc
    ms = rtm.findRTCmanager(hostname)
    rh = rtm.findRTC("HRP2A")
    if ms==None:
        print "no ms"
    else:
        user = initRTC("sony", "wpg")
        joystick= initRTC("GamepadRTC","joystick")
        kf= initRTC("KalmanFilter","kf")
        st= initRTC("Stabilizer","st")
    #log = initRTC("DataLogger", "log")
   
    servo= rtm.findRTC("JojoPDservo0")

    #user = rtm.findRTC("sony0")
    #joystick= rtm.findRTC("GamepadRTC0")
    #kf=rtm.findRTC("KalmanFilter0")
    #st=rtm.findRTC("Stabilizer0")
    
    if servo==None:
        print "no servo component"
        return

    if rh==None:
        print "no robot component"
        return

    if user==None:
        print "no user component"
        return

    user_svc = OpenHRP.sonyServiceHelper.narrow(user.service("service0"))
    if user_svc==None:
        print "no svc"

    st_svc = OpenHRP.StabilizerServiceHelper.narrow(st.service("service0"))
    if st_svc==None:
        print "no st svc"

    if joystick==None:
        print "no joystick component"

    rtcs=[rh, joystick, kf, user, st]
    

    #user.start()
    #joystick.start()

    #return [rh, user]
    return rtcs
예제 #12
0
def rtc_init():
    global ms, co, co_svc, root_link_name, root_link_offset

    initCORBA()
    ms = rtm.findRTCmanager(rtm.nshost)
    while ms == None:
        time.sleep(1)
        ms = rtm.findRTCmanager(rtm.nshost)
        rospy.loginfo("[collision_state.py] wait for RTCmanager : ", ms)

    co = rtm.findRTC(rospy.get_param('~comp_name', 'co'))
    if co == None:
        rospy.logerr("Could not found CollisionDetector, exiting...")
        exit(1)
    co_svc = narrow(co.service("service0"), "CollisionDetectorService")

    if modelfile:
        #import CosNaming
        obj = rtm.rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
        mdlldr = obj._narrow(ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
        rospy.loginfo("  bodyinfo URL = file://" + modelfile)
        body_info = mdlldr.getBodyInfo("file://" + modelfile)
        root_link_name = body_info._get_links()[0].name

        root_link_offset = inverse_matrix(
            concatenate_matrices(
                translation_matrix(body_info._get_links()[0].translation),
                rotation_matrix(body_info._get_links()[0].rotation[3],
                                body_info._get_links()[0].rotation[0:3])))
    else:
        root_link_name = "WAIST"
        root_link_offset = identity_matrix()

        rospy.loginfo("ssetup collision_state with " + root_link_name + " " +
                      root_link_offset)
예제 #13
0
def createComps(hostname=socket.gethostname()):
    global ms, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo
    ms = rtm.findRTCmanager(hostname)

    rh = rtm.findRTC("RobotHardware0")
    rh_svc = None
    if rh != None:
        rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
        servo = rh
    else:
        rh = rtm.findRTC(bodyinfo.modelName+"Controller(Robot)0")
        servo = rtm.findRTC("PDservo0")
    seq = initRTC("SequencePlayer", "seq")
    sh  = initRTC("StateHolder", "StateHolder0")
    seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
    sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))
    return [rh, seq, sh]
예제 #14
0
def createComps(hostname=socket.gethostname()):
    global ms, adm_svc, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo, kpg, kpg_svc, st, kf, log, simulation_mode, sot
    ms = rtm.findRTCmanager(hostname)
    rh = rtm.findRTC("RobotHardware0")
    if rh != None:
        simulation_mode = 0
        rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(
            rh.service("service0"))
        servo = rh
        adm = rtm.findRTC("SystemAdmin0")
        if adm != None:
            adm.start()
            adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(
                adm.service("service0"))
    else:
        simulation_mode = 1
        rh = rtm.findRTC(bodyinfo.modelName + "Controller(Robot)0")
        servo = rtm.findRTC("PDservo0")
    seq = initRTC("SequencePlayer", "seq")
    seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(
        seq.service("service0"))
    sh = initRTC("StateHolder", "StateHolder0")
    sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))

    if useStabilizerComp:
        kf = initRTC("KalmanFilter", "kf")
    kpg = initRTC("WalkGenerator", "kpg")
    kpg_svc = OpenHRP.WalkGeneratorServiceHelper.narrow(
        kpg.service("service0"))
    if useStabilizerComp:
        st = initRTC("Stabilizer", "st")
        st.setConfiguration(bodyinfo.hstConfig)

    log = initRTC("DataLogger", "log")

    print "Creating SoT"
    sot = initRTC("RtcStackOfTasks", "sot")
    print "Set configuration of SoT"
    sot.setConfiguration(sotinfo.sotConfig)
    if useStabilizerComp:
        return [rh, seq, kf, kpg, sh, st, log, sot]
    else:
        return [rh, seq, kpg, sh, log, sot]
예제 #15
0
 def findComp(self, compName, instanceName, max_timeout_count = 10):
     timeout_count = 0;
     comp = rtm.findRTC(instanceName)
     while comp == None and timeout_count < max_timeout_count:
         comp = rtm.findRTC(instanceName)
         if comp != None:
             break
         print self.configurator_name, " find Comp wait for", instanceName
         time.sleep(1)
         timeout_count += 1
     print self.configurator_name, " find Comp    : ", instanceName, " = ", comp
     if comp == None:
         print self.configurator_name, " Cannot find component: " + instanceName + " (" + compName +")"
         return [None, None]
     if comp.service("service0"):
         comp_svc = narrow(comp.service("service0"), compName+"Service")
         print self.configurator_name, " find CompSvc : ", instanceName + "_svc = ", comp_svc
         return [comp, comp_svc]
     else:
         return [comp, None]
예제 #16
0
 def findComp(self, compName, instanceName, max_timeout_count = 10):
     timeout_count = 0;
     comp = rtm.findRTC(instanceName)
     while comp == None and timeout_count < max_timeout_count:
         comp = rtm.findRTC(instanceName)
         if comp != None:
             break
         print self.configurator_name, " find Comp wait for", instanceName
         time.sleep(1)
         timeout_count += 1
     print self.configurator_name, " find Comp    : ", instanceName, " = ", comp
     if comp == None:
         print self.configurator_name, " Cannot find component: " + instanceName + " (" + compName +")"
         return [None, None]
     if comp.service("service0"):
         comp_svc = narrow(comp.service("service0"), compName+"Service")
         print self.configurator_name, " find CompSvc : ", instanceName + "_svc = ", comp_svc
         return [comp, comp_svc]
     else:
         return [comp, None]
예제 #17
0
파일: user.py 프로젝트: hsnuhayato/tsml
def createCompsConsole(nshC, rncC):
    global msC, rhC, servoC, rtcListC
    msC = rtm.findRTCmanager(nshC, rncC)
    rhC = rtm.findRTC("JVRC-State", rncC)
    servoC = rtm.findRTC("creekRobotState0", rncC)

    if rhC == None:
        rtcListC = rtcListR
        servoC = initRTC("creekRobotState", "creekRobotState0", msC)
    else:
        rtcListC = [rhC]
        #rtcListC = rtcListR
        if servoC == None:
            return

    global camera, camera_svc
    camera = initRTC("creekCameraViewer", "camera", msC)
    if camera != None:
        camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
        rtcListC.append(camera)
예제 #18
0
 def check_initCORBA(self, nshost, nsport=2809):
     try:
         ms = rh = None
         rtm.nshost = nshost
         rtm.nsport = nsport
         rtm.initCORBA()
         ms = rtm.findRTCmanager()
         rh = rtm.findRTC("RobotHardware0")
         self.assertTrue(ms and rh)
     except Exception as e:
         print "{0}, RTCmanager={1}, RTC(RobotHardware0)={2}".format(str(e),ms,rh)
         self.fail()
         pass
예제 #19
0
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"))
예제 #20
0
def createComps(hostname=socket.gethostname()):
    global ms, adm_svc, rh, rh_svc, seq, seq_svc, armR, armR_svc,\
           armL, armL_svc, grsp, grsp_svc, sa, tk_svc, log, log_svc, gf, ca, use_ros
    ms = rtm.findRTCmanager(hostname)

    adm = rtm.findRTC("SystemAdmin0")
    adm_svc = None
    if adm != None:
        adm.start()
        adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))

    rh = rtm.findRTC("RobotHardware0")
    rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))

    seq = initRTC("SequencePlayer", "seq")
    seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
    if dir(seq_svc).count("loadTrajectory") == 0:
        seq_svc = SequencePlayerRosService(seq_svc, rh_svc)

    armR = initRTC("ArmControl", "armR")
    armR_svc = OpenHRP.ArmControlServiceHelper.narrow(armR.service("service0"))

    armL = initRTC("ArmControl", "armL")
    armL_svc = OpenHRP.ArmControlServiceHelper.narrow(armL.service("service0"))

    grsp = initRTC("Grasper", "grsp")
    grsp_svc = OpenHRP.GrasperServiceHelper.narrow(grsp.service("service0"))

    sa = initRTC("StateArbitrator", "sa")
    tk_svc = OpenHRP.TimeKeeperServiceHelper.narrow(sa.service("service1"))

    log = initRTC("DataLogger", "log")
    log_svc = OpenHRP.DataLoggerServiceHelper.narrow(log.service("service0"))

    gf = initRTC("GazeFixer", "gf")

    ca = initRTC("CollisionAvoider", "ca")

    return [rh, seq, armR, armL, sa, grsp, gf, ca, log]
예제 #21
0
    def waitForRobotHardware(self, robotname="Robot"):
        self.rh = None
        timeout_count = 0;
        # wait for simulator or RobotHardware setup which sometime takes a long time
        while self.rh == None and timeout_count < 10: # <- time out limit
            time.sleep(1);
            self.rh = rtm.findRTC("RobotHardware0")
            if not self.rh:
                self.rh = rtm.findRTC(robotname)
            print self.configurator_name, "wait for", robotname, " : ",self.rh, "(timeout ", timeout_count, " < 10)"
            if self.rh and self.rh.isActive() == None: # just in case rh is not ready...
                self.rh = None
            timeout_count += 1

        if not self.rh:
            print self.configurator_name, "Could not find ", robotname
            if self.ms:
                print self.configurator_name, "Candidates are .... ", [x.name()  for x in self.ms.get_components()]
            print self.configurator_name, "Exitting.... ", robotname
            exit(1)

        print self.configurator_name, "findComps -> RobotHardware : ",self.rh, "isActive? = ", self.rh.isActive()
예제 #22
0
    def waitForRobotHardware(self, robotname="Robot"):
        self.rh = None
        timeout_count = 0;
        # wait for simulator or RobotHardware setup which sometime takes a long time
        while self.rh == None and timeout_count < 10: # <- time out limit
            time.sleep(1);
            self.rh = rtm.findRTC("RobotHardware0")
            if not self.rh:
                self.rh = rtm.findRTC(robotname)
            print self.configurator_name, "wait for", robotname, " : ",self.rh, "(timeout ", timeout_count, " < 10)"
            if self.rh and self.rh.isActive() == None: # just in case rh is not ready...
                self.rh = None
            timeout_count += 1

        if not self.rh:
            print self.configurator_name, "Could not find ", robotname
            if self.ms:
                print self.configurator_name, "Candidates are .... ", [x.name()  for x in self.ms.get_components()]
            print self.configurator_name, "Exitting.... ", robotname
            exit(1)

        print self.configurator_name, "findComps -> RobotHardware : ",self.rh, "isActive? = ", self.rh.isActive()
예제 #23
0
파일: user.py 프로젝트: hsnuhayato/tsml
def createCompsRobot(nshR, rncR):
    global ms, rh, servo, rtcListR
    ms = rtm.findRTCmanager(nshR, rncR)
    rh = rtm.findRTC(bodyinfo.modelName, rncR)
    servo = rtm.findRTC("creekPdServo0", rncR)

    if rh==None or servo==None:
        print "couldn't find ROBOT or SERVO"
        return
    rtcListR = [rh]

    global kf, seq, seq_svc, sh, wpg, wpg_svc, st, st_svc, arm, arm_svc
    kf = initRTC("creekStateEstimator", "kf", ms)
    if kf != None:
        rtcListR.append(kf)

    seq = initRTC("creekSequencePlayer", "seq", ms)
    if seq != None:
        seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
        rtcListR.append(seq)

    wpg = initRTC("sony", "wpg", ms)
    if wpg != None:
        wpg_svc = OpenHRP.sonyServiceHelper.narrow(wpg.service("service0"))
        rtcListR.append(wpg)

    arm = initRTC("creekArmControlCartesian", "arm", ms)
    if arm != None:
        arm_svc = OpenHRP.creekArmControlCartesianServiceHelper.narrow(arm.service("service0"))
        rtcListR.append(arm)

    sh  = initRTC("creekReferenceHolder", "holder", ms)
    if sh != None:
        rtcListR.append(sh)

    st= initRTC("Stabilizer","st", ms)
    if st != None:
        st_svc = OpenHRP.StabilizerServiceHelper.narrow(st.service("service0"))
        rtcListR.append(st)
예제 #24
0
파일: user.py 프로젝트: hsnuhayato/tsml
def createComps(hostname=socket.gethostname()):
    global ms, rh, servo, rtcList
    global rh_s, servo_s
    ms = rtm.findRTCmanager(hostname)
    rh = rtm.findRTC(bodyinfo.modelName)
    servo = rtm.findRTC("creekPdServo0")
    rh_s = rtm.findRTC("JVRC-State")
    servo_s = rtm.findRTC("creekRobotState0")

    if rh==None or servo==None:
        print "couldn't find ROBOT or SERVO"
        return
    rtcList = [rh]

    if servo_s == None:
        servo_s = initRTC("creekRobotState", "creekRobotState0")


    global kf, seq, seq_svc, sh, camera, camera_svc

    kf = initRTC("creekStateEstimator", "kf")
    if kf != None:
        rtcList.append(kf)

    seq = initRTC("creekSequencePlayer", "seq")
    if seq != None:
        seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
        rtcList.append(seq)

    sh  = initRTC("creekReferenceHolder", "holder")
    if sh != None:
        rtcList.append(sh)

    camera = initRTC("creekCameraViewer", "camera")
    if camera != None:
        camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
        rtcList.append(camera)

    return rtcList
예제 #25
0
def init(hostname=socket.gethostname()):
    global ms, simulation_mode, kinematics_mode

    ms = rtm.findRTCmanager()

    rh = rtm.findRTC("pandaController(Robot)0")
    servo = rtm.findRTC("PDcontroller0")
    simulation_mode = 1
    if rh.port("baseTformIn"):
        kinematics_mode = 1
    else:
        kinematics_mode = 0

    print("creating components")
    createComps()

    print("connecting components")
    connectComps()

    print("activating components")
    activateComps()

    print("initialized successfully")
예제 #26
0
def createComps():

    global rh, ms, tp, log, log_svc

    rh = rtm.findRTC("SimpleFoot0")

    ms = rtm.findRTCmanager()

    ms.load("TactileInfoProcessor")
    tp = ms.create("TactileInfoProcessor")

    ms.load("DataLogger")
    log = ms.create("DataLogger", "log")
    log_svc = narrow(log.service("service0"), "DataLoggerService")
예제 #27
0
파일: user.py 프로젝트: hsnuhayato/tsml
def createCompsConsole(nshC, rncC):
    global msC, rhC, servoC, servoC_svc, rtcListC
    msC = rtm.findRTCmanager(nshC, rncC)
    rhC = rtm.findRTC("JVRC-State", rncC)
    servoC = rtm.findRTC("creekRobotState0", rncC)

    if rhC == None:
        rtcListC = rtcListR
        servoC = initRTC("creekRobotState", "creekRobotState0", msC)
        rtcListC.append(servoC)
    else:
        rtcListC = [rhC]
        if servoC == None:
            return

    servoC_svc = OpenHRP.creekRobotStateServiceHelper.narrow(servoC.service("service0"))

    global camera, camera_svc, stick, pcl, pcl_svc
    camera = None
    stick = None
    pcl = None
    

    camera = initRTC("creekCameraViewer", "camera", msC)
    if camera != None:
        camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
        rtcListC.append(camera)

    stick = initRTC("GamepadRTC", "stick", msC)
    if stick != None:
        rtcListC.append(stick)

    pcl = initRTC("creekPointCloudViewer", "pcl", msC)
    if pcl != None:
        pcl_svc = OpenHRP.creekPointCloudViewerServiceHelper.narrow(pcl.service("service0"))
        rtcListC.append(pcl)
예제 #28
0
파일: user.py 프로젝트: hsnuhayato/tsml
def createCompsRobot(nshR, rncR):
    global ms, rh, servo, rtcListR
    ms = rtm.findRTCmanager(nshR, rncR)
    rh = rtm.findRTC(bodyinfo.modelName, rncR)
    servo = rtm.findRTC("creekPdServo0", rncR)

    if rh==None or servo==None:
        print "couldn't find ROBOT or SERVO"
        return
    rtcListR = [rh]

    global kf, seq, seq_svc, sh
    kf = initRTC("creekStateEstimator", "kf", ms)
    if kf != None:
        rtcListR.append(kf)

    seq = initRTC("creekSequencePlayer", "seq", ms)
    if seq != None:
        seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
        rtcListR.append(seq)

    sh  = initRTC("creekReferenceHolder", "holder", ms)
    if sh != None:
        rtcListR.append(sh)
예제 #29
0
파일: user.py 프로젝트: s-creek/script
def createComps(hostname=socket.gethostname()):
    global ms, adm_svc, rh, rh_svc, servo, seq, seq_svc, kf, sh, simulation_mode
    ms = rtm.findRTCmanager(hostname)
    rh = rtm.findRTC("RobotHardware0")
    if rh != None:
        simulation_mode = 0
        rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
        servo = rh
        adm = rtm.findRTC("SystemAdmin0")
        if adm != None:
          adm.start()
          adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
    else:
        simulation_mode = 1
        rh = rtm.findRTC("JVRC")
        servo = rtm.findRTC("creekPdServo0")
  
    seq = initRTC("creekSequencePlayer", "seq")
    seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))

    kf  = initRTC("creekStateEstimator", "kf")
    sh  = initRTC("creekReferenceHolder", "holder")

    return [rh, seq, kf, sh]
예제 #30
0
def createComps():

    global rh, ms, ted, log, log_svc

    rh = rtm.findRTC("SimpleFoot0")

    ms = rtm.findRTCmanager()

    ms.load("TactileInfoEdgeDetector")
    ted = ms.create("TactileInfoEdgeDetector")
    ted.setProperty("foot", "left")
    ted.setProperty("Wdin", "100000")

    ms.load("DataLogger")
    log = ms.create("DataLogger", "log")
    log_svc = narrow(log.service("service0"), "DataLoggerService")
예제 #31
0
def rtc_init () :
    global ms, co, co_svc, root_link_name, root_link_offset

    rtm.initCORBA()
    ms = rtm.findRTCmanager(rtm.nshost)
    while ms == None :
        time.sleep(1);
        ms = rtm.findRTCmanager(rtm.nshost)
        rospy.loginfo("[collision_state.py] wait for RTCmanager : ",ms)

    co = None
    count = 0
    while co  == None and count < 10:
        co_name = rospy.get_param('~comp_name', 'co')
        co = rtm.findRTC(co_name)
        if co and co.isActive():
            break
        if co and co.isActive() is False:
            rospy.logwarn("CollisionDetector(%s) is not activated, waiting..." % co_name)
        else:
            rospy.logwarn("Could not found CollisionDetector(%s), waiting..." % co_name)
        time.sleep(1)
        count += 1
        co = None
    if co == None:
        rospy.logerr("Could not found CollisionDetector, exiting...")
        exit(0)

    co_svc = rtm.narrow(co.service("service0"), "CollisionDetectorService")

    if modelfile:
        import CosNaming
        obj = rtm.rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
        mdlldr = obj._narrow(OpenHRP.ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
        rospy.loginfo("  bodyinfo URL = file://"+modelfile)
        body_info = mdlldr.getBodyInfo("file://"+modelfile)
        root_link_name = body_info._get_links()[0].segments[0].name

        root_link_offset = inverse_matrix(concatenate_matrices(translation_matrix(body_info._get_links()[0].translation),
                                                rotation_matrix(body_info._get_links()[0].rotation[3],
                                                                body_info._get_links()[0].rotation[0:3])))
    else:
        root_link_name = "WAIST"
        root_link_offset = identity_matrix()

        rospy.loginfo("ssetup collision_state with " + root_link_name + " " + root_link_offset)
예제 #32
0
    def test_rh_service(self):
        try:
            rh = rh_svc = None
            rtm.nshost = 'localhost'
            rtm.nsport = 2809
            rtm.initCORBA()
            rh = rtm.findRTC("RobotHardware0")
            rh_svc = rtm.narrow(rh.service("service0"), "RobotHardwareService")
            print "RTC(RobotHardware0)={0}, {1}".format(rh,rh_svc)
            self.assertTrue(rh and rh_svc)
            rh.start()
            self.assertTrue(rh.isActive())
            self.assertTrue(rh_svc.getStatus())

        except Exception as e:
            print "{0}, RTC(RobotHardware0)={1}, {2}".format(str(e),rh,rh_svc)
            self.fail()
            pass
예제 #33
0
    def test_rh_service(self):
        try:
            rh = rh_svc = None
            rtm.nshost = 'localhost'
            rtm.nsport = 2809
            rtm.initCORBA()
            rh = rtm.findRTC("RobotHardware0")
            rh_svc = rtm.narrow(rh.service("service0"), "RobotHardwareService")
            print "RTC(RobotHardware0)={0}, {1}".format(rh, rh_svc)
            self.assertTrue(rh and rh_svc)
            rh.start()
            self.assertTrue(rh.isActive())
            self.assertTrue(rh_svc.getStatus())

        except Exception as e:
            print "{0}, RTC(RobotHardware0)={1}, {2}".format(
                str(e), rh, rh_svc)
            self.fail()
            pass
예제 #34
0
 def check_initCORBA(self, nshost, nsport=2809):
     try:
         ms = rh = None
         rtm.nshost = nshost
         rtm.nsport = nsport
         rtm.initCORBA()
         count = 0
         while ( not (ms and rh) ) and count < 10:
             ms = rtm.findRTCmanager()
             rh = rtm.findRTC("RobotHardware0")
             if ms and rh :
                 break
             time.sleep(1)
             print >>sys.stderr, "wait for RTCmanager=%r, RTC(RobotHardware0)=%r"%(ms,rh)
             count += 1
         self.assertTrue(ms and rh)
     except Exception as e:
         self.fail("%r, nshost=%r, nsport=%r RTCmanager=%r, RTC(RobotHardware0)=%r"%(str(e),nshost,nsport,ms,rh))
         pass
예제 #35
0
 def check_initCORBA(self, nshost, nsport=2809):
     try:
         ms = rh = None
         rtm.nshost = nshost
         rtm.nsport = nsport
         rtm.initCORBA()
         count = 0
         while (not (ms and rh)) and count < 10:
             ms = rtm.findRTCmanager()
             rh = rtm.findRTC("RobotHardware0")
             if ms and rh:
                 break
             time.sleep(1)
             print >> sys.stderr, "wait for RTCmanager=%r, RTC(RobotHardware0)=%r" % (
                 ms, rh)
             count += 1
         self.assertTrue(ms and rh)
     except Exception as e:
         self.fail(
             "%r, nshost=%r, nsport=%r RTCmanager=%r, RTC(RobotHardware0)=%r"
             % (str(e), nshost, nsport, ms, rh))
         pass
#!/usr/bin/env python
import roslib; roslib.load_manifest("hrpsys")

import os
import rtm

from rtm import *
from OpenHRP import *

import socket
import time


rh=rtm.findRTC("RobotHardware0")
rhr=rtm.findRTC("RobotHardwareServiceROSBridge")

connectPorts(rhr.port("RobotHardwareService"),rh.port("RobotHardwareService"))

예제 #37
0
파일: vcTest.py 프로젝트: 130s/hrpsys-base
#
# test script for VirtualCamera component
# 
# start VirtualCameraComp 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/SampleRobot_inHouse.xml"
vc = rtm.findRTC("VirtualCamera0")
print "vc:",vc
vc.setProperty("project", project)
vc.setProperty("camera", "Robot:VISION_SENSOR1")
vc.start()
예제 #38
0
def findRTC(name):
    rtc = rtm.findRTC(name)
    if rtc == None:
        raise Exception("Failed to find {}.".format(name))
    return rtc
예제 #39
0
파일: user.py 프로젝트: hsnuhayato/script
from org.omg.CosNaming.NamingContextPackage import *
from com.sun.corba.se.impl.encoding import EncapsOutputStream

from java.lang import System, Class

from RTC import *
from RTM import *
from OpenRTM import *
from _SDOPackage import *


import string, math, socket, time, sys
global rootnc, nshost

java.lang.System.setProperty('NS_OPT', '-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService')
rtm.initCORBA()
global kobuki 
kobuki= rtm.findRTC("MobileRobot0")

if kobuki==None:
    print "no robot"

prof = kobuki.port("vel").get_port_profile()
prop = prof.properties
for p in prop:
    print "kobu",p.name
    if p.name == "dataport.data_type":
        print p.value.extract_string()


예제 #40
0
#
# 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()
예제 #41
0
#
# test script for Simulator component
# 
# start hrpsys-viewer and SimulatorComp 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/Sample.xml"
sim = rtm.findRTC("Simulator0")
print "sim:",sim
sim.setProperty("project", project)
sim.setProperty("useOLV", "1")
sim.start()
예제 #42
0
파일: activate.py 프로젝트: trigrass2/etc
import rtm

js = rtm.findRTC("Joystick0")
js.start()
예제 #43
0
#
# test script for VirtualCamera component
#
# start VirtualCameraComp 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/SampleRobot_inHouse.xml"
vc = rtm.findRTC("VirtualCamera0")
print "vc:", vc
vc.setProperty("project", project)
vc.setProperty("camera", "Robot:VISION_SENSOR1")
vc.start()
예제 #44
0
ns2 = rtm.orb.string_to_object('corbaloc:iiop:%s:%s/NameService' % (nshost2, nsport2))
nc2 = ns2._narrow(CosNaming.NamingContext)
if practicemode:
    mgr2 = mgr
else:
    mgr2obj = rtm.orb.string_to_object('corbaloc:iiop:%s:2810/manager' % (nshost2))
    mgr2 = rtm.RTCmanager(mgr2obj._narrow(RTM.Manager))

mgr2.load('SequencePlayer')
mgr2.load('StateHolder')
mgr2.load('ForwardKinematics')
mgr2.load('Joystick')
mgr2.load('Joystick2PanTiltAngles')
mgr2.load('MidJaxonController')

midjaxon = rtm.findRTC('MIDJAXON', nc2)
rh = rtm.findRTC('PDcontrollerMIDJAXON0', nc2)
seq = mgr2.create('SequencePlayer', 'seq')
seqsvc = rtm.narrow(seq.service('service0'), 'SequencePlayerService', 'hrpsys.OpenHRP')
sh = mgr2.create('StateHolder', 'sh')
shsvc = rtm.narrow(sh.service('service0'), 'StateHolderService', 'hrpsys.OpenHRP')
fk = mgr2.create('ForwardKinematics', 'fk')

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')
예제 #45
0
#!/usr/bin/env python
import roslib
roslib.load_manifest("hrpsys")

import os
import rtm

from rtm import *
from OpenHRP import *

import socket
import time

rh = rtm.findRTC("RobotHardware0")
rhr = rtm.findRTC("RobotHardwareServiceROSBridge")

connectPorts(rhr.port("RobotHardwareService"), rh.port("RobotHardwareService"))
예제 #46
0
#
# test script for Simulator component
#
# start hrpsys-viewer and SimulatorComp 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/Sample.xml"
sim = rtm.findRTC("Simulator0")
print "sim:", sim
sim.setProperty("project", project)
sim.setProperty("useOLV", "1")
sim.start()
예제 #47
0
from org.omg.CosNaming import *
from org.omg.CosNaming.NamingContextPackage import *
from com.sun.corba.se.impl.encoding import EncapsOutputStream

from java.lang import System, Class

from RTC import *
from RTM import *
from OpenRTM import *
from _SDOPackage import *

import string, math, socket, time, sys
global rootnc, nshost

java.lang.System.setProperty(
    'NS_OPT',
    '-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService')
rtm.initCORBA()
global kobuki
kobuki = rtm.findRTC("MobileRobot0")

if kobuki == None:
    print "no robot"

prof = kobuki.port("vel").get_port_profile()
prop = prof.properties
for p in prop:
    print "kobu", p.name
    if p.name == "dataport.data_type":
        print p.value.extract_string()
예제 #48
0
    rtm.nsport = nsport
    nsloc = "corbaloc:iiop:%s:%d/NameService" % (rtm.nshost, rtm.nsport)
    rtm.rootnc = rtm.orb.string_to_object(nsloc)._narrow(
        CosNaming.NamingContext)


try:
    import cnoid.Corba
    rtm.orb = cnoid.Corba.getORB()
except:
    rtm.initCORBA()

try:
    setNameserver()

    camera = rtm.findRTC("IIDCCamera0")
    if camera == None:
        raise Exception("Failed to find IIDCCamera0")
    viewer = rtm.findRTC("ImageViewer0")
    if viewer == None:
        raise Exception("Failed to find ImageViewer0")
    cpanel = rtm.findRTC("ControlPanel0")
    if cpanel == None:
        raise Exception("Failed to find ControlPanel0")

    connectPorts(cpanel.port("Command"), camera.port("Command"))
    connectPorts(camera.port("TimedCameraImage"), viewer.port("images"))

    camera.start()
    viewer.start()
    cpanel.start()
예제 #49
0
    rtm.nsport = nsport
    nsloc = "corbaloc:iiop:%s:%d/NameService" % (rtm.nshost, rtm.nsport)
    rtm.rootnc = rtm.orb.string_to_object(nsloc)._narrow(
        CosNaming.NamingContext)


try:
    import cnoid.Corba
    rtm.orb = cnoid.Corba.getORB()
except:
    rtm.initCORBA()

try:
    setNameserver()

    camera = rtm.findRTC("V4L2MultiCamera0")
    if camera == None:
        raise Exception("Failed to find V4L2MultiCamera0")
    viewer = rtm.findRTC("MultiImageViewer0")
    if viewer == None:
        raise Exception("Failed to find MultiImageViewer0")
    cpanel = rtm.findRTC("ControlPanel0")
    if cpanel == None:
        raise Exception("Failed to find ControlPanel0")

    connectPorts(camera.port("TimedImages"), viewer.port("images"))
    connectPorts(cpanel.port("Command"), camera.port("Command"))

    camera.start()
    viewer.start()
    cpanel.start()