Exemplo n.º 1
0
def main():
    global app, view, nav_data

    nav_data = np.array([[0, 0, 0]])

    lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData)

    app = ConsoleApp()

    app.setupGlobals(globals())
    app.showPythonConsole()

    view = app.createView()
    view.show()

    global d
    d = DebugData()
    d.addLine([0, 0, 0], [1, 0, 0], color=[0, 1, 0])
    d.addSphere((0, 0, 0), radius=0.02, color=[1, 0, 0])

    #vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255')

    startSwarmVisualization()

    app.start()
Exemplo n.º 2
0
 def __init__(self):
     self.timer = TimerCallback(targetFps=10)
     #self.timer.disableScheduledTimer()
     self.app = ConsoleApp()
     self.robotModel, self.jointController = roboturdf.loadRobotModel(
         'robot model')
     self.fpsCounter = simpletimer.FPSCounter()
     self.drakePoseJointNames = robotstate.getDrakePoseJointNames()
     self.fpsCounter.printToConsole = True
     self.timer.callback = self._tick
     self._initialized = False
     self.publishChannel = 'JOINT_POSITION_GOAL'
     # self.lastCommandMessage = newAtlasCommandMessageAtZero()
     self._numPositions = len(robotstate.getDrakePoseJointNames())
     self._previousElapsedTime = 100
     self._baseFlag = 0
     self.jointLimitsMin = np.array([
         self.robotModel.model.getJointLimits(jointName)[0]
         for jointName in robotstate.getDrakePoseJointNames()
     ])
     self.jointLimitsMax = np.array([
         self.robotModel.model.getJointLimits(jointName)[1]
         for jointName in robotstate.getDrakePoseJointNames()
     ])
     self.useControllerFlag = False
     self.drivingGainsFlag = False
     self.applyDefaults()
Exemplo n.º 3
0
    def __init__(self,
                 percentObsDensity=20,
                 endTime=40,
                 nonRandomWorld=False,
                 circleRadius=0.7,
                 worldScale=1.0,
                 autoInitialize=True,
                 verbose=True):
        self.verbose = verbose
        self.startSimTime = time.time()
        self.collisionThreshold = 1.3
        self.randomSeed = 5
        self.Sensor_rayLength = 8

        self.percentObsDensity = percentObsDensity
        self.defaultControllerTime = 1000
        self.nonRandomWorld = nonRandomWorld
        self.circleRadius = circleRadius
        self.worldScale = worldScale

        # create the visualizer object
        self.app = ConsoleApp()
        self.view = self.app.createView(useGrid=False)

        self.initializeOptions()
        self.initializeColorMap()

        if autoInitialize:
            self.initialize()
Exemplo n.º 4
0
    def __init__(self):

        self.app = ConsoleApp()
        self.view = self.app.createView()
        self.robotSystem = robotsystem.create(self.view)

        jointGroups = getJointGroups()
        self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups)
        self.jointCommandPanel = JointCommandPanel(self.robotSystem)

        self.jointCommandPanel.ui.speedSpinBox.setEnabled(False)

        self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms)
        self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs)
        self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders)
        self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged)
        self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged)

        self.widget = QtGui.QWidget()

        gl = QtGui.QGridLayout(self.widget)
        gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan
        gl.addWidget(self.view, 0, 1, 1, 1)
        gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1)
        gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1)
        gl.setRowStretch(0,1)
        gl.setColumnStretch(1,1)

        #self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
        lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
        lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
Exemplo n.º 5
0
def main():
    app = ConsoleApp()
    camVis = CameraVisualizer()
    camVis.createUI()
    camVis.showUI()

    app.start()
Exemplo n.º 6
0
    def __init__(self):

        self.app = ConsoleApp()
        self.view = self.app.createView()
        self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view)
        self.jointController.setZeroPose()
        self.view.show()
        self.sub = lcmUtils.addSubscriber('ATLAS_COMMAND', lcmdrc.atlas_command_t, self.onAtlasCommand)
        self.sub.setSpeedLimit(60)
Exemplo n.º 7
0
def main():

    app = ConsoleApp()

    view = app.createView(useGrid=False)
    imageManager = cameraview.ImageManager()
    cameraView = cameraview.CameraView(imageManager, view)

    view.show()
    app.start()
Exemplo n.º 8
0
    def __init__(self):
        self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmdrc.robot_state_t, self.onJointPositionGoal)
        self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
        lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause)
        self.debug = False

        if self.debug:

            self.app = ConsoleApp()
            self.view = self.app.createView()
            self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view)
            self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose)
            self.view.show()
            self.timer = TimerCallback(targetFps=30)
            self.timer.callback = self.onDebug
Exemplo n.º 9
0
    def setDefaultOptions(self):
        defaultOptions = dict()
        defaultOptions['Sensor'] = dict()
        defaultOptions['Sensor']['rayLength'] = 20
        defaultOptions['Sensor']['numRays'] = 100
        defaultOptions['Sensor']['FOV'] = 270

        defaultOptions['Sim'] = dict()
        defaultOptions['Sim']['dt'] = 0.05
        defaultOptions['Sim']['usePursuitController'] = True

        defaultOptions['featureDetector'] = {}
        defaultOptions['featureDetector']['detectCorners'] = True
        self.options = defaultOptions

        self.app = ConsoleApp()
        self.view = self.app.createView(useGrid=False)
    def __init__(self,
                 percentObsDensity=20,
                 endTime=40,
                 randomizeControl=False,
                 nonRandomWorld=False,
                 circleRadius=0.7,
                 worldScale=1.0,
                 supervisedTrainingTime=500,
                 autoInitialize=True,
                 verbose=True,
                 sarsaType="discrete"):
        self.verbose = verbose
        self.randomizeControl = randomizeControl
        self.startSimTime = time.time()
        self.collisionThreshold = 1.3
        self.randomSeed = 5
        self.Sarsa_numInnerBins = 4
        self.Sarsa_numOuterBins = 4
        self.Sensor_rayLength = 8
        self.sarsaType = sarsaType

        self.percentObsDensity = percentObsDensity
        self.supervisedTrainingTime = 10
        self.learningRandomTime = 10
        self.learningEvalTime = 10
        self.defaultControllerTime = 10
        self.nonRandomWorld = nonRandomWorld
        self.circleRadius = circleRadius
        self.worldScale = worldScale
        # create the visualizer object
        self.app = ConsoleApp()
        # view = app.createView(useGrid=False)
        self.view = self.app.createView(useGrid=False)

        self.initializeOptions()
        self.initializeColorMap()
        if autoInitialize:
            self.initialize()
Exemplo n.º 11
0
def main():

    app = ConsoleApp()

    view = app.createView(useGrid=False)
    view.orientationMarkerWidget().Off()
    view.backgroundRenderer().SetBackground([0, 0, 0])
    view.backgroundRenderer().SetBackground2([0, 0, 0])

    cameraChannel = parseChannelArgument()
    imageManager = cameraview.ImageManager()
    imageManager.queue.addCameraStream(cameraChannel)
    imageManager.addImage(cameraChannel)

    cameraView = cameraview.CameraImageView(imageManager,
                                            cameraChannel,
                                            view=view)
    cameraView.eventFilterEnabled = False
    view.renderWindow().GetInteractor().SetInteractorStyle(
        vtk.vtkInteractorStyleImage())

    view.show()
    app.start()
Exemplo n.º 12
0
        position, quat = transformUtils.poseFromTransform(pelvisToWorld)

        msg = lcmbot.pose_t()
        msg.utime = robotStateJointController.lastRobotStateMessage.utime
        msg.pos = [0.0, 0.0, 0.0]
        msg.orientation = quat.tolist()

        lcmUtils.publish(channel, msg)

    def enable(self):
        self.capture()
        self.timer.start()

    def disable(self):
        self.timer.stop()
        self.clear()


#---------------------------------------

app = ConsoleApp()
app.setupGlobals(globals())
app.showPythonConsole()
view = app.createView()
view.show()
robotsystem.create(view, globals())

estimator = FootLockEstimator()

app.start()