def initTeleop(self, robotSystem):
from director import roboturdf
from director import teleoppanel
directorConfig = robotSystem.directorConfig
teleopRobotModel, teleopJointController = roboturdf.loadRobotModel(
'teleop model',
robotSystem.view,
urdfFile=directorConfig['urdfConfig']['teleop'],
parent='planning',
color=roboturdf.getRobotBlueColor(),
visible=False,
colorMode=directorConfig['colorMode'])
teleopPanel = teleoppanel.TeleopPanel(
robotSystem.robotStateModel, robotSystem.robotStateJointController,
teleopRobotModel, teleopJointController, robotSystem.ikPlanner,
robotSystem.manipPlanner, robotSystem.affordanceManager,
robotSystem.playbackPanel.setPlan,
robotSystem.playbackPanel.hidePlan, robotSystem.planningUtils)
return FieldContainer(
teleopRobotModel=teleopRobotModel,
teleopJointController=teleopJointController,
teleopPanel=teleopPanel,
)
def initPlayback(self, robotSystem):
from director import roboturdf
from director import planplayback
from director import playbackpanel
from director import robotplanlistener
directorConfig = robotSystem.directorConfig
manipPlanner = robotplanlistener.ManipulationPlanDriver(
robotSystem.ikPlanner)
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
'playback model',
robotSystem.view,
urdfFile=directorConfig['urdfConfig']['playback'],
parent='planning',
color=roboturdf.getRobotOrangeColor(),
visible=False,
colorMode=directorConfig['colorMode'])
planPlayback = planplayback.PlanPlayback()
playbackPanel = playbackpanel.PlaybackPanel(
planPlayback, playbackRobotModel, playbackJointController,
robotSystem.robotStateModel, robotSystem.robotStateJointController,
manipPlanner)
manipPlanner.connectPlanReceived(playbackPanel.setPlan)
return FieldContainer(playbackRobotModel=playbackRobotModel,
playbackJointController=playbackJointController,
planPlayback=planPlayback,
manipPlanner=manipPlanner,
playbackPanel=playbackPanel)
def initPlanning(self, robotSystem):
from director import objectmodel as om
from director import planningutils
from director import roboturdf
from director import ikplanner
directorConfig = robotSystem.directorConfig
ikRobotModel, ikJointController = roboturdf.loadRobotModel('ik model', urdfFile=directorConfig['urdfConfig']['ik'], parent=None)
om.removeFromObjectModel(ikRobotModel)
ikJointController.addPose('q_end', ikJointController.getPose('q_nom'))
ikJointController.addPose('q_start', ikJointController.getPose('q_nom'))
handFactory = roboturdf.HandFactory(robotSystem.robotStateModel)
handModels = []
for side in ['left', 'right']:
if side in handFactory.defaultHandTypes:
handModels.append(handFactory.getLoader(side))
ikPlanner = ikplanner.IKPlanner(ikRobotModel, ikJointController, handModels)
planningUtils = planningutils.PlanningUtils(robotSystem.robotStateModel, robotSystem.robotStateJointController)
return FieldContainer(
ikRobotModel=ikRobotModel,
ikJointController=ikJointController,
handFactory=handFactory,
handModels=handModels,
ikPlanner=ikPlanner,
planningUtils=planningUtils
)
def initPlanning(self, robotSystem):
from director import objectmodel as om
from director import planningutils
from director import roboturdf
from director import ikplanner
directorConfig = robotSystem.directorConfig
ikRobotModel, ikJointController = roboturdf.loadRobotModel('ik model', urdfFile=directorConfig['urdfConfig']['ik'], parent=None)
om.removeFromObjectModel(ikRobotModel)
ikJointController.addPose('q_end', ikJointController.getPose('q_nom'))
ikJointController.addPose('q_start', ikJointController.getPose('q_nom'))
handFactory = roboturdf.HandFactory(robotSystem.robotStateModel)
handModels = []
for side in ['left', 'right']:
if side in handFactory.defaultHandTypes:
handModels.append(handFactory.getLoader(side))
ikPlanner = ikplanner.IKPlanner(ikRobotModel, ikJointController, handModels)
planningUtils = planningutils.PlanningUtils(robotSystem.robotStateModel, robotSystem.robotStateJointController)
return FieldContainer(
ikRobotModel=ikRobotModel,
ikJointController=ikJointController,
handFactory=handFactory,
handModels=handModels,
ikPlanner=ikPlanner,
planningUtils=planningUtils
)
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()
def initPlayback(self, robotSystem):
from director import roboturdf
from director import planplayback
from director import playbackpanel
from director import robotplanlistener
directorConfig = robotSystem.directorConfig
manipPlanner = robotplanlistener.ManipulationPlanDriver(robotSystem.ikPlanner)
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel('playback model', robotSystem.view, urdfFile=directorConfig['urdfConfig']['playback'], parent='planning', color=roboturdf.getRobotOrangeColor(), visible=False, colorMode=directorConfig['colorMode'])
planPlayback = planplayback.PlanPlayback()
playbackPanel = playbackpanel.PlaybackPanel(planPlayback, playbackRobotModel, playbackJointController,
robotSystem.robotStateModel, robotSystem.robotStateJointController, manipPlanner)
manipPlanner.connectPlanReceived(playbackPanel.setPlan)
return FieldContainer(
playbackRobotModel=playbackRobotModel,
playbackJointController=playbackJointController,
planPlayback=planPlayback,
manipPlanner=manipPlanner,
playbackPanel=playbackPanel
)
def test():
import os
import pydrake
from director import roboturdf
filename = os.path.join(pydrake.getDrakePath(), 'examples/PR2/pr2.urdf')
assert os.path.isfile(filename)
robotModel, jointController = roboturdf.loadRobotModel(urdfFile=filename,
view=view,
useConfigFile=False)
conf = {
'pr2LeftGripper': [0.07],
'pr2RightArm':
[-0.91698, 0.042600, -1.5, -2.01531, -1.57888, -1.65300, -2.04511],
'pr2Base': [0.0, 0.0, 0.0],
'pr2Torso': [0.3],
'pr2RightGripper': [0.07],
'pr2Head': [0.0, 0.0],
'pr2LeftArm': [2.1, 1.29, 0.0, -0.15, 0.0, -0.1, 0.0]
}
t = BHPNTranslator(jointController)
t.setRobotConf(conf)
def __init__(self, jointController):
self.jointController = jointController
self.lastFootstepPlan = None
self.lastFootstepRequest = None
self.goalSteps = None
self.lastWalkingPlan = None
self.walkingPlanCallback = None
self.default_step_params = DEFAULT_STEP_PARAMS
self.contact_slices = DEFAULT_CONTACT_SLICES
self.show_contact_slices = False
self.toolbarWidget = None
### Stuff pertaining to rendering BDI-frame steps
self.pose_bdi = None
self.bdi_plan = None
self.bdi_plan_adjusted = None
view = app.getDRCView()
self.bdiRobotModel, self.bdiJointController = roboturdf.loadRobotModel('bdi model', view, parent='bdi model', color=roboturdf.getRobotOrangeColor(), visible=False)
self.bdiRobotModel.setProperty('Visible', False)
self.showBDIPlan = False # hide the BDI plans when created
self.bdiChannel = "POSE_BDI"
self.bdiSubcribe = None
#enable this to used the bdi model to render a different state
#self.bdiJointController.addLCMUpdater("EST_ROBOT_STATE_ALT")
self._setupSubscriptions()
self._setupProperties()
self.showToolbarWidget()
# If we're a consoleapp and have no main window execButton won't exist
if hasattr(self, 'execButton'):
self.execButton.setEnabled(False)
self.committedPlans = []
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', lcmbotcore.atlas_command_t, self.onAtlasCommand)
self.sub.setSpeedLimit(60)
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', lcmbotcore.atlas_command_t, self.onAtlasCommand)
self.sub.setSpeedLimit(60)
def initConvexHullModel(self, robotSystem):
from director import roboturdf
directorConfig = robotSystem.directorConfig
chullRobotModel, chullJointController = roboturdf.loadRobotModel('convex hull model', view, urdfFile=directorConfig['urdfConfig']['chull'], parent='planning', color=roboturdf.getRobotOrangeColor(), colorMode=directorConfig['colorMode'], visible=False)
robotSystem.playbackJointController.models.append(chullRobotModel)
return FieldContainer(
chullRobotModel=chullRobotModel,
chullJointController=chullJointController
)
def initConvexHullModel(self, robotSystem):
from director import roboturdf
directorConfig = robotSystem.directorConfig
chullRobotModel, chullJointController = roboturdf.loadRobotModel('convex hull model', view, urdfFile=directorConfig['urdfConfig']['chull'], parent='planning', color=roboturdf.getRobotOrangeColor(), colorMode=directorConfig['colorMode'], visible=False)
robotSystem.playbackJointController.models.append(chullRobotModel)
return FieldContainer(
chullRobotModel=chullRobotModel,
chullJointController=chullJointController
)
def __init__(self):
self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.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
def __init__(self):
self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.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
def initTeleop(self, robotSystem):
from director import roboturdf
from director import teleoppanel
directorConfig = robotSystem.directorConfig
teleopRobotModel, teleopJointController = roboturdf.loadRobotModel('teleop model', robotSystem.view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False, colorMode=directorConfig['colorMode'])
teleopPanel = teleoppanel.TeleopPanel(robotSystem.robotStateModel, robotSystem.robotStateJointController, teleopRobotModel, teleopJointController,
robotSystem.ikPlanner, robotSystem.manipPlanner, robotSystem.affordanceManager, robotSystem.playbackPanel.setPlan, robotSystem.playbackPanel.hidePlan, robotSystem.planningUtils)
return FieldContainer(
teleopRobotModel=teleopRobotModel,
teleopJointController=teleopJointController,
teleopPanel=teleopPanel,
)
def initRobotState(self, robotSystem):
from director import roboturdf
robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
'robot state model',
robotSystem.view,
urdfFile=robotSystem.directorConfig['urdfConfig']['robotState'],
color=roboturdf.getRobotGrayColor(),
colorMode=robotSystem.directorConfig['colorMode'],
parent='sensors',
visible=True)
robotStateJointController.setPose('EST_ROBOT_STATE', robotStateJointController.getPose('q_nom'))
roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)])
robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')
return FieldContainer(robotStateModel=robotStateModel,
robotStateJointController=robotStateJointController)
def initRobotState(self, robotSystem):
from director import roboturdf
from director import objectmodel as om
robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
"robot state model",
robotSystem.view,
color=roboturdf.getRobotGrayColor(),
colorMode=robotSystem.directorConfig["colorMode"],
parent=om.getOrCreateContainer(
"sensors", om.getOrCreateContainer(robotSystem.robotName)),
visible=True,
robotName=robotSystem.robotName,
)
# robotStateJointController.setPose('EST_ROBOT_STATE', robotStateJointController.getPose('q_nom'))
# roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)])
return FieldContainer(
robotStateModel=robotStateModel,
robotStateJointController=robotStateJointController,
)
def onMousePress(self, displayPoint, modifiers=None):
print self.linkName, self.pickedPoint
###########################
app = ConsoleApp()
app.setupGlobals(globals())
view = app.createView()
view.show()
view.resize(1080, 768)
robotModel, jointController = roboturdf.loadRobotModel(
'robot model',
view,
parent='scene',
color=roboturdf.getRobotGrayColor(),
visible=True)
robotModel.addToView(view)
widget = LinkWidget(view, robotModel)
widget.start()
app.viewOptions.setProperty('Gradient background', False)
app.viewOptions.setProperty('Background color', [1, 1, 1])
app.viewOptions.setProperty('Orientation widget', False)
app.gridObj.setProperty('Color', [0, 0, 0])
app.gridObj.setProperty('Surface Mode', 'Surface with edges')
app.start()
type=str,
default=None,
help='recursively search this directory and load every urdf found')
args, unknown = parser.parse_known_args()
return args
app = mainwindowapp.construct()
view = app.view
args = getArgs()
if args.urdf:
robotModel, jointController = roboturdf.loadRobotModel(urdfFile=args.urdf,
view=view,
useConfigFile=False)
jointNames = robotModel.model.getJointNames()
jointController = jointcontrol.JointController([robotModel],
jointNames=jointNames)
elif args.glob_dir:
urdfFiles = []
for root, dirnames, filenames in os.walk(args.glob_dir):
for filename in fnmatch.filter(filenames, '*.urdf'):
urdfFiles.append(os.path.join(root, filename))
failedFiles = []
for urdfFile in urdfFiles:
from director import playbackpanel
from director import robotplanlistener
from director import robotviewbehaviors
from director import pointcloudlcm
from director import cameraview
from director import lcmUtils
from PythonQt import QtCore, QtGui
import drc as lcmdrc
import bot_core as lcmbotcore
# create the application
app = ConsoleApp()
view = app.createView()
# load robot state model
robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
'robot model', view, colorMode='Textures')
# listen for robot state updates
robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')
# reload urdf from model publisher lcm
roboturdf.startModelPublisherListener([(robotStateModel,
robotStateJointController)])
# load playback model
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
'playback robot model',
view,
color=roboturdf.getRobotOrangeColor(),
visible=False)
from director import segmentation
from director import segmentationroutines
from director import applogic
from director import visualization as vis
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'))
def init(self, view=None, globalsDict=None):
view = view or applogic.getCurrentRenderView()
useRobotState = True
usePerception = True
useFootsteps = True
useHands = True
usePlanning = True
useAtlasDriver = True
useAtlasConvexHull = False
useWidgets = False
directorConfig = drcargs.getDirectorConfig()
neckPitchJoint = 'neck_ay'
colorMode = 'URDF Colors'
if 'colorMode' in directorConfig:
assert directorConfig['colorMode'] in [
'URDF Colors', 'Solid Color', 'Textures'
]
colorMode = directorConfig['colorMode']
if useAtlasDriver:
atlasDriver = atlasdriver.init(None)
if useRobotState:
robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
'robot state model',
view,
urdfFile=directorConfig['urdfConfig']['robotState'],
parent='sensors',
color=roboturdf.getRobotGrayColor(),
visible=True,
colorMode=colorMode)
robotStateJointController.setPose(
'EST_ROBOT_STATE', robotStateJointController.getPose('q_nom'))
roboturdf.startModelPublisherListener([
(robotStateModel, robotStateJointController)
])
robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')
segmentationroutines.SegmentationContext.initWithRobot(
robotStateModel)
if usePerception:
multisenseDriver, mapServerSource = perception.init(view)
def getNeckPitch():
return robotStateJointController.q[
robotstate.getDrakePoseJointNames().index(neckPitchJoint)]
neckDriver = perception.NeckDriver(view, getNeckPitch)
def getSpindleAngleFunction():
if (robotStateJointController.lastRobotStateMessage):
if ('hokuyo_joint' in robotStateJointController.
lastRobotStateMessage.joint_name):
index = robotStateJointController.lastRobotStateMessage.joint_name.index(
'hokuyo_joint')
return (float(robotStateJointController.
lastRobotStateMessage.utime) / (1e6),
robotStateJointController.
lastRobotStateMessage.joint_position[index])
return (0, 0)
spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction)
robotStateModel.connectModelChanged(
spindleMonitor.onRobotStateChanged)
if useHands:
rHandDriver = handdriver.RobotiqHandDriver(side='right')
lHandDriver = handdriver.RobotiqHandDriver(side='left')
if useFootsteps:
footstepsDriver = footstepsdriver.FootstepsDriver(
robotStateJointController)
irisDriver = irisdriver.IRISDriver(robotStateJointController,
footstepsDriver.params)
raycastDriver = raycastdriver.RaycastDriver()
if usePlanning:
ikRobotModel, ikJointController = roboturdf.loadRobotModel(
'ik model',
urdfFile=directorConfig['urdfConfig']['ik'],
parent=None)
om.removeFromObjectModel(ikRobotModel)
ikJointController.addPose('q_end',
ikJointController.getPose('q_nom'))
ikJointController.addPose('q_start',
ikJointController.getPose('q_nom'))
if 'leftFootLink' in directorConfig:
ikServer = ik.AsyncIKCommunicator(
directorConfig['urdfConfig']['ik'],
directorConfig['fixedPointFile'],
directorConfig['leftFootLink'],
directorConfig['rightFootLink'],
directorConfig['pelvisLink'])
else: # assume that robot has no feet e.g. fixed base arm
ikServer = ik.AsyncIKCommunicator(
directorConfig['urdfConfig']['ik'],
directorConfig['fixedPointFile'], '', '', '')
def startIkServer():
ikServer.startServerAsync()
ikServer.comm.writeCommandsToLogFile = True
#startIkServer()
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
'playback model',
view,
urdfFile=directorConfig['urdfConfig']['playback'],
parent='planning',
color=roboturdf.getRobotOrangeColor(),
visible=False,
colorMode=colorMode)
teleopRobotModel, teleopJointController = roboturdf.loadRobotModel(
'teleop model',
view,
urdfFile=directorConfig['urdfConfig']['teleop'],
parent='planning',
color=roboturdf.getRobotBlueColor(),
visible=False,
colorMode=colorMode)
if useAtlasConvexHull:
chullRobotModel, chullJointController = roboturdf.loadRobotModel(
'convex hull atlas',
view,
urdfFile=urdfConfig['chull'],
parent='planning',
color=roboturdf.getRobotOrangeColor(),
visible=False)
playbackJointController.models.append(chullRobotModel)
planPlayback = planplayback.PlanPlayback()
handFactory = roboturdf.HandFactory(robotStateModel)
handModels = []
for side in ['left', 'right']:
if side in handFactory.defaultHandTypes:
handModels.append(handFactory.getLoader(side))
ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel,
ikJointController, handModels)
manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner)
affordanceManager = affordancemanager.AffordanceObjectModelManager(
view)
affordanceitems.MeshAffordanceItem.getMeshManager().initLCM()
affordanceitems.MeshAffordanceItem.getMeshManager(
).collection.sendEchoRequest()
affordanceManager.collection.sendEchoRequest()
segmentation.affordanceManager = affordanceManager
plannerPub = plannerPublisher.PlannerPublisher(
ikPlanner, affordanceManager, ikRobotModel)
ikPlanner.setPublisher(plannerPub)
# This joint angle is mapped to the Multisense panel
neckPitchJoint = ikPlanner.neckPitchJoint
applogic.resetCamera(viewDirection=[-1, 0, 0], view=view)
if useWidgets:
playbackPanel = playbackpanel.PlaybackPanel(
planPlayback, playbackRobotModel, playbackJointController,
robotStateModel, robotStateJointController, manipPlanner)
teleopPanel = teleoppanel.TeleopPanel(
robotStateModel, robotStateJointController, teleopRobotModel,
teleopJointController, ikPlanner, manipPlanner,
playbackPanel.setPlan, playbackPanel.hidePlan)
footstepsDriver.walkingPlanCallback = playbackPanel.setPlan
manipPlanner.connectPlanReceived(playbackPanel.setPlan)
viewBehaviors = None
robotSystemArgs = dict(locals())
for arg in ['globalsDict', 'self']:
del robotSystemArgs[arg]
if globalsDict is not None:
globalsDict.update(robotSystemArgs)
robotSystem = FieldContainer(**robotSystemArgs)
robotSystem.viewBehaviors = robotviewbehaviors.RobotViewBehaviors(
view, robotSystem)
return robotSystem
return args
app = ConsoleApp()
view = app.createView()
args = getArgs()
if args.urdf:
robotModel = roboturdf.openUrdf(args.urdf, view)
jointNames = robotModel.model.getJointNames()
jointController = jointcontrol.JointController([robotModel],
jointNames=jointNames)
else:
robotModel, jointController = roboturdf.loadRobotModel('robot model', view)
print 'urdf file:', robotModel.getProperty('Filename')
for joint in robotModel.model.getJointNames():
print 'joint:', joint
for link in robotModel.model.getLinkNames():
print 'link:', link
robotModel.getLinkFrame(link)
if app.getTestingInteractiveEnabled():
view.show()
app.start()
