Example #1
0
    def sendWorkspaceDepthRequest(repeatTime=0.0):
        sendDataRequest(lcmdrc.data_request_t.DEPTH_MAP_WORKSPACE_C, repeatTime)

    def sendSceneDepthRequest(repeatTime=0.0):
        sendDataRequest(lcmdrc.data_request_t.DEPTH_MAP_SCENE, repeatTime)

    def sendFusedDepthRequest(repeatTime=0.0):
        sendDataRequest(lcmdrc.data_request_t.FUSED_DEPTH, repeatTime)

    def sendFusedHeightRequest(repeatTime=0.0):
        sendDataRequest(lcmdrc.data_request_t.FUSED_HEIGHT, repeatTime)


    handJoints = []
    if drcargs.args().directorConfigFile.find('atlas') != -1:
        handJoints = roboturdf.getRobotiqJoints() + ['neck_ay']
    else:
        for handModel in ikPlanner.handModels:
            handJoints += handModel.handModel.model.getJointNames()
        # filter base joints out
        handJoints = [ joint for joint in handJoints if joint.find('base')==-1 ]

    teleopJointPropagator = JointPropagator(robotStateModel, teleopRobotModel, handJoints)
    playbackJointPropagator = JointPropagator(robotStateModel, playbackRobotModel, handJoints)
    def doPropagation(model=None):
        if teleopRobotModel.getProperty('Visible'):
            teleopJointPropagator.doPropagation()
        if playbackRobotModel.getProperty('Visible'):
            playbackJointPropagator.doPropagation()
    robotStateModel.connectModelChanged(doPropagation)
Example #2
0
        sendDataRequest(lcmdrc.data_request_t.HEIGHT_MAP_SCENE, repeatTime)

    def sendWorkspaceDepthRequest(repeatTime=0.0):
        sendDataRequest(lcmdrc.data_request_t.DEPTH_MAP_WORKSPACE_C, repeatTime)

    def sendSceneDepthRequest(repeatTime=0.0):
        sendDataRequest(lcmdrc.data_request_t.DEPTH_MAP_SCENE, repeatTime)

    def sendFusedDepthRequest(repeatTime=0.0):
        sendDataRequest(lcmdrc.data_request_t.FUSED_DEPTH, repeatTime)

    def sendFusedHeightRequest(repeatTime=0.0):
        sendDataRequest(lcmdrc.data_request_t.FUSED_HEIGHT, repeatTime)


    teleopJointPropagator = JointPropagator(robotStateModel, teleopRobotModel, roboturdf.getRobotiqJoints() + ['neck_ay'])
    playbackJointPropagator = JointPropagator(robotStateModel, playbackRobotModel, roboturdf.getRobotiqJoints())
    def doPropagation(model=None):
        if teleopRobotModel.getProperty('Visible'):
            teleopJointPropagator.doPropagation()
        if playbackRobotModel.getProperty('Visible'):
            playbackJointPropagator.doPropagation()
    robotStateModel.connectModelChanged(doPropagation)

    #app.addToolbarMacro('scene height', sendSceneHeightRequest)
    #app.addToolbarMacro('scene depth', sendSceneDepthRequest)
    #app.addToolbarMacro('stereo height', sendFusedHeightRequest)
    #app.addToolbarMacro('stereo depth', sendFusedDepthRequest)


    jointLimitChecker = teleoppanel.JointLimitChecker(robotStateModel, robotStateJointController)