def projectDrillDemoInCamera():
q = om.findObjectByName('camera overlay')
om.removeFromObjectModel(q)
imageView = cameraview.views['CAMERA_LEFT']
imageView.imageActor.SetOpacity(.2)
drawFrameInCamera(drillDemo.drill.frame.transform, 'drill frame',visible=False)
tf = transformUtils.copyFrame( drillDemo.drill.frame.transform )
tf.PreMultiply()
tf.Concatenate( drillDemo.drill.drillToButtonTransform )
drawFrameInCamera(tf, 'drill button')
tf2 = transformUtils.copyFrame( tf )
tf2.PreMultiply()
tf2.Concatenate( transformUtils.frameFromPositionAndRPY( [0,0,0] , [180,0,0] ) )
drawFrameInCamera(tf2, 'drill button flip')
drawObjectInCamera('drill',visible=False)
drawObjectInCamera('sensed pointer tip')
obj = om.findObjectByName('sensed pointer tip frame')
if (obj is not None):
drawFrameInCamera(obj.transform, 'sensed pointer tip frame',visible=False)
#drawObjectInCamera('left robotiq',visible=False)
#drawObjectInCamera('right pointer',visible=False)
v = imageView.view
v.render()
def moveDrill(self,Pos=[0,0,0],RPY=[0,0,0],Style='Local'):
linkMap = { 'left' : 'l_hand_face', 'right': 'r_hand_face'}
linkName = linkMap[self.graspingHand]
affordance = om.findObjectByName('drill')
affordanceReach = om.findObjectByName('reach frame')
frame = om.findObjectByName('drill frame')
drillTransform = affordance.actor.GetUserTransform()
reach = transformUtils.copyFrame(affordanceReach.actor.GetUserTransform())
drillTransformCopy = transformUtils.copyFrame(affordance.actor.GetUserTransform())
drillToReach = vtkTransform()
drillToReach.Identity()
drillToReach.PostMultiply()
drillToReach.Concatenate(drillTransformCopy)
drillToReach.Concatenate(reach.GetLinearInverse())
handfaceToWorld = self.ikPlanner.getLinkFrameAtPose(linkName, self.getPlanningStartPose())
# find a transform that move forward wrt hand palm
delta = transformUtils.frameFromPositionAndRPY(Pos, RPY)
drillTransform.Identity()
drillTransform.PostMultiply()
drillTransform.Concatenate(drillToReach)
drillTransform.Concatenate(delta)
drillTransform.Concatenate(handfaceToWorld)
def computeLeftFootOverPlatformFrame(self, startPose, height):
lFoot2World = transformUtils.copyFrame(self.polaris.leftFootEgressOutsideFrame.transform)
rFoot2World = self.robotSystem.ikPlanner.getLinkFrameAtPose('r_foot', startPose)
lFoot2World = transformUtils.copyFrame(rFoot2World)
lFoot2World.PreMultiply()
lFoot2World.Translate([0.05, 0.26, height])
return lFoot2World
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 spawnHandAtCurrentLocation(side='left'):
if (side is 'left'):
tf = transformUtils.copyFrame( getLinkFrame( 'l_hand_face') )
handFactory.placeHandModelWithTransform( tf , app.getCurrentView(), 'left')
else:
tf = transformUtils.copyFrame( getLinkFrame( 'right_pointer_tip') )
handFactory.placeHandModelWithTransform( tf , app.getCurrentView(), 'right')
def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):
print 'planning with safe regions. %d blocks.' % len(blocks)
folder = om.getOrCreateContainer('Safe terrain regions')
om.removeFromObjectModel(folder)
footsteps = []
for i, block in enumerate(blocks):
corners = block.getCorners()
rpy = np.radians(block.cornerTransform.GetOrientation())
#rpy = [0.0, 0.0, 0.0]
self.convertStepToSafeRegion(corners, rpy)
lastBlock = blocks[-1]
goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
goalOffset = vtk.vtkTransform()
goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
goalFrame.PreMultiply()
goalFrame.Concatenate(goalOffset)
goalPosition = np.array(goalFrame.GetPosition())
if len(blocks) > 1:
goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))
vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)
request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)
assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
if standingFootName == self.ikPlanner.rightFootLink:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
else:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT
request.params.leading_foot = leadingFoot
request.params.max_forward_step = 0.5
request.params.nom_forward_step = 0.12
request.params.nom_step_width = 0.22
request.params.max_num_steps = 8 #2*len(blocks)
plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)
if not plan:
return []
#print 'received footstep plan with %d steps.' % len(plan.footsteps)
footsteps = []
for i, footstep in enumerate(plan.footsteps):
footstepTransform = self.transformFromFootstep(footstep)
footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))
return footsteps[2:]
def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):
print 'planning with safe regions. %d blocks.' % len(blocks)
folder = om.getOrCreateContainer('Safe terrain regions')
om.removeFromObjectModel(folder)
footsteps = []
for i, block in enumerate(blocks):
corners = block.getCorners()
rpy = np.radians(block.cornerTransform.GetOrientation())
#rpy = [0.0, 0.0, 0.0]
self.convertStepToSafeRegion(corners, rpy)
lastBlock = blocks[-1]
goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
goalOffset = vtk.vtkTransform()
goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
goalFrame.PreMultiply()
goalFrame.Concatenate(goalOffset)
goalPosition = np.array(goalFrame.GetPosition())
if len(blocks) > 1:
goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))
vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)
request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)
assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
if standingFootName == self.ikPlanner.rightFootLink:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
else:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT
request.params.leading_foot = leadingFoot
request.params.max_forward_step = 0.5
request.params.nom_forward_step = 0.12
request.params.nom_step_width = 0.22
request.params.max_num_steps = 8 #2*len(blocks)
plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)
if not plan:
return []
#print 'received footstep plan with %d steps.' % len(plan.footsteps)
footsteps = []
for i, footstep in enumerate(plan.footsteps):
footstepTransform = self.transformFromFootstep(footstep)
footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))
return footsteps[2:]
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 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 computeRobotStanceFrame(self, objectTransform,
relativeStanceTransform):
'''
Given a robot model, determine the height of the ground
using an XY and Yaw standoff, combined to determine the relative 6DOF standoff
For a grasp or approach stance
'''
groundFrame = self.computeGroundFrame(self.robotModel)
groundHeight = groundFrame.GetPosition()[2]
graspPosition = np.array(objectTransform.GetPosition())
graspYAxis = [0.0, 1.0, 0.0]
graspZAxis = [0.0, 0.0, 1.0]
objectTransform.TransformVector(graspYAxis, graspYAxis)
objectTransform.TransformVector(graspZAxis, graspZAxis)
xaxis = graspYAxis
#xaxis = graspZAxis
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
graspGroundTransform = transformUtils.getTransformFromAxes(
xaxis, yaxis, zaxis)
graspGroundTransform.PostMultiply()
graspGroundTransform.Translate(graspPosition[0], graspPosition[1],
groundHeight)
robotStance = transformUtils.copyFrame(relativeStanceTransform)
robotStance.Concatenate(graspGroundTransform)
return robotStance
def planReach(self):
startPose = self.getPlanningStartPose()
reachFrame = vtk.vtkTransform()
reachFrame.Identity()
reachFrame.PostMultiply()
if self.graspingHand == 'right':
reachFrame.Concatenate(
transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 180, 0]))
reachFrame.Concatenate(
transformUtils.copyFrame(
om.findObjectByName('reach frame').actor.GetUserTransform()))
constraintSet = self.ikPlanner.planEndEffectorGoal(startPose,
self.graspingHand,
reachFrame,
lockBase=False,
lockBack=True)
## broken robot arm has a new joint limit
if self.graspingHand == 'left':
constraintSet.constraints.append(self.createBrokenArmConstraint())
endPose, info = constraintSet.runIk()
#print endPose
if info > 10:
self.log("in Target received: Bad movement")
return
reachPlan = constraintSet.runIkTraj()
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 computeRelativeGraspTransform(self):
t = transformUtils.copyFrame(transformUtils.frameFromPositionAndRPY(self.graspFrameXYZ,
self.graspFrameRPY))
t.PostMultiply()
t.RotateX(180)
t.RotateY(-90)
return t
def computeRobotStanceFrame(self, objectTransform, relativeStanceTransform):
"""
Given a robot model, determine the height of the ground
using an XY and Yaw standoff, combined to determine the relative 6DOF standoff
For a grasp or approach stance
"""
groundFrame = self.computeGroundFrame(self.robotModel)
groundHeight = groundFrame.GetPosition()[2]
graspPosition = np.array(objectTransform.GetPosition())
graspYAxis = [0.0, 1.0, 0.0]
graspZAxis = [0.0, 0.0, 1.0]
objectTransform.TransformVector(graspYAxis, graspYAxis)
objectTransform.TransformVector(graspZAxis, graspZAxis)
xaxis = graspYAxis
# xaxis = graspZAxis
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
graspGroundTransform = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
graspGroundTransform.PostMultiply()
graspGroundTransform.Translate(graspPosition[0], graspPosition[1], groundHeight)
robotStance = transformUtils.copyFrame(relativeStanceTransform)
robotStance.Concatenate(graspGroundTransform)
return robotStance
def getFrameToOriginTransform(self, t):
tCopy = transformUtils.copyFrame(t)
tCopy.PostMultiply()
tCopy.Concatenate(
self.polaris.originFrame.transform.GetLinearInverse())
print transformUtils.poseFromTransform(tCopy)
return tCopy
def flipHandSide():
for obj in [pickedObj] + pickedObj.children():
if not isGraspSeed(obj):
continue
side = 'right' if obj.side == 'left' else 'left'
obj.side = side
color = [1.0, 1.0, 0.0]
if side == 'right':
color = [0.33, 1.0, 0.0]
obj.setProperty('Color', color)
polyData = handFactory.getNewHandPolyData(side)
obj.setPolyData(polyData)
handFrame = obj.children()[0]
t = transformUtils.copyFrame(handFrame.transform)
t.PreMultiply()
t.RotateY(180)
handFrame.copyFrame(t)
objName = obj.getProperty('Name')
frameName = handFrame.getProperty('Name')
if side == 'left':
obj.setProperty('Name', objName.replace("right", "left"))
handFrame.setProperty('Name', frameName.replace("right", "left"))
else:
obj.setProperty('Name', objName.replace("left", "right"))
handFrame.setProperty('Name', frameName.replace("left", "right"))
obj._renderAllViews()
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform()
or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform('local', imageView.imageName,
localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
'''
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
'''
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData,
t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
def getFootstepRelativeTransform(self):
self.footstepsToPlan = []
for n in xrange(1,self.numFootsteps + 1):
stepFrameName = 'step ' + str(n) + ' frame'
fToWorld = transformUtils.copyFrame(om.findObjectByName(stepFrameName).transform)
fToPlan = self.getTransformToPlanningFrame(fToWorld)
self.footstepsToPlan.append(fToPlan)
def run(self):
self.ikPlanner = robotSystem.ikPlanner
side = self.properties.getPropertyEnumValue('Side').lower()
self.graspingHand = side
targetPoints = self.getAnnotationInputPoints()
gazeTargetFrame = self.getGazeTargetFrame()
self.initGazeConstraintSet(gazeTargetFrame)
numberOfSamples = len(targetPoints)
for i in xrange(numberOfSamples):
targetPos = targetPoints[i]
targetFrame = transformUtils.copyFrame(gazeTargetFrame.transform)
targetFrame.Translate(targetPos - np.array(targetFrame.GetPosition()))
self.appendPositionConstraintForTargetFrame(targetFrame, i+1)
gazeConstraint = self.constraintSet.constraints[0]
assert isinstance(gazeConstraint, ikplanner.ik.WorldGazeDirConstraint)
gazeConstraint.tspan = [1.0, numberOfSamples]
plan = self.constraintSet.runIkTraj()
_addPlanItem(plan, '%s gaze plan' % gazeTargetFrame.getProperty('Name'), ManipulationPlanItem)
def planGraspLineMotion(self):
self.turnPointwiseOffSlow()
startPose = self.getPlanningStartPose()
graspFrame = vtk.vtkTransform()
graspFrame.Identity()
graspFrame.PostMultiply()
if self.graspingHand == 'right':
graspFrame.Concatenate(transformUtils.frameFromPositionAndRPY([0,0,0], [0,180,0]))
graspFrame.Concatenate(transformUtils.copyFrame(om.findObjectByName('grasp frame').actor.GetUserTransform()))
constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, graspFrame, \
lockBase=False, lockBack=True)
# constraint orientation
p,q = self.ikPlanner.createPositionOrientationGraspConstraints(self.graspingHand, graspFrame)
q.tspan=[0.5,1]
constraintSet.constraints.append(q)
# constraint line axis
positionConstraint, orientationConstraint, axisConstraint = self.ikPlanner.createMoveOnLineConstraints(startPose, graspFrame)
## broken robot arm has a new joint limit
if self.graspingHand == 'left':
constraintSet.constraints.append(self.createBrokenArmConstraint())
constraintSet.constraints.append(axisConstraint)
constraintSet.constraints[-1].tspan = [0.5,np.inf]
endPose, info = constraintSet.runIk()
#print endPose
if info > 10:
self.log("in Target received: Bad movement")
return
graspPlan = constraintSet.runIkTraj()
def getPinchToPalmFrame(self):
pinchToPalm = transformUtils.copyFrame(self.getPinchToHandFrame())
palmToHand = self.ikPlanner.getPalmToHandLink(side='right')
pinchToPalm.PostMultiply()
pinchToPalm.Concatenate(palmToHand.GetLinearInverse())
return pinchToPalm
def run(self):
self.ikPlanner = robotSystem.ikPlanner
side = self.properties.getPropertyEnumValue('Side').lower()
self.graspingHand = side
targetPoints = self.getAnnotationInputPoints()
gazeTargetFrame = self.getGazeTargetFrame()
self.initGazeConstraintSet(gazeTargetFrame)
numberOfSamples = len(targetPoints)
for i in xrange(numberOfSamples):
targetPos = targetPoints[i]
targetFrame = transformUtils.copyFrame(gazeTargetFrame.transform)
targetFrame.Translate(targetPos -
np.array(targetFrame.GetPosition()))
self.appendPositionConstraintForTargetFrame(targetFrame, i + 1)
gazeConstraint = self.constraintSet.constraints[0]
assert isinstance(gazeConstraint, ikplanner.ik.WorldGazeDirConstraint)
gazeConstraint.tspan = [1.0, numberOfSamples]
plan = self.constraintSet.runIkTraj()
_addPlanItem(plan,
'%s gaze plan' % gazeTargetFrame.getProperty('Name'),
ManipulationPlanItem)
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
def fitSwitchBox(self, polyData, points):
boxPosition = points[0]
wallPoint = points[1]
# find a frame that is aligned with wall
searchRadius = 0.2
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0)
obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False)
obj.setProperty('Point Size', 7)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
origin = segmentation.computeCentroid(planePoints)
zaxis = [0,0,1]
xaxis = normal
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
# translate that frame to the box position
t.PostMultiply()
t.Translate(boxPosition)
boxFrame = transformUtils.copyFrame(t)
self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
def flipHandThumb():
handFrame = pickedObj.children()[0]
t = transformUtils.copyFrame(handFrame.transform)
t.PreMultiply()
t.RotateY(180)
handFrame.copyFrame(t)
pickedObj._renderAllViews()
def getPinchToPalmFrame(self):
pinchToPalm = transformUtils.copyFrame(self.getPinchToHandFrame())
palmToHand = self.ikPlanner.getPalmToHandLink(side='right')
pinchToPalm.PostMultiply()
pinchToPalm.Concatenate(palmToHand.GetLinearInverse())
return pinchToPalm
def updateReachFrame(self):
graspFrame = transformUtils.copyFrame(self.pinchToBox)
boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform
graspFrame.PostMultiply()
graspFrame.Concatenate(boxFrame)
vis.updateFrame(graspFrame, 'pinch reach frame', scale=0.2)
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform('local', imageView.imageName, localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
'''
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
'''
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
def fitSwitchBox(self, polyData, points):
boxPosition = points[0]
wallPoint = points[1]
# find a frame that is aligned with wall
searchRadius = 0.2
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0)
obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False)
obj.setProperty('Point Size', 7)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
origin = segmentation.computeCentroid(planePoints)
zaxis = [0,0,1]
xaxis = normal
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
# translate that frame to the box position
t.PostMultiply()
t.Translate(boxPosition)
boxFrame = transformUtils.copyFrame(t)
self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
def makeStepMessages(self, stepFrames, leadingFoot, snapToTerrain=False):
assert leadingFoot in ('left', 'right')
isRightFootOffset = 0 if leadingFoot == 'left' else 1
footOriginToSole = -np.mean(FootstepsDriver.getContactPts(), axis=0)
stepMessages = []
for i, stepFrame in enumerate(stepFrames):
t = transformUtils.copyFrame(stepFrame)
t.PreMultiply()
t.Translate(footOriginToSole)
step = lcmdrc.footstep_t()
step.pos = transformUtils.positionMessageFromFrame(t)
step.is_right_foot = (i + isRightFootOffset) % 2
step.params = self.footstepsDriver.getDefaultStepParams()
step.fixed_x = True
step.fixed_y = True
step.fixed_z = True
step.fixed_roll = True
step.fixed_pitch = True
step.fixed_yaw = True
if snapToTerrain:
step.fixed_z = False
step.fixed_roll = False
step.fixed_pitch = False
stepMessages.append(step)
return stepMessages
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):
obj, _ = vis.findPickedObject(displayPoint, view)
if not obj:
return False
name = obj.getProperty('Name')
if name in ('footstep widget', 'footstep widget frame'):
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
return True
match = re.match('^step (\d+)$', name)
if not match:
return False
stepIndex = int(match.group(1))
existingWidget = om.findObjectByName('footstep widget')
if existingWidget:
previousStep = existingWidget.stepIndex
print 'have existing widget for step:', stepIndex
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
print 'returning because widget was for selected step'
return True
footMesh = shallowCopy(obj.polyData)
footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)
if useHorizontalWidget:
rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))
footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
footObj.stepIndex = stepIndex
frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
footObj.actor.SetUserTransform(frameObj.transform)
footObj.setProperty('Color', obj.getProperty('Color'))
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
def onFootWidgetChanged(frame):
footstepsDriver.onStepModified(stepIndex - 1, frame)
frameObj.connectFrameModified(onFootWidgetChanged)
return True
def updateReachFrame(self):
graspFrame = transformUtils.copyFrame(self.pinchToBox)
boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform
graspFrame.PostMultiply()
graspFrame.Concatenate(boxFrame)
vis.updateFrame(graspFrame, 'pinch reach frame', scale=0.2)
def computeRelativeGraspTransform(self):
t = transformUtils.copyFrame(
transformUtils.frameFromPositionAndRPY(self.graspFrameXYZ,
self.graspFrameRPY))
t.PostMultiply()
t.RotateX(180)
t.RotateY(-90)
return t
def getFootstepRelativeTransform(self):
self.footstepsToPlan = []
for n in xrange(1, self.numFootsteps + 1):
stepFrameName = 'step ' + str(n) + ' frame'
fToWorld = transformUtils.copyFrame(
om.findObjectByName(stepFrameName).transform)
fToPlan = self.getTransformToPlanningFrame(fToWorld)
self.footstepsToPlan.append(fToPlan)
def computeNextRoomFrame(self):
assert self.targetAffordance
t = transformUtils.frameFromPositionAndRPY(self.nextPosition,
[0, 0, 0])
self.faceTransformLocal = transformUtils.copyFrame(t) # copy required
t.Concatenate(self.targetFrame)
self.faceFrameDesired = vis.showFrame(t,
'target frame desired',
parent=self.targetAffordance,
visible=False,
scale=0.2)
def _computeBaseTransform(self, frame):
currentDelta = None
for frameId, frameData in self.frames.items():
if frameData.ref() is None:
self._removeFrameId(frameId)
elif frameData.ref() is frame:
continue
else:
currentDelta = transformUtils.copyFrame(frameData.baseTransform.GetLinearInverse())
currentDelta.Concatenate(transformUtils.copyFrame(frameData.ref().transform))
break
t = transformUtils.copyFrame(frame.transform)
t.PostMultiply()
if currentDelta:
t.Concatenate(currentDelta.GetLinearInverse())
return t
def cycleEndEffector(self):
if len(self.endEffectorFrames) is not 0:
self.clearKeyframePoses()
self.endEffectorFrames[self.endEffectorIndex].setProperty('Edit', False)
self.endEffectorIndex = (self.endEffectorIndex + 1) % len(self.endEffectorFrames)
self.endEffectorFrames[self.endEffectorIndex].setProperty('Edit', True)
om.setActiveObject(self.endEffectorFrames[self.endEffectorIndex])
self.baseFrame = self.endEffectorFrames[self.endEffectorIndex]
self.baseTransform = transformUtils.copyFrame(self.baseFrame.transform)
self.resetDeltas()
self.addKeyframePose()
def planGraspLift(self):
t3 = transformUtils.frameFromPositionAndRPY( [0.00,0.0, 0.04] , [0,0,0] )
liftTransform = transformUtils.copyFrame(self.board.graspFrame.transform)
liftTransform.Concatenate(t3)
self.board.liftFrame = vis.updateFrame(liftTransform, 'lift frame', parent="affordances", visible=False, scale=0.2)
startPose = self.getPlanningStartPose()
constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, self.board.liftFrame, lockBase=self.lockBase, lockBack=self.lockBack)
endPose, info = constraintSet.runIk()
liftPlan = constraintSet.runIkTraj()
self.addPlan(liftPlan)
def addNewFrame():
t = transformUtils.copyFrame(affordanceObj.getChildFrame().transform)
t.PostMultiply()
t.Translate(np.array(pickedPoint) - np.array(t.GetPosition()))
newFrame = vis.showFrame(
t,
'%s frame %d' %
(affordanceObj.getProperty('Name'), len(affordanceObj.children())),
scale=0.2,
parent=affordanceObj)
affordanceObj.getChildFrame().getFrameSync().addFrame(
newFrame, ignoreIncoming=True)
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 computeTableStanceFrame(self):
assert self.tableData
zGround = 0.0
tableHeight = self.tableData.frame.GetPosition()[2] - zGround
t = transformUtils.copyFrame(self.tableData.frame)
t.PreMultiply()
t1 = transformUtils.frameFromPositionAndRPY([-x/2 for x in self.tableData.dims],[0,0,0])
t.Concatenate(t1)
t2 = transformUtils.frameFromPositionAndRPY([-0.35, self.tableData.dims[1]*0.5, -tableHeight],[0,0,0])
t.Concatenate(t2)
self.tableStanceFrame = vis.showFrame(t, 'table stance frame', parent=self.tableObj, scale=0.2)
def run(self):
aff = self.getSelectedAffordance()
affFrame = aff.getChildFrame().transform
groundFrame = self.getGroundFrame().transform
projectedXYZ = np.hstack([affFrame.GetPosition()[0:2], groundFrame.GetPosition()[2]])
result = transformUtils.copyFrame(affFrame)
result.Translate(projectedXYZ - np.array(result.GetPosition()))
outputName = self.properties.getProperty('Frame output name')
outputName = outputName or '%s ground frame' % aff.getProperty('Name')
vis.updateFrame(result, outputName, scale=0.2)
def cycleEndEffector(self):
if len(self.endEffectorFrames) is not 0:
self.clearKeyframePoses()
self.endEffectorFrames[self.endEffectorIndex].setProperty(
'Edit', False)
self.endEffectorIndex = (self.endEffectorIndex + 1) % len(
self.endEffectorFrames)
self.endEffectorFrames[self.endEffectorIndex].setProperty(
'Edit', True)
om.setActiveObject(self.endEffectorFrames[self.endEffectorIndex])
self.baseFrame = self.endEffectorFrames[self.endEffectorIndex]
self.baseTransform = transformUtils.copyFrame(
self.baseFrame.transform)
self.resetDeltas()
self.addKeyframePose()
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 spawnRunningBoardAffordance(self):
boxDimensions = [0.4, 1.0, 0.05]
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
boxFrame = transformUtils.copyFrame(stanceFrame)
boxFrame.PreMultiply()
boxFrame.Translate(0.0, 0.0, -boxDimensions[2]/2.0)
box = om.findObjectByName('running board')
if not box:
pose = transformUtils.poseFromTransform(boxFrame)
desc = dict(classname='BoxAffordanceItem', Name='running board', Dimensions=boxDimensions, pose=pose)
box = self.robotSystem.affordanceManager.newAffordanceFromDescription(desc)
return box
def drawObjectInCamera(objectName,visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
obj = om.findObjectByName(objectName)
if obj is None:
return
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
objPolyDataOriginal = obj.polyData
pd = objPolyDataOriginal
pd = filterUtils.transformPolyData(pd, objToLocalT)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd)
vis.showPolyData(pd, ('overlay ' + objectName), view=v, color=[0,1,0],parent='camera overlay',visible=visible)
def spawnFootstepFrame(self):
# should snap this to
boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform
goalFrame = transformUtils.copyFrame(self.footToBox)
goalFrame.PostMultiply()
goalFrame.Concatenate(boxFrame)
# translate goal frame to match current robot height
stanceFrame = self.getStanceFrame()
stanceHeight = stanceFrame.GetPosition()[2]
goalHeight = goalFrame.GetPosition()[2]
goalFrame.PreMultiply()
goalFrame.Translate(0.0, 0.0, stanceHeight - goalHeight)
vis.updateFrame(goalFrame, 'switch box stance frame', scale=0.2)