from director import roboturdf app = ConsoleApp() # create a view view = app.createView() segmentation._defaultSegmentationView = view robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True) segmentationroutines.SegmentationContext.initWithRobot(robotStateModel) # Move robot to near to table: robotStateJointController.q [5] = math.radians(120) robotStateJointController.q[0] = -1.5 robotStateJointController.q[1] = 2 robotStateJointController.q[2] = 0.95 robotStateJointController.push() # load poly data dataDir = app.getTestingDataDirectory() polyData = ioUtils.readPolyData(os.path.join(dataDir, 'tabletop/table-and-bin-scene.vtp')) vis.showPolyData(polyData, 'pointcloud snapshot') #segmentation.segmentTableScene(polyData, [-1.58661389, 2.91242337, 0.79958105] ) segmentation.segmentTableSceneClusters(polyData, [-1.58661389, 2.91242337, 0.79958105], clusterInXY=True ) if app.getTestingInteractiveEnabled(): view.show() app.showObjectModel() app.start()
class AtlasCommandPanel(object): def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotSystem = robotsystem.create(self.view) self.config = drcargs.getDirectorConfig() jointGroups = self.config['teleopJointGroups'] 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) def onSingleJointPositionGoal(self, msg): jointPositionGoal = msg.joint_position jointName = msg.joint_name allowedJointNames = ['l_leg_aky', 'l_arm_lwy'] if not (jointName in allowedJointNames): print 'Position goals are not allowed for this joint' print 'ignoring this position goal' print 'use the sliders instead' return if (jointName == 'l_arm_lwy') and ( not self.jointCommandPanel.steeringControlEnabled): print 'Steering control not enabled' print 'ignoring steering command' return if (jointName == 'l_leg_aky') and ( not self.jointCommandPanel.throttleControlEnabled): print 'Throttle control not enabled' print 'ignoring throttle command' return jointIdx = self.jointTeleopPanel.toJointIndex(joint_name) self.jointTeleopPanel.endPose[jointIdx] = jointPositionGoal self.jointTeleopPanel.updateSliders() self.jointTeleopPanel.sliderChanged(jointName) def onRobotPlan(self, msg): playback = planplayback.PlanPlayback() playback.interpolationMethod = 'pchip' poseTimes, poses = playback.getPlanPoses(msg) f = playback.getPoseInterpolator(poseTimes, poses) jointController = self.robotSystem.teleopJointController timer = simpletimer.SimpleTimer() def setPose(pose): jointController.setPose('plan_playback', pose) self.jointTeleopPanel.endPose = pose self.jointTeleopPanel.updateSliders() commandStream.setGoalPose(pose) def updateAnimation(): tNow = timer.elapsed() if tNow > poseTimes[-1]: pose = poses[-1] setPose(pose) return False pose = f(tNow) setPose(pose) self.animationTimer = TimerCallback() self.animationTimer.targetFps = 60 self.animationTimer.callback = updateAnimation self.animationTimer.start() def mirrorJointsChanged(self): self.jointTeleopPanel.mirrorLegs = self.jointCommandPanel.ui.mirrorLegsCheck.checked self.jointTeleopPanel.mirrorArms = self.jointCommandPanel.ui.mirrorArmsCheck.checked def resetJointTeleopSliders(self): self.jointTeleopPanel.resetPoseToRobotState()
print reader import time app = ConsoleApp() view = app.createView() def spin(): polyData = vtk.vtkPolyData() reader.GetMesh(polyData) vis.updatePolyData(polyData, 'mesh') print "Number of points (a)", polyData.GetNumberOfPoints() if (polyData.GetNumberOfPoints() == 0): return polyDataPC = vtk.vtkPolyData() reader.GetPointCloud(polyDataPC) vis.updatePolyData(polyDataPC, 'output') print "Number of points (b)", polyDataPC.GetNumberOfPoints() quitTimer = TimerCallback(targetFps=5.0) quitTimer.callback = spin quitTimer.start() if app.getTestingInteractiveEnabled(): view.show() app.showObjectModel() app.start()
class AtlasCommandPanel(object): def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotSystem = robotsystem.create(self.view) self.config = drcargs.getDirectorConfig() jointGroups = self.config['teleopJointGroups'] 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) def onSingleJointPositionGoal(self, msg): jointPositionGoal = msg.joint_position jointName = msg.joint_name allowedJointNames = ['l_leg_aky','l_arm_lwy'] if not (jointName in allowedJointNames): print 'Position goals are not allowed for this joint' print 'ignoring this position goal' print 'use the sliders instead' return if (jointName == 'l_arm_lwy') and (not self.jointCommandPanel.steeringControlEnabled): print 'Steering control not enabled' print 'ignoring steering command' return if (jointName == 'l_leg_aky') and (not self.jointCommandPanel.throttleControlEnabled): print 'Throttle control not enabled' print 'ignoring throttle command' return jointIdx = self.jointTeleopPanel.toJointIndex(joint_name) self.jointTeleopPanel.endPose[jointIdx] = jointPositionGoal self.jointTeleopPanel.updateSliders() self.jointTeleopPanel.sliderChanged(jointName) def onRobotPlan(self, msg): playback = planplayback.PlanPlayback() playback.interpolationMethod = 'pchip' poseTimes, poses = playback.getPlanPoses(msg) f = playback.getPoseInterpolator(poseTimes, poses) jointController = self.robotSystem.teleopJointController timer = simpletimer.SimpleTimer() def setPose(pose): jointController.setPose('plan_playback', pose) self.jointTeleopPanel.endPose = pose self.jointTeleopPanel.updateSliders() commandStream.setGoalPose(pose) def updateAnimation(): tNow = timer.elapsed() if tNow > poseTimes[-1]: pose = poses[-1] setPose(pose) return False pose = f(tNow) setPose(pose) self.animationTimer = TimerCallback() self.animationTimer.targetFps = 60 self.animationTimer.callback = updateAnimation self.animationTimer.start() def mirrorJointsChanged(self): self.jointTeleopPanel.mirrorLegs = self.jointCommandPanel.ui.mirrorLegsCheck.checked self.jointTeleopPanel.mirrorArms = self.jointCommandPanel.ui.mirrorArmsCheck.checked def resetJointTeleopSliders(self): self.jointTeleopPanel.resetPoseToRobotState()