def placeHandModel(displayPoint, view, side='left'):
obj, _ = vis.findPickedObject(displayPoint, view)
if isinstance(obj, vis.FrameItem):
_, handFrame = handFactory.placeHandModelWithTransform(
obj.transform, view, side=side, parent=obj.parent())
handFrame.frameSync = vis.FrameSync()
handFrame.frameSync.addFrame(obj)
handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
return
pickedPointFields = vis.pickPoint(displayPoint,
view,
pickType='cells',
tolerance=0.0)
pickedPoint = pickedPointFields.pickedPoint
prop = pickedPointFields.pickedProp
obj = vis.getObjectByProp(prop)
if not obj:
return
yaxis = -normal
zaxis = [0, 0, 1]
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(-zaxis, yaxis, xaxis)
t.PostMultiply()
t.Translate(pickedPoint)
if side == 'right':
t.PreMultiply()
t.RotateY(180)
handObj, handFrame = handFactory.placeHandModelWithTransform(t,
view,
side=side,
parent=obj)
syncFrame = getChildFrame(obj)
if syncFrame:
handFrame.frameSync = vis.FrameSync()
handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
handFrame.frameSync.addFrame(syncFrame)
def __init__(self):
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
self.pointcloud = ioUtils.readPolyData(director.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp')
self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None)
segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0, 0. , 0. , 0.0])))
self.originFrame = self.pointcloudPD.getChildFrame()
t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625, 0. , 0. , -0.34604951]))
self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-3.31840048, 0.36408685, -0.67413123]), array([ 0.93449475, 0. , 0. , -0.35597691]))
self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004, 0. , 0. , -0.34940972]))
self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572, 0. , 0. , 0.91450893]))
self.drillWallWalkFarthestSafeFrame = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519, 0. , 0. , -0.16124022]))
self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-1.16122318, 0.04723203, -0.67493468]), array([ 0.93163145, 0. , 0. , -0.36340451]))
self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497, 0. , 0. , -0.53906374]))
self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075, 0. , 0. , -0.16525575]))
self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977, 0. , 0. , -0.3299461 ]))
self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1., 0., 0., 0.]))
self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
def run(self):
inputFrame = self.getInputFrame()
translation = self.properties.getProperty('Translation')
rpy = self.properties.getProperty('Rotation')
offset = transformUtils.frameFromPositionAndRPY(translation, rpy)
offset.PostMultiply()
offset.Concatenate(transformUtils.copyFrame(inputFrame.transform))
outputFrame = vis.updateFrame(offset, self.properties.getProperty('Frame output name'), scale=0.2, parent=inputFrame.parent())
if not hasattr(inputFrame, 'frameSync'):
inputFrame.frameSync = vis.FrameSync()
inputFrame.frameSync.addFrame(inputFrame)
inputFrame.frameSync.addFrame(outputFrame, ignoreIncoming=True)
def graspAffordance(self, affordanceName, side):
if affordanceName in self.frameSyncs:
return
affordanceFrame = self.getAffordanceFrame(affordanceName)
# linkName = 'l_hand' if side == 'left' else 'r_hand'
linkName = self.ikPlanner.getHandLink(side)
linkFrame = self.updateLinkFrame(self.robotModel, linkName)
frameSync = vis.FrameSync()
frameSync.addFrame(linkFrame)
frameSync.addFrame(affordanceFrame, ignoreIncoming=True)
self.frameSyncs[affordanceName] = frameSync
self.attachedAffordances[affordanceName] = linkName
def findBoardAffordance(self):
self.affordance = om.findObjectByName('board')
self.frame = om.findObjectByName('board frame')
self.boardLength = self.affordance.params.get('zwidth')
# so as not to grasp at the tips:
graspLength = self.boardLength / 2 - 0.05
if self.b.val:
self.graspLeftHandFrameXYZ = [-0.045, 0.0, -graspLength]
self.graspLeftHandFrameRPY = [0, -90, -90]
self.graspRightHandFrameXYZ = [-0.045, 0.0, graspLength]
self.graspRightHandFrameRPY = [0, -90, -90]
else:
self.graspLeftHandFrameXYZ = [-0.045, 0.0, -graspLength]
self.graspLeftHandFrameRPY = [0, 90, -90]
self.graspRightHandFrameXYZ = [-0.045, 0.0, graspLength]
self.graspRightHandFrameRPY = [0, 90, -90]
self.relativeStanceXYZ = [-0.6, -graspLength, 0.0]
self.relativeAsymmetricStanceXYZ = [-0.6, -2 * graspLength, 0.0]
self.relativeStanceRPY = [0, 0, 0]
self.reachDepth = 0.12 # depth to reach to before going for grasp
self.computeBoardGraspFrames()
self.computeBoardReachFrames()
self.computeBoardStanceFrame()
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.frame)
self.frameSync.addFrame(self.graspLeftFrame)
self.frameSync.addFrame(self.graspRightFrame)
self.frameSync.addFrame(self.reachLeftFrame)
self.frameSync.addFrame(self.reachRightFrame)
self.frameSync.addFrame(self.stanceFrame)
def main():
om.init(QtGui.QTreeWidget(), PythonQt.dd.ddPropertiesPanel())
# create a frame
t1 = vtk.vtkTransform()
f1 = vis.FrameItem('frame 1', t1, view=None)
om.addToObjectModel(f1)
# test object model lookup
assert om.findObjectByName('frame 1')
assert om.findObjectByName('frame 2') is None
# test reference cleanup
f1Ref = weakref.ref(f1)
assert f1Ref() is f1
om.removeFromObjectModel(f1)
del f1
assert f1Ref() is None
# add frame again
f1 = vis.FrameItem('frame 1', t1, view=None)
om.addToObjectModel(f1)
# create second frame
t2 = vtk.vtkTransform()
f2 = vis.FrameItem('frame 2', t2, view=None)
om.addToObjectModel(f2)
# test transform reference is input transform
assert f2.transform is t2
# test frame sync
frameSync = vis.FrameSync()
frameSync.addFrame(f1)
frameSync.addFrame(f2)
t1.Translate(10, 0, 0)
t1.Modified()
assert t2.GetPosition() == (10.0, 0.0, 0.0)
# test frame sync cleanup
f1Ref = weakref.ref(f1)
assert f1Ref() is f1
assert len(frameSync.frames) == 2
om.removeFromObjectModel(f1)
del f1
assert f1Ref() is None
assert len(frameSync.frames) == 2
t2.Translate(10, 0, 0)
t2.Modified()
assert t2.GetPosition() == (20.0, 0.0, 0.0)
assert len(frameSync.frames) == 1
# add frame again
f1 = vis.FrameItem('frame 1', t1, view=None)
om.addToObjectModel(f1)
frameSync.addFrame(f1)
t1.Translate(0, 5, 0)
t1.Modified()
assert t1.GetPosition() == (10.0, 5.0, 0.0)
assert t2.GetPosition() == (20.0, 5.0, 0.0)
frameSync.removeFrame(f1)
t1.Translate(0, 5, 0)
t1.Modified()
assert t1.GetPosition() == (10.0, 10.0, 0.0)
assert t2.GetPosition() == (20.0, 5.0, 0.0)
# this has to be wrapped in a function, otherwise the exception
# handling holds a reference to the FrameSync object which breaks
# the delete test at the end
def testException(fs):
try:
fs.removeFrame('test')
except KeyError:
pass
else:
assert False
testException(frameSync)
# test cleanup
f1Ref = weakref.ref(f1)
om.removeFromObjectModel(f1)
del f1
assert f1Ref() is None
t1Ref = weakref.ref(t1)
del t1
assert t1Ref() is None
# add frame again
t1 = vtk.vtkTransform()
f1 = vis.FrameItem('frame 1', t1, view=None)
om.addToObjectModel(f1)
frameSync.addFrame(f1)
t1.Translate(0, 0, 10)
t1.Modified()
assert t2.GetPosition() == (20.0, 5.0, 10.0)
# verify FrameSync object can be deleted
frameSyncRef = weakref.ref(frameSync)
del frameSync
assert frameSyncRef() is None
# verify frames are no longer synced after FrameSync is deleted
t1.Translate(0, 0, 10)
t1.Modified()
assert t2.GetPosition() == (20.0, 5.0, 10.0)
sys.exit(0)
def __init__(self):
self.aprilTagSubsciber = lcmUtils.addSubscriber('APRIL_TAG_TO_CAMERA_LEFT', lcmbotcore.rigid_transform_t, self.onAprilTag)
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
desc = dict(classname='MeshAffordanceItem', Name='polaris',
Filename='software/models/polaris/polaris_cropped.vtp', pose=pose)
self.pointcloudAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
self.originFrame = self.pointcloudAffordance.getChildFrame()
self.originToAprilTransform = transformUtils.transformFromPose(np.array([-0.038508 , -0.00282131, -0.01000079]),
np.array([ 9.99997498e-01, -2.10472556e-03, -1.33815696e-04, 7.46246794e-04])) # offset for . . . who knows why
# t = transformUtils.transformFromPose(np.array([ 0.14376024, 0.95920689, 0.36655712]), np.array([ 0.28745842, 0.90741428, -0.28822068, 0.10438304]))
t = transformUtils.transformFromPose(np.array([ 0.10873244, 0.93162364, 0.40509084]),
np.array([ 0.32997378, 0.88498408, -0.31780588, 0.08318602]))
self.leftFootEgressStartFrame = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.265, 0.874, 0.274]),
np.array([ 0.35290731, 0.93443693, -0.04181263, 0.02314636]))
self.leftFootEgressMidFrame = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.54339115, 0.89436275, 0.26681047]),
np.array([ 0.34635985, 0.93680077, -0.04152008, 0.02674412]))
self.leftFootEgressOutsideFrame = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.pointcloudAffordance)
# pose = [np.array([-0.78962299, 0.44284877, -0.29539116]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] #old location
# pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] # updated location
pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.53047159, 0.46554963, -0.48086192, 0.52022615])] # update orientation
desc = dict(classname='CapsuleRingAffordanceItem', Name='Steering Wheel', uuid=newUUID(), pose=pose,
Color=[1, 0, 0], Radius=float(0.18), Segments=20)
self.steeringWheelAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
pose = [np.array([-0.05907324, 0.80460545, 0.45439687]), np.array([ 0.14288327, 0.685944 , -0.703969 , 0.11615873])]
desc = dict(classname='BoxAffordanceItem', Name='pedal', Dimensions=[0.12, 0.33, 0.04], pose=pose, Color=[0,1,0])
self.pedalAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
# t = transformUtils.transformFromPose(np.array([ 0.04045136, 0.96565326, 0.25810111]),
# np.array([ 0.26484648, 0.88360091, -0.37065556, -0.10825996]))
# t = transformUtils.transformFromPose(np.array([ -4.34908919e-04, 9.24901627e-01, 2.65614116e-01]),
# np.array([ 0.25022251, 0.913271 , -0.32136359, -0.00708626]))
t = transformUtils.transformFromPose(np.array([ 0.0384547 , 0.89273742, 0.24140762]),
np.array([ 0.26331831, 0.915796 , -0.28055337, 0.11519963]))
self.leftFootPedalSwingFrame = vis.updateFrame(t,'left foot pedal swing', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.9012598 , -0.05709763, 0.34897024]),
np.array([ 0.03879584, 0.98950919, 0.03820214, 0.13381721]))
self.leftFootDrivingFrame = vis.updateFrame(t,'left foot driving', scale=0.2, visible=True, parent=self.pointcloudAffordance)
# t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]),
# np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529]))
# t = transformUtils.transformFromPose(np.array([-0.14940375, 0.90347275, 0.23812658]),
# np.array([ 0.27150909, 0.91398724, -0.28877386, 0.0867167 ]))
# t = transformUtils.transformFromPose(np.array([-0.17331227, 0.87879312, 0.25645152]),
# np.array([ 0.26344489, 0.91567196, -0.28089824, 0.11505581]))
# self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]),
np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529]))
self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.13194951, 0.89871423, 0.24956246]),
np.array([ 0.21589082, 0.91727326, -0.30088849, 0.14651633]))
self.leftFootOnPedal = vis.updateFrame(t,'left foot on pedal', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.17712239, 0.87619935, 0.27001509]),
np.array([ 0.33484372, 0.88280787, -0.31946488, 0.08044963]))
self.leftFootUpFrame = vis.updateFrame(t,'left foot up frame', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.47214939, -0.04856998, 0.01375837]),
np.array([ 6.10521653e-03, 4.18621358e-04, 4.65520611e-01,
8.85015882e-01]))
self.rightHandGrabFrame = vis.updateFrame(t,'right hand grab bar', scale=0.2, visible=True, parent=self.pointcloudAffordance)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.steeringWheelAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.pedalAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootPedalSwingFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootDrivingFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootDrivingKneeInFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootOnPedal, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootUpFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.rightHandGrabFrame, ignoreIncoming=True)
def reachToFrame(frameObj, side, collisionObj):
goalFrame = teleoppanel.panel.endEffectorTeleop.newReachTeleop(
frameObj.transform, side, collisionObj)
goalFrame.frameSync = vis.FrameSync()
goalFrame.frameSync.addFrame(goalFrame, ignoreIncoming=True)
goalFrame.frameSync.addFrame(frameObj)
