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()
