def planDeltaMove(self, Direction, LocalOrWorld, Amount): linkMap = { 'left' : 'l_hand_face', 'right': 'r_hand_face'} linkName = linkMap[self.graspingHand] #handToWorld = self.robotModel.getLinkFrame(linkName) if Direction == 'X': delta = transformUtils.frameFromPositionAndRPY([Amount,0,0],[0,0,0]) elif Direction == 'Y': delta = transformUtils.frameFromPositionAndRPY([0,Amount,0],[0,0,0]) else: delta = transformUtils.frameFromPositionAndRPY([0,0,Amount],[0,0,0]) startPose = self.getPlanningStartPose() constraintSet = self.ikPlanner.planEndEffectorDelta(startPose, self.graspingHand, delta.GetPosition(), constraints=None, LocalOrWorldDelta=LocalOrWorld) handfaceToWorld = self.ikPlanner.getLinkFrameAtPose(linkName, self.getPlanningStartPose()) # constraint orientation p,q = self.ikPlanner.createPositionOrientationGraspConstraints(self.graspingHand,handfaceToWorld) q.tspan=[0.5,np.inf] constraintSet.constraints.append(q) ## endPose, info = constraintSet.runIk() if info>10: return None graspPlan = constraintSet.runIkTraj() return graspPlan
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 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 spawnValveAffordance(self): radius = 0.10 tubeRadius = 0.02 position = [0, 0, 1.2] rpy = [0, 0, 0] t_feet_mid = self.footstepPlanner.getFeetMidPoint(self.robotModel) t = transformUtils.frameFromPositionAndRPY(position, rpy) t_grasp = self.computeRelativeGraspTransform() t_grasp.Concatenate(t) t_stance = self.computeRobotStanceFrame(t_grasp, self.computeRelativeStanceTransform()) t_valve = t_stance.GetInverse() # This is necessary to get the inversion to actually happen. We don't know why. t_valve.GetMatrix() t_valve.Concatenate(t) t_valve.Concatenate(t_feet_mid) pose = transformUtils.poseFromTransform(t_valve) desc = dict(classname='CapsuleRingAffordanceItem', Name='valve', uuid=newUUID(), pose=pose, Color=[0, 1, 0], Radius=float(radius), Segments=20) desc['Tube Radius'] = tubeRadius import affordancepanel obj = affordancepanel.panel.affordanceFromDescription(desc) obj.params = dict(radius=radius)
def computeRelativeGraspTransform(self): t = transformUtils.copyFrame(transformUtils.frameFromPositionAndRPY(self.graspFrameXYZ, self.graspFrameRPY)) t.PostMultiply() t.RotateX(180) t.RotateY(-90) return t
def spawnBoardAffordance(self, randomize=False): self.boardLength = 1.5 if randomize: position = [random.uniform(0.5, 0.8), random.uniform(-0.2, 0.2), random.uniform(0.5, 0.8)] rpy = [random.choice((random.uniform(-35, 35), random.uniform(70, 110))), random.uniform(-10, 10), random.uniform(-5, 5)] zwidth = random.uniform(0.5, 1.0) else: if self.b.val: position = [0.4, 0.0, 1.] else: position = [0.6, 0.0, 1.] rpy = [90, 1, 0] zwidth = self.boardLength xwidth = 3.75 * .0254 ywidth = 1.75 * .0254 t = transformUtils.frameFromPositionAndRPY(position, rpy) t.Concatenate(self.b.computeGroundFrame(self.b.robotModel)) xaxis = [1,0,0] yaxis = [0,1,0] zaxis = [0,0,1] for axis in (xaxis, yaxis, zaxis): t.TransformVector(axis, axis) self.affordance = segmentation.createBlockAffordance(t.GetPosition(), xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, 'board', parent='affordances') self.affordance.setProperty('Color', QtGui.QColor(200, 150, 100)) t = self.affordance.actor.GetUserTransform() self.frame = vis.showFrame(t, 'board frame', parent=self.affordance, visible=False, scale=0.2)
def makeDebugRegions(self): stepWidth = (15 + 3/8.0) * 0.0254 stepLength = (15 + 5/8.0) * 0.0254 stepHeight = (5 + 5/8.0) * 0.0254 stepPoints = np.array([ [-stepLength/2.0, -stepWidth/2.0, 0.0], [-stepLength/2.0, stepWidth/2.0, 0.0], [stepLength/2.0, stepWidth/2.0, 0.0], [stepLength/2.0, -stepWidth/2.0, 0.0] ]) t = vtk.vtkTransform() t.Translate(0.0, 0.0, 0.0) t.RotateZ(4.5) for i in xrange(len(stepPoints)): stepPoints[i] = np.array(t.TransformPoint(stepPoints[i])) stepOffset = np.array([stepLength, 0.0, stepHeight]) numSteps = 5 goalFrame = transformUtils.frameFromPositionAndRPY([0.4, 0.0, 0.1], [0,0,0]) vis.showFrame(goalFrame, 'goal frame', scale=0.2) rpySeed = np.radians(goalFrame.GetOrientation()) for i in xrange(numSteps): step = stepPoints + (i+1)*stepOffset self.convertStepToSafeRegion(step, rpySeed) self.footstepsPanel.onNewWalkingGoal(goalFrame)
def getSpawnFrame(self): pos = self.jointController.q[:3] rpy = np.degrees(self.jointController.q[3:6]) frame = transformUtils.frameFromPositionAndRPY(pos, rpy) frame.PreMultiply() frame.Translate(0.5, 0.0, 0.3) return frame
def findFarRightCorner(self, polyData, linkFrame): ''' Within a point cloud find the point to the far right from the link The input is typically the 4 corners of a minimum bounding box ''' diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45]) diagonalTransform.Concatenate(linkFrame) vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False) #polyData = shallowCopy(polyData) points = vtkNumpy.getNumpyFromVtk(polyData, 'Points') #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x') #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y') #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z') viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0]) viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0]) viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0]) viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0]) #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x') polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y') #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z') vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False) farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin() points = vtkNumpy.getNumpyFromVtk(polyData, 'Points') return points[farRightIndex,:]
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 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 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 makeGoalFrames(self): for linkName, positionGoal, _ in self.positionCosts: orientationGoal = [0.0, 0.0, 0.0] t = transformUtils.frameFromPositionAndRPY(positionGoal, orientationGoal) f = vis.showFrame(t, '%s position goal' % linkName) f.connectFrameModified(self.onGoalFrameModified)
def planStandUp(self): startPose = self.getPlanningStartPose() startPoseName = 'q_egress_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_egress_end' pelvisFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('pelvis', startPose) t = transformUtils.frameFromPositionAndRPY([self.pelvisLiftX, 0, self.pelvisLiftZ], [0, 0, 0]) liftFrame = transformUtils.concatenateTransforms([t, pelvisFrame]) constraints = [] utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose) g = self.createUtorsoGazeConstraints([1.0, 1.0]) constraints.append(g[1]) p = ik.PositionConstraint(linkName='pelvis', referenceFrame=liftFrame, lowerBound=np.array([0.0, -np.inf, 0.0]), upperBound=np.array([np.inf, np.inf, 0.0])) constraints.append(p) constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=self.quasiStaticShrinkFactor)) constraints.append(self.robotSystem.ikPlanner.createXYZMovingBasePostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName)) constraints.extend(self.robotSystem.ikPlanner.createFixedFootConstraints(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5])) constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseMetersPerSecond=0.02) constraintSet.runIk() keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=True) poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan) ts = [poseTimes[0]] supportsList = [['r_foot', 'l_foot']] plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True) self.addPlan(plan) return plan
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 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 __init__(self, jointController): self.jointController = jointController pos, rpy = jointController.q[:3], jointController.q[3:6] t = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy)) self.frame = vis.showFrame(t, 'mover widget', scale=0.3) self.frame.setProperty('Edit', True) self.frame.connectFrameModified(self.onFrameModified)
def spawnValveAffordance(self): radius = 0.10 tubeRadius = 0.02 position = [0, 0, 1.2] rpy = [0, 0, 0] t_feet_mid = self.footstepPlanner.getFeetMidPoint(self.robotModel) t = transformUtils.frameFromPositionAndRPY(position, rpy) t_grasp = self.computeRelativeGraspTransform() t_grasp.Concatenate(t) t_stance = self.computeRobotStanceFrame( t_grasp, self.computeRelativeStanceTransform()) t_valve = t_stance.GetInverse() # This is necessary to get the inversion to actually happen. We don't know why. t_valve.GetMatrix() t_valve.Concatenate(t) t_valve.Concatenate(t_feet_mid) pose = transformUtils.poseFromTransform(t_valve) desc = dict(classname='CapsuleRingAffordanceItem', Name='valve', uuid=newUUID(), pose=pose, Color=[0, 1, 0], Radius=float(radius), Segments=20) desc['Tube Radius'] = tubeRadius import affordancepanel obj = affordancepanel.panel.affordanceFromDescription(desc) obj.params = dict(radius=radius)
def computeRelativeGraspTransform(self): t = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY(self.graspFrameXYZ, self.graspFrameRPY)) t.PostMultiply() t.RotateX(180) t.RotateY(-90) return t
def getRoomSweepFrames(self, rotateHandFrame=False): topFrame = transformUtils.frameFromPositionAndRPY([0.65, 0.0, 0.8], [160, 0, 90]) yawFrame = transformUtils.frameFromPositionAndRPY( [0, 0.0, 0], [0, 0, self.currentYawDegrees]) if rotateHandFrame: fixHandFrame = transformUtils.frameFromPositionAndRPY([0, 0.0, 0], [0, -90, 0]) topFrame.PreMultiply() topFrame.Concatenate(fixHandFrame) topFrame.PostMultiply() topFrame.Concatenate(yawFrame) bottomFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.0, 0.4], [210, 0, 90]) yawFrame = transformUtils.frameFromPositionAndRPY( [0, 0.0, 0], [0, 0, self.currentYawDegrees]) if rotateHandFrame: bottomFrame.PreMultiply() bottomFrame.Concatenate(fixHandFrame) bottomFrame.PostMultiply() bottomFrame.Concatenate(yawFrame) if (self.fromTop): self.startFrame = vis.showFrame(topFrame, 'frame start', visible=False, scale=0.1, parent=self.mapFolder) self.endFrame = vis.showFrame(bottomFrame, 'frame end', visible=False, scale=0.1, parent=self.mapFolder) else: self.startFrame = vis.showFrame(bottomFrame, 'frame start', visible=False, scale=0.1, parent=self.mapFolder) self.endFrame = vis.showFrame(topFrame, 'frame end', visible=False, scale=0.1, parent=self.mapFolder)
def planFootOut(self, startPose=None, finalFootHeight=0.05): if startPose is None: startPose = self.getPlanningStartPose() startPoseName = 'q_egress_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_egress_end' utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose) finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(startPose, finalFootHeight) constraints = [] constraints.extend(self.createUtorsoGazeConstraints([0.0, 1.0])) constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=0.01)) constraints.append(self.robotSystem.ikPlanner.createMovingBaseSafeLimitsConstraint()) constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName)) #constraints.append(self.robotSystem.ikPlanner.createLockedBackPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5])) constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot')) constraints.extend(self.createLeftFootPoseConstraint(finalLeftFootFrame, tspan=[1,1])) constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10, rescaleBodyNames=['l_foot'], rescaleBodyPts=[0.0, 0.0, 0.0], maxBodyTranslationSpeed=self.maxFootTranslationSpeed) #constraintSet.seedPoseName = 'q_start' #constraintSet.nominalPoseName = 'q_start' constraintSet.runIk() footFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('l_foot', startPose) t = transformUtils.frameFromPositionAndRPY([0, 0, self.polaris.leftFootEgressOutsideFrame.transform.GetPosition()[2]-footFrame.GetPosition()[2]], [0, 0, 0]) liftFrame = transformUtils.concatenateTransforms([footFrame, t]) vis.updateFrame(liftFrame, 'lift frame') c = ik.WorldFixedOrientConstraint() c.linkName = 'l_foot' c.tspan = [0.0, 0.1, 0.2] constraints.append(c) constraints.extend(self.createLeftFootPoseConstraint(liftFrame, tspan=[0.2, 0.2])) constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressMidFrame, tspan=[0.5, 0.5])) constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressOutsideFrame, tspan=[0.8, 0.8])) #plan = constraintSet.planEndPoseGoal(feetOnGround=False) keyFramePlan = constraintSet.runIkTraj() poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan) ts = [poseTimes[0]] supportsList = [['r_foot']] plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, False) self.addPlan(plan) return plan
def handleStatus(self, msg): if msg.plan_type == msg.STANDING: goal = transformUtils.frameFromPositionAndRPY( np.array([robotStateJointController.q[0] + 2 * self.max_distance_per_plan * (np.random.random() - 0.5), robotStateJointController.q[1] + 2 * self.max_distance_per_plan * (np.random.random() - 0.5), robotStateJointController.q[2] - 0.84]), [0, 0, robotStateJointController.q[5] + 2 * np.degrees(np.pi) * (np.random.random() - 0.5)]) request = footstepsDriver.constructFootstepPlanRequest(robotStateJointController.q, goal) request.params.max_num_steps = 18 footstepsDriver.sendFootstepPlanRequest(request)
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 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 spawnBoardAffordance(self, randomize=False): self.boardLength = 1.5 if randomize: position = [ random.uniform(0.5, 0.8), random.uniform(-0.2, 0.2), random.uniform(0.5, 0.8) ] rpy = [ random.choice((random.uniform(-35, 35), random.uniform(70, 110))), random.uniform(-10, 10), random.uniform(-5, 5) ] zwidth = random.uniform(0.5, 1.0) else: if self.b.val: position = [0.4, 0.0, 1.] else: position = [0.6, 0.0, 1.] rpy = [90, 1, 0] zwidth = self.boardLength xwidth = 3.75 * .0254 ywidth = 1.75 * .0254 t = transformUtils.frameFromPositionAndRPY(position, rpy) t.Concatenate(self.b.computeGroundFrame(self.b.robotModel)) xaxis = [1, 0, 0] yaxis = [0, 1, 0] zaxis = [0, 0, 1] for axis in (xaxis, yaxis, zaxis): t.TransformVector(axis, axis) self.affordance = segmentation.createBlockAffordance( t.GetPosition(), xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, 'board', parent='affordances') self.affordance.setProperty('Color', QtGui.QColor(200, 150, 100)) t = self.affordance.actor.GetUserTransform() self.frame = vis.showFrame(t, 'board frame', parent=self.affordance, visible=False, scale=0.2)
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 getFeetMidPoint(model, useWorldZ=True): ''' Returns a frame in world coordinate system that is the average of the left and right foot reference point positions in world frame, the average of the left and right foot yaw in world frame, and Z axis aligned with world Z. The foot reference point is the average of the foot contact points in the foot frame. ''' contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts() contact_pts_mid_left = np.mean(contact_pts_left, axis=0) # mid point on foot relative to foot frame contact_pts_mid_right = np.mean(contact_pts_right, axis=0) # mid point on foot relative to foot frame t_lf_mid = model.getLinkFrame(_leftFootLink) t_lf_mid.PreMultiply() t_lf_mid.Translate(contact_pts_mid_left) t_rf_mid = model.getLinkFrame(_rightFootLink) t_rf_mid.PreMultiply() t_rf_mid.Translate(contact_pts_mid_right) if (_robotType == 0): # Atlas t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5) elif (_robotType == 1): # Valkyrie v1 Foot orientation is silly l_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [180.0, 82.5, 0]) l_foot_sole.PostMultiply() l_foot_sole.Concatenate(t_lf_mid) r_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [0.0, -82.5, 0]) r_foot_sole.PostMultiply() r_foot_sole.Concatenate(t_rf_mid) t_feet_mid = transformUtils.frameInterpolate(l_foot_sole, r_foot_sole, 0.5) elif (_robotType == 2): # Valkyrie v2 is better t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5) if useWorldZ: rpy = [0.0, 0.0, np.degrees(transformUtils.rollPitchYawFromTransform(t_feet_mid)[2])] return transformUtils.frameFromPositionAndRPY(t_feet_mid.GetPosition(), rpy) else: return t_feet_mid
def placeStepsOnBlocks(self, blocks, groundPlane, standingFootName, standingFootFrame, removeFirstLeftStep = True): footsteps = [] for i, block in enumerate(blocks): # move back less for stereo: # lidar: -0.27 and -0.23 if self.processContinuousStereo or self.processRawStereo: nextLeftTransform = transformUtils.frameFromPositionAndRPY([-0.24,0.29,0.08], [0,0,0]) nextRightTransform = transformUtils.frameFromPositionAndRPY([-0.20,0.1,0.08], [0,0,0]) else: nextLeftTransform = transformUtils.frameFromPositionAndRPY([-0.27,0.29,0.08], [0,0,0]) nextRightTransform = transformUtils.frameFromPositionAndRPY([-0.23,0.1,0.08], [0,0,0]) nextLeftTransform.Concatenate(block.cornerTransform) footsteps.append(Footstep(nextLeftTransform,False)) nextRightTransform.Concatenate(block.cornerTransform) footsteps.append(Footstep(nextRightTransform,True)) #footOnGround = False #if (groundPlane): # # TODO: 0.08 is distance from foot frames to sole. remove hard coding! # distOffGround = abs(groundPlane.cornerTransform.GetPosition()[2]-standingFootFrame.GetPosition()[2] + 0.08) # #print "distOffGround",distOffGround # footOnGround = (distOffGround < 0.05) # if (footOnGround): # # the robot is standing on the ground plane # nextRightTransform = transformUtils.frameFromPositionAndRPY([(-0.23-0.38),0.1,0.08-0.13], [0,0,0]) # nextRightTransform.Concatenate(blocks[0].cornerTransform) # footsteps = [Footstep(nextRightTransform,True)] + footsteps #if (footOnGround is False): # # if we are standing on right foot, we can see the next block. # # but the next left step has been committed - so remove it from the the list if (removeFirstLeftStep is True): if (standingFootName is self.ikPlanner.rightFootLink ): footsteps = footsteps[1:] #print "removing the first left step" return footsteps
def __init__(self, robotModel, playbackRobotModel, teleopRobotModel, footstepPlanner, manipPlanner, ikPlanner, lhandDriver, rhandDriver, atlasDriver, multisenseDriver, sensorJointController, planPlaybackFunction, showPoseFunction): self.robotModel = robotModel self.playbackRobotModel = playbackRobotModel # not used inside the demo self.teleopRobotModel = teleopRobotModel # not used inside the demo self.footstepPlanner = footstepPlanner self.manipPlanner = manipPlanner self.ikPlanner = ikPlanner self.lhandDriver = lhandDriver self.rhandDriver = rhandDriver self.atlasDriver = atlasDriver self.multisenseDriver = multisenseDriver self.sensorJointController = sensorJointController self.planPlaybackFunction = planPlaybackFunction self.showPoseFunction = showPoseFunction self.goalTransform1 = transformUtils.frameFromPositionAndRPY([1, 0, 0], [0, 0, 0]) self.goalTransform2 = transformUtils.frameFromPositionAndRPY( [2, 0, 0], [0, 0, 10]) #self.goalTransform2 = transformUtils.frameFromPositionAndRPY([1,1,0],[0,0,90]) self.visOnly = False # True for development, False for operation self.planFromCurrentRobotState = True # False for development, True for operation useDevelopment = False if (useDevelopment): self.visOnly = True # True for development, False for operation self.planFromCurrentRobotState = False # False for development, True for operation self.optionalUserPromptEnabled = False self.requiredUserPromptEnabled = True self.constraintSet = None self.plans = [] self._setupSubscriptions()
def run(self): polyData = self.getPointCloud() annotation = self.getAnnotationInput() annotationPoint = annotation.annotationPoints[0] mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance')) annotation.setProperty('Visible', False) om.removeFromObjectModel(om.findObjectByName('shelf item')) obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0]) t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0]) segmentation.makeMovable(obj, t)
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 updateCamera(msg): T = transformUtils.frameFromPositionAndRPY(msg.q[:3],np.degrees(msg.q[3:])) axes = transformUtils.getAxesFromTransform(T) #vis.updateFrame(T, 'vicon camera') #return camera = app.view.camera() camera.SetPosition(T.GetPosition()) camera.SetFocalPoint(np.array(T.GetPosition())+axes[0]) camera.SetViewUp(axes[2]) camera.SetViewAngle(122.6) app.view.render()
def updateCamera(msg): T = transformUtils.frameFromPositionAndRPY(msg.q[:3], np.degrees(msg.q[3:])) axes = transformUtils.getAxesFromTransform(T) # vis.updateFrame(T, 'vicon camera') # return camera = app.view.camera() camera.SetPosition(T.GetPosition()) camera.SetFocalPoint(np.array(T.GetPosition()) + axes[0]) camera.SetViewUp(axes[2]) camera.SetViewAngle(122.6) app.view.render()
def createSpheres(self, sensorValues): d = DebugData() for key in sensorValues.keys(): frame, pos, rpy = self.sensorLocations[key] t = transformUtils.frameFromPositionAndRPY(pos, rpy) t.PostMultiply() t.Concatenate(self.frames[frame]) d.addSphere(t.GetPosition(), radius=0.005, color=self.getColor(sensorValues[key], key), resolution=8) vis.updatePolyData(d.getPolyData(), self.name, colorByName='RGB255')
def planReachToTableObject(self, side='left'): obj, frame = self.getNextTableObject(side) startPose = self.getPlanningStartPose() if self.ikPlanner.fixedBaseArm: # includes reachDist hack instead of in ikPlanner (TODO!) f = transformUtils.frameFromPositionAndRPY( np.array(frame.transform.GetPosition())-np.array([self.reachDist,0,0]), [0,0,-90] ) f.PreMultiply() f.RotateY(90) f.Update() self.constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, side, f, lockBase=False, lockBack=True) #newFrame = vis.FrameItem('reach_item', f, self.view) #self.constraintSet = self.ikPlanner.planGraspOrbitReachPlan(startPose, side, newFrame, constraints=None, dist=self.reachDist, lockBase=self.lockBase, lockBack=self.lockBack, lockArm=False) else: self.constraintSet = self.ikPlanner.planGraspOrbitReachPlan(startPose, side, frame, constraints=None, dist=self.reachDist, lockBase=self.lockBase, lockBack=self.lockBack, lockArm=False) loweringSide = 'left' if side == 'right' else 'right' armPose = self.getLoweredArmPose(startPose, loweringSide) armPoseName = 'lowered_arm_pose' self.ikPlanner.ikServer.sendPoseToServer(armPose, armPoseName) loweringSideJoints = [] if (loweringSide == 'left'): loweringSideJoints += self.ikPlanner.leftArmJoints else: loweringSideJoints += self.ikPlanner.rightArmJoints reachingSideJoints = [] if (side == 'left'): reachingSideJoints += self.ikPlanner.leftArmJoints else: reachingSideJoints += self.ikPlanner.rightArmJoints armPostureConstraint = self.ikPlanner.createPostureConstraint(armPoseName, loweringSideJoints) armPostureConstraint.tspan = np.array([1.0, 1.0]) self.constraintSet.constraints.append(armPostureConstraint) self.constraintSet.runIk() #armPose = self.getRaisedArmPose(startPose, side) #armPoseName = 'raised_arm_pose' #self.ikPlanner.ikServer.sendPoseToServer(armPose, armPoseName) #armPostureConstraint = self.ikPlanner.createPostureConstraint(armPoseName, reachingSideJoints) #armPostureConstraint.tspan = np.array([0.5, 0.5]) #self.constraintSet.constraints.append(armPostureConstraint) print 'planning reach to' plan = self.constraintSet.runIkTraj() self.addPlan(plan)
def __init__(self, robotModel, playbackRobotModel, teleopRobotModel, footstepPlanner, manipPlanner, ikPlanner, lhandDriver, rhandDriver, atlasDriver, multisenseDriver, sensorJointController, planPlaybackFunction, showPoseFunction): self.robotModel = robotModel self.playbackRobotModel = playbackRobotModel # not used inside the demo self.teleopRobotModel = teleopRobotModel # not used inside the demo self.footstepPlanner = footstepPlanner self.manipPlanner = manipPlanner self.ikPlanner = ikPlanner self.lhandDriver = lhandDriver self.rhandDriver = rhandDriver self.atlasDriver = atlasDriver self.multisenseDriver = multisenseDriver self.sensorJointController = sensorJointController self.planPlaybackFunction = planPlaybackFunction self.showPoseFunction = showPoseFunction self.goalTransform1 = transformUtils.frameFromPositionAndRPY([1,0,0],[0,0,0]) self.goalTransform2 = transformUtils.frameFromPositionAndRPY([2,0,0],[0,0,10]) #self.goalTransform2 = transformUtils.frameFromPositionAndRPY([1,1,0],[0,0,90]) self.visOnly = False # True for development, False for operation self.planFromCurrentRobotState = True # False for development, True for operation useDevelopment = False if (useDevelopment): self.visOnly = True # True for development, False for operation self.planFromCurrentRobotState = False # False for development, True for operation self.optionalUserPromptEnabled = False self.requiredUserPromptEnabled = True self.constraintSet = None self.plans = [] self._setupSubscriptions()
def testEulerToFrame(): ''' Test some euler converions ''' rpy = transformations.euler_from_quaternion( transformations.random_quaternion()) frame = transformUtils.frameFromPositionAndRPY([0, 0, 0], np.degrees(rpy)) mat = transformUtils.getNumpyFromTransform(frame) mat2 = transformations.euler_matrix(rpy[0], rpy[1], rpy[2]) print mat print mat2 assert np.allclose(mat, mat2)
def getRoomSweepFrames(self, rotateHandFrame=False): topFrame = transformUtils.frameFromPositionAndRPY([0.65,0.0,0.8],[160,0,90]) yawFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,0,self.currentYawDegrees]) if rotateHandFrame: fixHandFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,-90,0]) topFrame.PreMultiply() topFrame.Concatenate( fixHandFrame ) topFrame.PostMultiply() topFrame.Concatenate( yawFrame ) bottomFrame = transformUtils.frameFromPositionAndRPY([0.6,0.0,0.4],[210,0,90]) yawFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,0,self.currentYawDegrees]) if rotateHandFrame: bottomFrame.PreMultiply() bottomFrame.Concatenate( fixHandFrame ) bottomFrame.PostMultiply() bottomFrame.Concatenate( yawFrame ) if (self.fromTop): self.startFrame = vis.showFrame(topFrame, 'frame start', visible=False, scale=0.1,parent=self.mapFolder) self.endFrame = vis.showFrame(bottomFrame, 'frame end', visible=False, scale=0.1,parent=self.mapFolder) else: self.startFrame = vis.showFrame(bottomFrame, 'frame start', visible=False, scale=0.1,parent=self.mapFolder) self.endFrame = vis.showFrame(topFrame, 'frame end', visible=False, scale=0.1,parent=self.mapFolder)
def findSafeRegion(self, pose): pose = np.asarray(pose) tformForProjection = transformUtils.frameFromPositionAndRPY([0,0,0], pose[3:] * 180 / np.pi) tform = transformUtils.frameFromPositionAndRPY(pose[:3], pose[3:] * 180 / np.pi) contact_pts_on_plane = np.zeros((2, self.bot_pts.shape[1])) for j in range(self.bot_pts.shape[1]): contact_pts_on_plane[:,j] = tformForProjection.TransformPoint([self.bot_pts[0,j], self.bot_pts[1,j], 0])[:2] Rdot = np.array([[0, -1], [1, 0]]) contact_vel_in_world = Rdot.dot(contact_pts_on_plane) c_region = {'A': [], 'b': []} for i in range(self.planar_polyhedron.A.shape[0]): ai = self.planar_polyhedron.A[i,:] n = np.linalg.norm(ai) ai = ai / n bi = self.planar_polyhedron.b[i] / n p = ai.dot(contact_pts_on_plane) v = ai.dot(contact_vel_in_world) mask = np.logical_or(p >= 0, v >= 0) for j, tf in enumerate(mask): if tf: c_region['A'].append(np.hstack((ai, v[j]))) c_region['b'].append([bi - p[j]]) A = np.vstack(c_region['A']) b = np.hstack(c_region['b']) b = b + A.dot(np.array([0,0,pose[5]])) self.c_space_polyhedron = Hrep(A, b) return SafeTerrainRegion(A, b, [], [], tform)