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 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 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 "atlas" in _modelName: # atlas_v3/v4/v5 t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5) elif (_modelName == "valkyrie"): # valkyrie t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5) else: raise ValueError("Model Name not recognised") 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 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 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 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 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 __init__(self, channel='OPTITRACK_FRAMES', name='Optitrack Visualier'): self.name = name self.channel = channel self.subscriber = None self.optitrackToWorld = transformUtils.frameFromPositionAndRPY([0,0,0],[90,0,90]) self.subscriber = None self.fpsCounter = FPSCounter() self.fpsCounter.printToConsole = False
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 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 initCameraFrustumVisualizer(depthScanner): cameraObj = vis.showPolyData(vtk.vtkPolyData(), 'camera', parent=depthScanner.parentFolder, view=depthScanner.pointCloudView) cameraFrame = vis.addChildFrame(cameraObj) cameraFrame.setProperty('Scale', 50) cameraObj.setProperty('Visible', False) pointCloudToCamera = transformUtils.frameFromPositionAndRPY((0,0,0,), (-90, 90, 0)).GetLinearInverse() def updateCameraMesh(): scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0 rayLength = scale d = DebugData() d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1,0.5,0]) d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1,0.5,0]) cameraModelMesh = d.getPolyData() t = vtk.vtkTransform() t.Scale(scale, scale, scale) cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t) cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength) cameraMesh = filterUtils.transformPolyData(cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse()) cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh]) cameraObj.setPolyData(cameraMesh) def onCameraModified(): cameraToWorld = getCameraTransform(depthScanner.view.camera()) depthScanner.pointCloudObj.actor.SetUserTransform(transformUtils.concatenateTransforms([pointCloudToCamera, cameraToWorld])) cameraFrame.copyFrame(cameraToWorld) def enableFrustum(): updateCameraMesh() cameraObj.setProperty('Visible', True) onCameraModified() depthScanner._updateFunc = onCameraModified applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True) applogic.resetCamera(viewDirection=[1, 1, -0.4], view=depthScanner.pointCloudView) depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1]) def disableFrustum(): cameraObj.setProperty('Visible', False) depthScanner.pointCloudObj.actor.SetUserTransform(None) depthScanner._updateFunc = None applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, False) applogic.resetCamera(viewDirection=[0, 0, -1], view=depthScanner.pointCloudView) depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0]) return FieldContainer( updateCameraMesh=updateCameraMesh, onCameraModified=onCameraModified, enableFrustum=enableFrustum, disableFrustum=disableFrustum )
def getSpawnFrame(self): if self.jointController: # get spawn frame in front of robot 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) else: frame = vtk.vtkTransform() return frame
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 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 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 __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 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)
def newWalkingGoal(displayPoint, view): footFrame = footstepsDriver.getFeetMidPoint(robotModel) worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint) groundOrigin = footFrame.GetPosition() groundNormal = [0.0, 0.0, 1.0] selectedGroundPoint = [0.0, 0.0, 0.0] t = vtk.mutable(0.0) vtk.vtkPlane.IntersectWithLine(worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint) walkingTarget = transformUtils.frameFromPositionAndRPY(selectedGroundPoint, np.array(footFrame.GetOrientation())) footstepsdriverpanel.panel.onNewWalkingGoal(walkingTarget)
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 makeStepFrames(stepFrames, relativeFrame=None, showFrames=False): frames = [] for i, stepFrame in enumerate(stepFrames): stepFrame = transformUtils.frameFromPositionAndRPY(stepFrame, [0,0,0]) stepFrame.PostMultiply() if relativeFrame: stepFrame.Concatenate(relativeFrame) if showFrames: obj = vis.updateFrame(stepFrame, 'step frame %d' % i, parent='step frames', scale=0.2) stepFrame = obj.transform frames.append(stepFrame) return frames
def onIkStartup(ikServer, startSuccess): side = 'left' goalFrame = transformUtils.frameFromPositionAndRPY([0.5, 0.5, 1.2], [0, 90, -90]) assert not checkGraspFrame(goalFrame, side) frame = teleopPanel.endEffectorTeleop.newReachTeleop(goalFrame, side) assert checkGraspFrame(goalFrame, side) teleopPanel.ui.planButton.click() assert playbackPanel.plan is not None teleopPanel.ikPlanner.useCollision = True; teleopPanel.ui.planButton.click() assert playbackPanel.plan is not None frame.setProperty('Edit', True) app.startTestingModeQuitTimer()
def getFootstepToWorldTransforms(self,footstepIdx, stepOffDirection='sideways'): self.updateFramesAndAffordance() footstepsToWorldList = [] for j in footstepIdx: if stepOffDirection == 'sideways': rpy = np.array([0,0,self.footstepYaw[j]]) position = self.footstepPosition[j] else: rpy = np.array([0,0,0], self.footstepYawForwards[j]) position = self.footstepPositionForwards[j] footstepToPlan = transformUtils.frameFromPositionAndRPY(position,rpy) footstepToWorld = footstepToPlan footstepToWorld.PostMultiply(); footstepToWorld.Concatenate(self.planToWorld) footstepsToWorldList.append(footstepToWorld) return footstepsToWorldList
def drawModel(self, model): modelFolder = om.getOrCreateContainer(model.name, parentObj=self.getRootFolder()) markerFolder = om.getOrCreateContainer("markers", parentObj=modelFolder) modelName = model.name markerObjects = markerFolder.children() if len(markerObjects) != model.nummarkers: for obj in markerObjects: om.removeFromObjectModel(obj) modelColor = vis.getRandomColor() markerObjects = self.createMarkerObjects(model.nummarkers, markerFolder, modelName, modelColor) self.models[modelName] = markerObjects if len(markerObjects): modelColor = markerObjects[0].getProperty("Color") for i, marker in enumerate(model.markers): xyz = np.array(marker.xyz) * self.unitConversion markerFrame = vtk.vtkTransform() markerFrame.Translate(xyz) markerObjects[i].getChildFrame().copyFrame(markerFrame) if self.drawEdges: d = DebugData() for m1 in model.markers: xyz = np.array(m1.xyz) * self.unitConversion for m2 in model.markers: xyz2 = np.array(m2.xyz) * self.unitConversion d.addLine(xyz, xyz2) edges = shallowCopy(d.getPolyData()) vis.updatePolyData(edges, modelName + " edges", color=modelColor, parent=modelFolder) else: edgesObj = om.findObjectByName(modelName + " edges") om.removeFromObjectModel(edgesObj) computeModelFrame = True if computeModelFrame: pos = np.array(model.segments[0].T) * self.unitConversion rpy = np.array(model.segments[0].A) modelFrame = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy)) vis.updateFrame(modelFrame, modelName + " frame", parent=modelFolder, scale=0.1)
def spawnTargetAffordance(self): for obj in om.getObjects(): if obj.getProperty('Name') == 'target': om.removeFromObjectModel(obj) targetFrame = transformUtils.frameFromPositionAndRPY([0.6,0.2,0.6],[180,0,90]) folder = om.getOrCreateContainer('affordances') z = DebugData() z.addLine(np.array([0,0,0]), np.array([-self.boxLength,0,0]), radius=0.02) # main bar z.addLine(np.array([-self.boxLength,0,0]), np.array([-self.boxLength,0,self.boxLength]), radius=0.02) # main bar z.addLine(np.array([-self.boxLength,0,self.boxLength]), np.array([0,0,self.boxLength]), radius=0.02) # main bar z.addLine(np.array([0,0,self.boxLength]), np.array([0,0,0]), radius=0.02) # main bar targetMesh = z.getPolyData() self.targetAffordance = vis.showPolyData(targetMesh, 'target', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=0.3) self.targetAffordance.actor.SetUserTransform(targetFrame) self.targetFrame = vis.showFrame(targetFrame, 'target frame', parent=self.targetAffordance, visible=False, scale=0.2) self.targetFrame = self.targetFrame.transform params = dict(length=self.boxLength, otdf_type='target', friendly_name='target') self.targetAffordance.setAffordanceParams(params) self.targetAffordance.updateParamsFromActorTransform()
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( ikconstraints.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 = ikconstraints.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 pointPickerStoredFootsteps(self, p1, p2): yaw = math.atan2(p2[1] - p1[1], p2[0] - p1[0]) * 180 / math.pi + 90 frame_p1 = transformUtils.frameFromPositionAndRPY(p1, [0, 0, yaw]) blockl = 0.3937 blockh = 0.142875 sep = 0.11 frame_pt_to_centerline = transformUtils.frameFromPositionAndRPY( [0, -blockl / 2, 0], [0, 0, 0]) frame_pt_to_centerline.PostMultiply() frame_pt_to_centerline.Concatenate(frame_p1) vis.updateFrame(frame_pt_to_centerline, "corner", parent="navigation") # up/down 1 block (Original) # flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] , # [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] , # [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] , # [ blockl*0.5 + 0.06 ,-.1 , blockh , 0 , 0 , 0] , # [ blockl*1.5 , .1 , 0 , 0 , 0 , 0] , # [ blockl*1.5 +0.03 ,-.1 , 0 , 0 , 0 , 0]]) # up/down 1 block (Newer) # flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] , # [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] , # [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] , # [ blockl*0.5 + 0.04 ,-.1 , blockh , 0 , 0 , 0] , # [ blockl*1.5 - 0.03 , .1 , 0 , 0 , 0 , 0] , # [ blockl*1.5 + 0.00 ,-.1 , 0 , 0 , 0 , 0]]) # up 3 blocks # flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] , # [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] , # [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] , # [ blockl*0.5 + 0.0 ,-.1 , blockh , 0 , 0 , 0] , # [ blockl*1.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] , # [ blockl*1.5 + 0.0 ,-.1 , 2*blockh, 0 , 0 , 0], # [ blockl*2.5 - 0.03 , .1 , 3*blockh, 0 , 0 , 0] , # [ blockl*2.5 + 0.0 ,-.1 , 3*blockh, 0 , 0 , 0]]) # up and down 3 blocks (original) # flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] , # [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] , # [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] , # [ blockl*0.5 + 0.0 ,-.1 , blockh , 0 , 0 , 0] , # [ blockl*1.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] , # [ blockl*1.5 + 0.0 ,-.1 , 2*blockh, 0 , 0 , 0], # [ blockl*2.5 - 0.03 , .1 , 3*blockh, 0 , 0 , 0] , # [ blockl*2.5 + 0.03 ,-.1 , 3*blockh, 0 , 0 , 0], # [ blockl*3.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] , # [ blockl*3.5 + 0.03 ,-.1 , 2*blockh, 0 , 0 , 0], # [ blockl*4.5 - 0.03 , .1 , 1*blockh, 0 , 0 , 0] , # [ blockl*4.5 + 0.03 ,-.1 , 1*blockh, 0 , 0 , 0], # [ blockl*5.5 - 0.03 , .1 , 0 , 0 , 0 , 0] , # [ blockl*5.5 + 0.03 ,-.1 , 0 , 0 , 0 , 0]]) # up and down 3 blocks (used in video) # r =1 # flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0], # [ blockl*-0.5 , -sep , 0 , 0 , 0 , 0, r], # [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0], # [ blockl*0.5 + 0.0 ,-sep , blockh , 0 , 0 , 0, r], # [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0], # [ blockl*1.5 + 0.0 ,-sep , 2*blockh, 0 , 0 , 0, r], # [ blockl*2.5 - 0.03 , sep , 3*blockh, 0 , 0 , 0, 0], # [ blockl*2.5 + 0.03 ,-sep , 3*blockh, 0 , 0 , 0, r], # [ blockl*3.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0], # [ blockl*3.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r], # [ blockl*4.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0], # [ blockl*4.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # [ blockl*5.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # [ blockl*5.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r], # half # [ blockl*5.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # extra step for planner # [ blockl*4.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # invert order # [ blockl*4.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0], # [ blockl*3.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r], # [ blockl*3.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0], # [ blockl*2.5 + 0.03 ,-sep , 3*blockh, 0 , 0 , 0, r], # top # [ blockl*2.5 - 0.06 , sep , 3*blockh, 0 , 0 , 0, 0], # top # [ blockl*1.5 + 0.04 ,-sep , 2*blockh, 0 , 0 , 0, r], # [ blockl*1.5 - 0.06 , sep , 2*blockh, 0 , 0 , 0, 0], # [ blockl*0.5 + 0.04 ,-sep , blockh , 0 , 0 , 0, r], # [ blockl*0.5 - 0.06 , sep , blockh , 0 , 0 , 0, 0], # [ blockl*-0.5+ 0.04 , -sep , 0 , 0 , 0 , 0, r], # [ blockl*-0.5 - 0.03, sep , 0 , 0 , 0 , 0, 0]]) # up and down 2 blocks (used in vicon april 2014) # r =1 # flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0], # [ blockl*-0.5 , -sep , 0 , 0 , 0 , 0, r], # start # [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0], # [ blockl*0.5 + 0.0 ,-sep , blockh , 0 , 0 , 0, r], # 1 # [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0], # [ blockl*1.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r], # 2 # [ blockl*2.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0], # [ blockl*2.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # 1d # [ blockl*3.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # [ blockl*3.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r], # half # [ blockl*3.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # extra step for planner # [ blockl*2.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # invert order # [ blockl*2.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0], # [ blockl*1.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r], # [ blockl*1.5 - 0.06 , sep , 2*blockh, 0 , 0 , 0, 0], # [ blockl*0.5 + 0.04 ,-sep , blockh , 0 , 0 , 0, r], # [ blockl*0.5 - 0.06 , sep , blockh , 0 , 0 , 0, 0], # [ blockl*-0.5+ 0.04 , -sep , 0 , 0 , 0 , 0, r], # [ blockl*-0.5 - 0.03, sep , 0 , 0 , 0 , 0, 0]]) # up 6 blocks (used in journal paper in oct 2014) # r =1 # flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0], # [ blockl*-0.5 ,-sep , 0 , 0 , 0 , 0, r], # [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0], # [ blockl*0.5 - 0.02 ,-sep , blockh , 0 , 0 , 0, r], # [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0], # [ blockl*1.5 - 0.02 ,-sep , 2*blockh, 0 , 0 , 0, r], # [ blockl*2.5 - 0.03 , sep , 3*blockh, 0 , 0 , 0, 0], # [ blockl*2.5 - 0.02 ,-sep , 3*blockh, 0 , 0 , 0, r], # [ blockl*3.5 - 0.03 , sep , 4*blockh, 0 , 0 , 0, 0], # [ blockl*3.5 - 0.02 ,-sep , 4*blockh, 0 , 0 , 0, r], # [ blockl*4.5 - 0.03 , sep , 5*blockh, 0 , 0 , 0, 0], # [ blockl*4.5 - 0.02 ,-sep , 5*blockh, 0 , 0 , 0, r], # [ blockl*5.5 - 0.03 , sep , 6*blockh, 0 , 0 , 0, 0], # [ blockl*5.5 - 0.02 ,-sep , 6*blockh, 0 , 0 , 0, r]]) # continuous walking - first two blocks up r = 1 flist = np.array([[blockl * -0.5 - 0.03, sep, 0, 0, 0, 0, 0], [blockl * -0.5 + 0.03, -sep, 0, 0, 0, 0, r], [blockl * 0.5 - 0.03, sep, blockh, 0, 0, 0, 0], [blockl * 0.5 + 0.03, -sep, blockh, 0, 0, 0, r]]) contact_pts = self.footstepDriver.getContactPts() contact_pts_mid = np.mean( contact_pts, axis=0) # mid point on foot relative to foot frame foot_to_sole = transformUtils.frameFromPositionAndRPY( contact_pts_mid, [0, 0, 0]).GetLinearInverse() flist_shape = flist.shape self.goalSteps = [] for i in range(flist_shape[0]): step_t = vtk.vtkTransform() step_t.PostMultiply() step_t.Concatenate( transformUtils.frameFromPositionAndRPY(flist[i, 0:3], flist[i, 3:6])) step_t.Concatenate(foot_to_sole) step_t.Concatenate(frame_pt_to_centerline) step = lcmdrc.footstep_t() step.pos = positionMessageFromFrame(step_t) step.is_right_foot = flist[i, 6] # is_right_foot step.params = self.footstepDriver.getDefaultStepParams() # Visualization via triads #vis.updateFrame(step_t, str(i), parent="navigation") self.goalSteps.append(step) startPose = self.jointController.q request = self.footstepDriver.constructFootstepPlanRequest(startPose) request.num_goal_steps = len(self.goalSteps) request.goal_steps = self.goalSteps lcmUtils.publish('FOOTSTEP_PLAN_REQUEST', request)
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 om.removeFromObjectModel(existingWidget) if previousStep == stepIndex: 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) return True
class OptitrackVisualizer(object): ''' Usage: optitrackVis = OptitrackVisualizer('OPTITRACK_CHANNEL_NAME') # You can enable visualization of edges between optitrack markers, # but this visualization is slower. To enable: optitrackVis.drawEdges = True # By default the lcm update rate is throttled to 10 hz. # To increase the update rate: optitrackVis.subscriber.setSpeedLimit(100) # Note, the constructor calls initSubscriber() automatically. # To remove the lcm subscriber: optitrackVis.removeSubscriber() ''' defaultOptitrackToWorld = transformUtils.frameFromPositionAndRPY( [0,0,0],[90,0,90]) def __init__(self, channel="OPTITRACK_FRAMES", desc_channel="OPTITRACK_DATA_DESCRIPTIONS", name='Optitrack Visualier'): self.name = name self.channel = channel self.desc_channel = desc_channel self.subscriber = None self.desc_subscriber = None self.unitConversion = 0.001 self.data_descriptions = None self.marker_sets = om.getOrCreateContainer( "Marker Sets", parentObj=self.getRootFolder()) self.rigid_bodies = om.getOrCreateContainer( "Rigid Bodies", parentObj=self.getRootFolder()) self.labeled_markers = om.getOrCreateContainer( "Labeled Markers", parentObj=self.getRootFolder()) self.unlabeled_markers = om.getOrCreateContainer( "Unlabeled Markers", parentObj=self.getRootFolder()) self.drawEdges = False self.markerGeometry = None self.optitrackToWorld = vtk.vtkTransform() self.optitrackToWorld.SetMatrix( self.defaultOptitrackToWorld.GetMatrix()) self.callbacks = callbacks.CallbackRegistry([ 'RIGID_BODY_LIST_CHANGED', ]) def connectRigidBodyListChanged(self, func): return self.callbacks.connect('RIGID_BODY_LIST_CHANGED', func) def initSubscriber(self): self.subscriber = lcmUtils.addSubscriber( self.channel, optitrack_frame_t, self.onMessage) self.subscriber.setSpeedLimit(10) self.desc_subscriber = lcmUtils.addSubscriber( self.desc_channel, optitrack_data_descriptions_t, self.onDescMessage) def removeSubscriber(self): if self.subscriber is not None: lcmUtils.removeSubscriber(self.subscriber) self.subscriber = None if self.desc_subscriber is not None: lcmUtils.removeSubscriber(self.desc_subscriber) self.desc_subscriber = None def isEnabled(self): return self.subscriber is not None def setEnabled(self, enabled): if enabled and not self.isEnabled(): self.initSubscriber() elif not enabled and self.isEnabled(): self.removeSubscriber() self.removeRootFolder() def enable(self): self.setEnabled(True) def disable(self): self.setEnabled(False) def getRootFolder(self): folder = om.getOrCreateContainer(self.channel) return folder def removeRootFolder(self): om.removeFromObjectModel(self.getRootFolder()) def getMarkerGeometry(self): if self.markerGeometry is None: d = DebugData() d.addSphere(np.zeros(3), radius=0.007, resolution=8) self.markerGeometry = shallowCopy(d.getPolyData()) return self.markerGeometry def createMarkerObjects(self, marker_ids, modelFolder, modelName, modelColor): geom = self.getMarkerGeometry() visible = modelFolder.getProperty('Visible') def makeMarker(i): obj = vis.showPolyData( shallowCopy(geom), modelName + ' marker %d' % i, color=modelColor, parent=modelFolder) obj.setProperty('Visible', visible) vis.addChildFrame(obj) return obj return [makeMarker(i) for i in marker_ids] def setRobotBaseTransform(self, transform): self.optitrackToWorld.Translate(transform.GetInverse().GetPosition()) # Move down a little to account for the fact that the markers # aren't at the base of the robot. self.optitrackToWorld.Translate(0, 0.025, 0) # TODO(sam.creasey) handle rotation def _updateMarkerCollection(self, prefix, folder, marker_ids, positions, base_transform=None): markers = folder.children() if len(markers) != len(positions): for obj in markers: om.removeFromObjectModel(obj) modelColor = vis.getRandomColor() markers = self.createMarkerObjects( marker_ids, folder, prefix, modelColor) if len(markers): modelColor = markers[0].getProperty('Color') for i, pos in enumerate(positions): marker_frame = transformUtils.transformFromPose(pos, (1, 0, 0, 0)) marker_frame = transformUtils.concatenateTransforms( [marker_frame, self.optitrackToWorld]) if base_transform is not None: marker_frame = transformUtils.concatenateTransforms( [marker_frame, base_transform]) markers[i].getChildFrame().copyFrame(marker_frame) # TODO(sam.creasey) we could try drawing edges here def _handleMarkerSets(self, marker_sets): # Get the list of existing marker sets so we can track any # which disappear. remaining_set_names = set( [x.getProperty('Name') for x in self.marker_sets.children()]) for marker_set in marker_sets: set_name = 'Marker set ' + marker_set.name remaining_set_names.discard(set_name) set_folder = om.getOrCreateContainer( set_name, parentObj=self.marker_sets) marker_ids = range(marker_set.num_markers) self._updateMarkerCollection( marker_set.name + '.', set_folder, marker_ids, marker_set.xyz) for remaining_set in remaining_set_names: obj = om.findObjectByName(remaining_set, self.marker_sets) om.removeFromObjectModel(obj) def _handleRigidBodies(self, rigid_bodies): # Get the list of existing rigid bodies so we can track any # which disappear. remaining_body_names = set( [x.getProperty('Name') for x in self.rigid_bodies.children()]) bodies_added = False bodies_removed = False for body in rigid_bodies: body_name = 'Body ' + str(body.id) for desc in self.data_descriptions.rigid_bodies: if desc.id == body.id: body_name = desc.name if body_name in remaining_body_names: body_obj = om.findObjectByName( body_name, parent=self.rigid_bodies) else: bodies_added = True # The use of a box here is arbitrary. body_obj = affordanceitems.BoxAffordanceItem( body_name, applogic.getCurrentRenderView()) om.addToObjectModel(body_obj, parentObj=self.rigid_bodies) vis.addChildFrame(body_obj).setProperty('Deletable', False) body_obj.setProperty('Surface Mode', 'Wireframe') body_obj.setProperty('Color', [1,0,0]) remaining_body_names.discard(body_name) x,y,z,w = body.quat quat = (w,x,y,z) objToOptitrack = transformUtils.transformFromPose(body.xyz, quat) # Dimension our box based on a bounding across all of our # markers. all_xyz = body.marker_xyz + [body.xyz] all_xyz = [(xyz[0] - body.xyz[0], xyz[1] - body.xyz[1], xyz[2] - body.xyz[2]) for xyz in all_xyz] marker_transforms = [transformUtils.transformFromPose(xyz, (1, 0, 0, 0)) for xyz in all_xyz] inv_transform = transformUtils.transformFromPose((0, 0, 0), (w, -x, -y, -z)) marker_transforms = [transformUtils.concatenateTransforms([t, inv_transform]) for t in marker_transforms] all_xyz = [t.GetPosition() for t in marker_transforms] (min_x, min_y, min_z) = ( min(xyz[0] for xyz in all_xyz), min(xyz[1] for xyz in all_xyz), min(xyz[2] for xyz in all_xyz)) (max_x, max_y, max_z) = ( max(xyz[0] for xyz in all_xyz), max(xyz[1] for xyz in all_xyz), max(xyz[2] for xyz in all_xyz)) dimensions = ( max(0.01, max_x - min_x), max(0.01, max_y - min_y), max(0.01, max_z - min_z)) #print "max, min", (max_x, max_y, max_z), (min_x, min_y, min_z) body_obj.setProperty('Dimensions', dimensions) objToWorld = transformUtils.concatenateTransforms([objToOptitrack, self.optitrackToWorld]) body_obj.getChildFrame().copyFrame(objToWorld) folder = om.getOrCreateContainer('Models', parentObj=body_obj) self._updateMarkerCollection( body_name + '.', folder, body.marker_ids, body.marker_xyz)#, base_transform=transform) if len(remaining_body_names): bodies_removed = True for remaining_body in remaining_body_names: obj = om.findObjectByName(remaining_body, self.rigid_bodies) om.removeFromObjectModel(obj) if bodies_added or bodies_removed: self.callbacks.process( 'RIGID_BODY_LIST_CHANGED', sorted([x.getProperty('Name') for x in self.rigid_bodies.children()])) def _handleLabeledMarkers(self, labeled_markers): marker_ids = [x.id for x in labeled_markers] marker_positions = [x.xyz for x in labeled_markers] # We'll rename the items ourselves later. self._updateMarkerCollection( "dummy prefix", self.labeled_markers, marker_ids, marker_positions) for i, marker in enumerate(self.labeled_markers.children()): marker_name = 'Marker ' + str(marker_ids[i]) if marker.getProperty('Name') != marker_name: marker.rename(marker_name) def _handleUnlabeledMarkers(self, positions): self._updateMarkerCollection( "Unlabeled", self.unlabeled_markers, range(len(positions)), positions) def onMessage(self, msg): self.lastMessage = msg if self.data_descriptions is None: return self._handleMarkerSets(msg.marker_sets) self._handleRigidBodies(msg.rigid_bodies) #self._handleLabeledMarkers(msg.labeled_markers) #self._handleUnlabeledMarkers(msg.other_markers) def onDescMessage(self, msg): self.data_descriptions = msg
def newWalkingGoal(self, displayPoint, view): # put walking goal at robot's base mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"] footFrame = self.robotModel.getLinkFrame(mainLink) if not footFrame: print( "ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in " "the robot's URDF. Cannot place walking goal.".format(mainLink) ) return worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint) groundOrigin = footFrame.GetPosition() groundNormal = [0.0, 0.0, 1.0] selectedGroundPoint = [0.0, 0.0, 0.0] t = vtk.mutable(0.0) vtk.vtkPlane.IntersectWithLine( worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint ) walkingTarget = transformUtils.frameFromPositionAndRPY( selectedGroundPoint, np.array(footFrame.GetOrientation()) ) frameObj = vis.updateFrame( walkingTarget, self.robotName + " walking goal", parent="planning", scale=0.25, ) frameObj.setProperty("Edit", True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() terrain = om.findObjectByName("HEIGHT_MAP_SCENE") if terrain: pos = np.array(frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine( polyData, pos, pos + [0, 0, 1] ) polyData = segmentation.thresholdPoints( polyData, "distance_to_line", [0.0, 0.1] ) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, "Points")[:, 2]) frameObj.transform.Translate( pos - np.array(frameObj.transform.GetPosition()) ) d = DebugData() d.addSphere((0, 0, 0), radius=0.03) handle = vis.showPolyData( d.getPolyData(), "walking goal terrain handle " + self.robotName, parent=frameObj, visible=True, color=[1, 1, 0], ) handle.actor.SetUserTransform(frameObj.transform) placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == "Edit": if propertySet.getProperty(propertyName): placer.start() else: placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, "Edit") frameObj.connectFrameModified(self.onWalkingGoalModified)
def toFrame(xyzrpy): rpy = [math.degrees(rad) for rad in xyzrpy[3:]] return transformUtils.frameFromPositionAndRPY(xyzrpy[:3], rpy)
def getCameraToKukaEndEffectorFrame(): return transformUtils.frameFromPositionAndRPY([0.1, 0, 0.0], [-90, -22.5, -90])
import numpy if ikPlanner.pushToMatlab == True: print "FAILURE - pushing requests to matlab" exit() qT = numpy.array([ 0, 0, 0, 0, 0, 0, -6.310489698080346e-05, 0.34103086590766907, 3.8130277971504256e-05, 1.4273228645324707, 5.833456089021638e-05, -0.4845042824745178, -3.8867587136337534e-05 ]) q0 = numpy.array([ 0., 0., 0., 0., 0., 0., 0., 0.78539816, 0., 1.57079633, 0., -0.78539816, 0. ]) goalFrame = transformUtils.frameFromPositionAndRPY( [0.36932988056397303, -0.009998017176602909, 0.8891143571732633], [-1.3262913021702864e-12, 89.99999979432002, -89.99963750134272]) constraintSet = ikPlanner.planEndEffectorGoal(q0, 'left', goalFrame, lockBase=True, lockBack=True) q = numpy.array(constraintSet.runIk()[0]) ret = constraintSet.runIkTraj() if ((q - qT).__abs__() > 1e-3).any(): print "FAILURE - IK pose incorrect." exit() if ret.plan_info[0] != 0:
def computeAffordanceFrame(self): position = self.properties.getProperty('Position') rpy = self.properties.getProperty('Rotation') t = transformUtils.frameFromPositionAndRPY(position, rpy) t.Concatenate(self.getGroundFrame()) return t
def setTagToWorld(pos, rpy): global tagToWorld tagToWorld = transformUtils.frameFromPositionAndRPY(pos, rpy)
def getDefaultCameraToWorld(): return transformUtils.frameFromPositionAndRPY([0,0,0], [-90,0,-90])
def updateTargetFrame(self): q = self.robotModel.model.getJointPositions() pos = q[:3] rpy = np.degrees(q[3:6]) transform = transformUtils.frameFromPositionAndRPY(pos, rpy) self.targetFrame.copyFrame(transform)
def transformOpticalFrameToBodyFrame(opticalFrame): rpy = [-90, 0, -90] opticalToBody = transformUtils.frameFromPositionAndRPY([0, 0, 0], rpy) bodyFrame = transformUtils.concatenateTransforms( [opticalToBody.GetLinearInverse(), opticalFrame]) return bodyFrame
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 om.removeFromObjectModel(existingWidget) if previousStep == stepIndex: 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 initCameraFrustumVisualizer(depthScanner): cameraObj = vis.showPolyData(vtk.vtkPolyData(), 'camera', parent=depthScanner.parentFolder, view=depthScanner.pointCloudView) cameraFrame = vis.addChildFrame(cameraObj) cameraFrame.setProperty('Scale', 50) cameraObj.setProperty('Visible', False) pointCloudToCamera = transformUtils.frameFromPositionAndRPY(( 0, 0, 0, ), (-90, 90, 0)).GetLinearInverse() def updateCameraMesh(): scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0 rayLength = scale d = DebugData() d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1, 0.5, 0]) d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1, 0.5, 0]) cameraModelMesh = d.getPolyData() t = vtk.vtkTransform() t.Scale(scale, scale, scale) cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t) cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength) cameraMesh = filterUtils.transformPolyData( cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse()) cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh]) cameraObj.setPolyData(cameraMesh) def onCameraModified(): cameraToWorld = getCameraTransform(depthScanner.view.camera()) depthScanner.pointCloudObj.actor.SetUserTransform( transformUtils.concatenateTransforms( [pointCloudToCamera, cameraToWorld])) cameraFrame.copyFrame(cameraToWorld) def enableFrustum(): updateCameraMesh() cameraObj.setProperty('Visible', True) onCameraModified() depthScanner._updateFunc = onCameraModified applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True) applogic.resetCamera(viewDirection=[1, 1, -0.4], view=depthScanner.pointCloudView) depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1]) def disableFrustum(): cameraObj.setProperty('Visible', False) depthScanner.pointCloudObj.actor.SetUserTransform(None) depthScanner._updateFunc = None applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, False) applogic.resetCamera(viewDirection=[0, 0, -1], view=depthScanner.pointCloudView) depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0]) return FieldContainer(updateCameraMesh=updateCameraMesh, onCameraModified=onCameraModified, enableFrustum=enableFrustum, disableFrustum=disableFrustum)