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 computeStanceFrame(self, useIkTraj=False): objectTransform = transformUtils.copyFrame( self.computeGraspFrame().transform) if useIkTraj: startPose = self.getNominalPose() plan = self.planInsertTraj(self.speedLow, lockFeet=False, lockBase=False, resetPoses=True, startPose=startPose) stancePose = robotstate.convertStateMessageToDrakePose( plan.plan[0]) stanceRobotModel = self.ikPlanner.getRobotModelAtPose(stancePose) self.nominalPelvisXYZ = stancePose[:3] robotStance = self.footstepPlanner.getFeetMidPoint( stanceRobotModel) else: robotStance = self.computeRobotStanceFrame( objectTransform, self.computeRelativeStanceTransform()) stanceFrame = vis.updateFrame(robotStance, 'valve grasp stance', parent=self.getValveAffordance(), visible=False, scale=0.2) stanceFrame.addToView(app.getDRCView()) return stanceFrame
def computeGraspFrame(self): t = self.computeRelativeGraspTransform() t.Concatenate(self.getValveAffordance().getChildFrame().transform) graspFrame = vis.updateFrame(t, 'valve grasp frame', parent=self.getValveAffordance(), visible=False, scale=0.2) graspFrame.addToView(app.getDRCView()) return graspFrame
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 showImageOverlay(self, size=400, viewName='CAMERALHAND'): self.imageView = self.cameraview.views[viewName] _prevParent = self.imageView.view.parent() self.imageView.view.hide() view = app.getDRCView() self.imageView.view.setParent(view) self.imageView.view.resize(size, size) self.imageView.view.move(0, 0) self.imageView.view.show() self.imagePicker.start()
def showImageOverlay(self, size=400, viewName='CAMERALHAND'): self.imageView = self.cameraview.views[viewName] _prevParent = self.imageView.view.parent() self.imageView.view.hide() view = app.getDRCView() self.imageView.view.setParent(view) self.imageView.view.resize(size, size) self.imageView.view.move(0,0) self.imageView.view.show() self.imagePicker.start()
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 computeStanceFrame(self, useIkTraj=False): objectTransform = transformUtils.copyFrame(self.computeGraspFrame().transform) if useIkTraj: startPose = self.getNominalPose() plan = self.planInsertTraj(self.speedLow, lockFeet=False, lockBase=False, resetPoses=True, startPose=startPose) stancePose = robotstate.convertStateMessageToDrakePose(plan.plan[0]) stanceRobotModel = self.ikPlanner.getRobotModelAtPose(stancePose) self.nominalPelvisXYZ = stancePose[:3] robotStance = self.footstepPlanner.getFeetMidPoint(stanceRobotModel) else: robotStance = self.computeRobotStanceFrame(objectTransform, self.computeRelativeStanceTransform()) stanceFrame = vis.updateFrame(robotStance, 'valve grasp stance', parent=self.getValveAffordance(), visible=False, scale=0.2) stanceFrame.addToView(app.getDRCView()) return stanceFrame
import functools import math import numpy as np from ddapp.debugVis import DebugData from ddapp 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