def computeBoardStanceFrame(self): objectTransform = transformUtils.copyFrame( self.graspLeftFrame.transform) self.relativeStanceTransform = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY(self.relativeStanceXYZ, self.relativeStanceRPY)) robotStance = self.b.computeRobotStanceFrame( objectTransform, self.relativeStanceTransform) self.stanceFrame = vis.updateFrame(robotStance, 'board stance', parent=self.affordance, visible=False, scale=0.2) self.stanceFrame.addToView(app.getDRCView()) objectTransform = transformUtils.copyFrame( self.graspLeftFrame.transform) self.relativeAsymmetricStanceTransform = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY( self.relativeAsymmetricStanceXYZ, self.relativeStanceRPY)) robotAsymmetricStance = self.b.computeRobotStanceFrame( objectTransform, self.relativeAsymmetricStanceTransform) self.asymmetricStanceFrame = vis.updateFrame(robotAsymmetricStance, 'board Asymmetric stance', parent=self.affordance, visible=False, scale=0.2) self.asymmetricStanceFrame.addToView(app.getDRCView())
def computeBoardReachFrames(self): ''' Reach ~10cm short of the grasp frame ''' reachLeftXYZ = copy.deepcopy(self.graspLeftHandFrameXYZ) reachLeftXYZ[0] = reachLeftXYZ[0] - self.reachDepth reachLeftRPY = copy.deepcopy(self.graspLeftHandFrameRPY) tl = transformUtils.frameFromPositionAndRPY(reachLeftXYZ, reachLeftRPY) tl.Concatenate(self.frame.transform) self.reachLeftFrame = vis.updateFrame(tl, 'reach left frame', parent=self.affordance, visible=False, scale=0.2) self.reachLeftFrame.addToView(app.getDRCView()) reachRightXYZ = copy.deepcopy(self.graspRightHandFrameXYZ) reachRightXYZ[0] = reachRightXYZ[0] - self.reachDepth reachRightRPY = copy.deepcopy(self.graspRightHandFrameRPY) tr = transformUtils.frameFromPositionAndRPY(reachRightXYZ, reachRightRPY) tr.Concatenate(self.frame.transform) self.reachRightFrame = vis.updateFrame(tr, 'reach right frame', parent=self.affordance, visible=False, scale=0.2) self.reachRightFrame.addToView(app.getDRCView())
def computeBoardStanceFrame(self): objectTransform = transformUtils.copyFrame( self.graspLeftFrame.transform ) self.relativeStanceTransform = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY( self.relativeStanceXYZ , self.relativeStanceRPY ) ) robotStance = self.b.computeRobotStanceFrame( objectTransform, self.relativeStanceTransform ) self.stanceFrame = vis.updateFrame(robotStance, 'board stance', parent=self.affordance, visible=False, scale=0.2) self.stanceFrame.addToView(app.getDRCView()) objectTransform = transformUtils.copyFrame( self.graspLeftFrame.transform ) self.relativeAsymmetricStanceTransform = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY( self.relativeAsymmetricStanceXYZ , self.relativeStanceRPY ) ) robotAsymmetricStance = self.b.computeRobotStanceFrame( objectTransform, self.relativeAsymmetricStanceTransform ) self.asymmetricStanceFrame = vis.updateFrame(robotAsymmetricStance, 'board Asymmetric stance', parent=self.affordance, visible=False, scale=0.2) self.asymmetricStanceFrame.addToView(app.getDRCView())
def computeBoardGraspFrames(self): t = transformUtils.frameFromPositionAndRPY( self.graspLeftHandFrameXYZ , self.graspLeftHandFrameRPY ) t_copy = transformUtils.copyFrame(t) t_copy.Concatenate(self.frame.transform) self.graspLeftFrame = vis.updateFrame(t_copy, 'grasp left frame', parent=self.affordance, visible=False, scale=0.2) self.graspLeftFrame.addToView(app.getDRCView()) t = transformUtils.frameFromPositionAndRPY( self.graspRightHandFrameXYZ , self.graspRightHandFrameRPY ) t_copy = transformUtils.copyFrame(t) t_copy.Concatenate(self.frame.transform) self.graspRightFrame = vis.updateFrame(t_copy, 'grasp right frame', parent=self.affordance, visible=False, scale=0.2) self.graspRightFrame.addToView(app.getDRCView())
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 computeBoardReachFrames(self): ''' Reach ~10cm short of the grasp frame ''' reachLeftXYZ = copy.deepcopy( self.graspLeftHandFrameXYZ ) reachLeftXYZ[0] = reachLeftXYZ[0] - self.reachDepth reachLeftRPY = copy.deepcopy ( self.graspLeftHandFrameRPY ) tl = transformUtils.frameFromPositionAndRPY( reachLeftXYZ , reachLeftRPY ) tl.Concatenate(self.frame.transform) self.reachLeftFrame = vis.updateFrame(tl, 'reach left frame', parent=self.affordance, visible=False, scale=0.2) self.reachLeftFrame.addToView(app.getDRCView()) reachRightXYZ = copy.deepcopy( self.graspRightHandFrameXYZ ) reachRightXYZ[0] = reachRightXYZ[0] - self.reachDepth reachRightRPY = copy.deepcopy ( self.graspRightHandFrameRPY ) tr = transformUtils.frameFromPositionAndRPY( reachRightXYZ , reachRightRPY ) tr.Concatenate(self.frame.transform) self.reachRightFrame = vis.updateFrame(tr, 'reach right frame', parent=self.affordance, visible=False, scale=0.2) self.reachRightFrame.addToView(app.getDRCView())
def computeBoardGraspFrames(self): t = transformUtils.frameFromPositionAndRPY(self.graspLeftHandFrameXYZ, self.graspLeftHandFrameRPY) t_copy = transformUtils.copyFrame(t) t_copy.Concatenate(self.frame.transform) self.graspLeftFrame = vis.updateFrame(t_copy, 'grasp left frame', parent=self.affordance, visible=False, scale=0.2) self.graspLeftFrame.addToView(app.getDRCView()) t = transformUtils.frameFromPositionAndRPY(self.graspRightHandFrameXYZ, self.graspRightHandFrameRPY) t_copy = transformUtils.copyFrame(t) t_copy.Concatenate(self.frame.transform) self.graspRightFrame = vis.updateFrame(t_copy, 'grasp right frame', parent=self.affordance, visible=False, scale=0.2) self.graspRightFrame.addToView(app.getDRCView())
import functools import math import numpy as np from director.debugVis import DebugData from director import ioUtils as io drcargs.requireStrict() drcargs.args() app.startup(globals()) om.init(app.getMainWindow().objectTree(), app.getMainWindow().propertiesPanel()) actionhandlers.init() quit = app.quit exit = quit view = app.getDRCView() camera = view.camera() tree = app.getMainWindow().objectTree() orbit = cameracontrol.OrbitController(view) showPolyData = segmentation.showPolyData updatePolyData = segmentation.updatePolyData ############################################################################### robotSystem = robotsystem.create(view) globals().update(dict(robotSystem)) useIk = True
import math import numpy as np from director.debugVis import DebugData from director import ioUtils as io drcargs.requireStrict() drcargs.args() app.startup(globals()) om.init(app.getMainWindow().objectTree(), app.getMainWindow().propertiesPanel()) actionhandlers.init() quit = app.quit exit = quit view = app.getDRCView() camera = view.camera() tree = app.getMainWindow().objectTree() orbit = cameracontrol.OrbitController(view) showPolyData = segmentation.showPolyData updatePolyData = segmentation.updatePolyData ############################################################################### robotSystem = robotsystem.create(view) globals().update(dict(robotSystem)) useIk = True useRobotState = True usePerception = True useGrid = True