def resolvePackageFilename(filename):
if not packagepath.PackageMap.isPackageUrl(filename):
return filename
if Geometry.PackageMap is None:
import pydrake
m = packagepath.PackageMap()
m.populateFromSearchPaths([pydrake.getDrakePath()])
m.populateFromEnvironment(['DRAKE_PACKAGE_PATH', 'ROS_PACKAGE_PATH'])
Geometry.PackageMap = m
return Geometry.PackageMap.resolveFilename(filename) or filename
robotToBodyOffset=[0.0, -0.035, 0.0, -90, -90.5, 0],
optitrackBodyName='Body 1'):
bodyToWorld = om.findObjectByName(optitrackBodyName + ' frame').transform
robotToBody = transformUtils.frameFromPositionAndRPY(
robotToBodyOffset[:3], robotToBodyOffset[3:])
robotToWorld = transformUtils.concatenateTransforms(
[robotToBody, bodyToWorld])
pos, quat = transformUtils.poseFromTransform(robotToWorld)
rpy = transformUtils.quaternionToRollPitchYaw(quat)
robotSystem.robotStateJointController.q[:6] = np.hstack((pos, rpy))
robotSystem.robotStateJointController.push()
#####################################################
packageMap = packagepath.PackageMap()
packageMap.populateFromSearchPaths(
os.path.join(os.environ['SPARTAN_SOURCE_DIR'], 'models'))
roboturdf.addPathsFromPackageMap(packageMap)
robotSystem = makeRobotSystem(view)
# TODO: move this to director/robotsystem.py as optional feature
if useOptitrackVisualizer:
optitrackVis = OptitrackVisualizer('OPTITRACK_FRAMES')
optitrackVis.setEnabled(True)
applogic.MenuActionToggleHelper('Tools', optitrackVis.name,
optitrackVis.isEnabled,
optitrackVis.setEnabled)
app.addWidgetToDock(robotSystem.teleopPanel.widget,
def getEnvironmentPackagePaths():
packageMap = packagepath.PackageMap()
packageMap.populateFromEnvironment(['ROS_PACKAGE_PATH'])
return list(packageMap.map.values())
