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 __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 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
