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 applyFrameTransform(x, y, z, yaw):
    if lastEditedFrame is not None and lastEditedFrame.getProperty("Edit"):
        t = vtk.vtkTransform()
        t.Concatenate(lastEditedFrame.transform)
        t.RotateZ(yaw)
        t.Translate(x, y, z)
        lastEditedFrame.copyFrame(t)

    def getNextDoubleSupportPose(self, lfootTransform, rfootTransform):

        vis.updateFrame(lfootTransform, 'lfootTransform', visible=True, scale=0.2)
        vis.updateFrame(rfootTransform, 'rfootTransform', visible=True, scale=0.2)

        startPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
        startPoseName = 'stride_start'
        self.ikPlanner.addPose(startPose, startPoseName)

        constraints = []
        # lock everything except the feet, constrain the feet
        constraints.append(self.ikPlanner.createQuasiStaticConstraint())
        constraints.append(self.ikPlanner.createMovingBackPostureConstraint())
        constraints.append(self.ikPlanner.createMovingBasePostureConstraint(startPoseName))
        constraints.append(self.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
        constraints.append(self.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))

        nullFrame = vtk.vtkTransform()
        positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.rightFootLink, rfootTransform, nullFrame)
        positionConstraint.tspan = [1.0, 1.0]
        orientationConstraint.tspan = [1.0, 1.0]
        constraints.append(positionConstraint)
        constraints.append(orientationConstraint)

        positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.leftFootLink, lfootTransform, nullFrame)
        positionConstraint.tspan = [1.0, 1.0]
        orientationConstraint.tspan = [1.0, 1.0]
        constraints.append(positionConstraint)
        constraints.append(orientationConstraint)

        constraintSet = ikplanner.ConstraintSet(self.ikPlanner, constraints, 'stride_end', startPoseName)
        nextDoubleSupportPose, info = constraintSet.runIk()
        return nextDoubleSupportPose

    def updateFrame(self):
        norm = np.linalg.norm(np.array([self.thetadot, self.phidot, self.yawdot, self.Xdot, self.Ydot, self.Zdot]))
        dt = 0.01 #self.timer.elapsed

        if self.baseFrame is not None:
            cameraFocus = np.asarray(self.camera.GetFocalPoint())
            cameraInterp = cameraFocus + (self.cameraTarget - cameraFocus)*0.02
            self.camera.SetFocalPoint(cameraInterp)
        
        if self.cameraMode is not True:
            if self.baseFrame is not None and norm > 0.1:
                t = vtk.vtkTransform()
                t.Concatenate(self.baseFrame.transform)
                t.RotateZ(-self.thetadot * dt * self.speedMultiplier)
                t.RotateX(self.phidot * dt * self.speedMultiplier)
                t.RotateY(self.yawdot * dt * self.speedMultiplier)
                t.Translate(self.Xdot * dt * self.speedMultiplier, self.Ydot * dt * self.speedMultiplier, self.Zdot * dt * self.speedMultiplier)
                self.baseFrame.copyFrame(t)
        else:
            
            self.camera.Elevation(self.Zdot * self.speedMultiplier)
            self.camera.Azimuth(self.Xdot * self.speedMultiplier)
            self.camera.Zoom(1.0 + self.phidot/1000.0)

        self.view.render()

def getTransformFromNumpy(mat):
    '''
    Given a numpy 4x4 array, return a vtkTransform.
    '''
    assert mat.shape == (4,4)
    t = vtk.vtkTransform()
    t.SetMatrix(mat.flatten())
    return t

    def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):

        print 'planning with safe regions.  %d blocks.' % len(blocks)

        folder = om.getOrCreateContainer('Safe terrain regions')
        om.removeFromObjectModel(folder)

        footsteps = []

        for i, block in enumerate(blocks):
            corners = block.getCorners()
            rpy = np.radians(block.cornerTransform.GetOrientation())
            #rpy = [0.0, 0.0, 0.0]
            self.convertStepToSafeRegion(corners, rpy)

        lastBlock = blocks[-1]

        goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
        goalOffset = vtk.vtkTransform()
        goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
        goalFrame.PreMultiply()
        goalFrame.Concatenate(goalOffset)
        goalPosition = np.array(goalFrame.GetPosition())

        if len(blocks) > 1:
            goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
            goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))

        vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)

        request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)

        assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
        if standingFootName == self.ikPlanner.rightFootLink:
            leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
        else:
            leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT

        request.params.leading_foot = leadingFoot
        request.params.max_forward_step = 0.5
        request.params.nom_forward_step = 0.12
        request.params.nom_step_width = 0.22
        request.params.max_num_steps = 8 #2*len(blocks)

        plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)

        if not plan:
            return []

        #print 'received footstep plan with %d steps.' % len(plan.footsteps)

        footsteps = []
        for i, footstep in enumerate(plan.footsteps):
            footstepTransform = self.transformFromFootstep(footstep)
            footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))

        return footsteps[2:]

def concatenateTransforms(transformList):
    '''
    Given a list of vtkTransform objects, returns a new vtkTransform
    which is a concatenation of the whole list using vtk post multiply.
    See documentation for vtkTransform::PostMultiply.
    '''
    result = vtk.vtkTransform()
    result.PostMultiply()
    for t in transformList:
        result.Concatenate(t)
    return result

    def __init__(self):

        pose = transformUtils.poseFromTransform(vtk.vtkTransform())


        self.pointcloud  = ioUtils.readPolyData(director.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp')
        self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None)
        segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0,  0.        ,  0.        , 0.0])))

        self.originFrame = self.pointcloudPD.getChildFrame()

        t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625,  0.        ,  0.        , -0.34604951]))
        self.valveWalkFrame  = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([-3.31840048,  0.36408685, -0.67413123]), array([ 0.93449475,  0.        ,  0.        , -0.35597691]))
        self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004,  0.        ,  0.        , -0.34940972]))
        self.drillWalkFrame  = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572,  0.        ,  0.        ,  0.91450893]))
        self.drillWallWalkFarthestSafeFrame  = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519,  0.        ,  0.        , -0.16124022]))
        self.drillWallWalkBackFrame  = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([-1.16122318,  0.04723203, -0.67493468]), array([ 0.93163145,  0.        ,  0.        , -0.36340451]))
        self.surprisePreWalkFrame  = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497,  0.        ,  0.        , -0.53906374]))
        self.surpriseWalkFrame  = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075,  0.        ,  0.        , -0.16525575]))
        self.surpriseWalkBackFrame  = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977,  0.        ,  0.        , -0.3299461 ]))
        self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)

        t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1.,  0.,  0.,  0.]))
        self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)

        self.frameSync = vis.FrameSync()
        self.frameSync.addFrame(self.originFrame)
        self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)

    def createUtorsoGazeConstraints(self, tspan):
        constraints = []
        g = ik.WorldGazeDirConstraint()
        g.linkName = 'utorso'
        g.targetFrame = vtk.vtkTransform()
        axes = transformUtils.getAxesFromTransform(self.polaris.leftFootEgressOutsideFrame.transform)
        g.targetAxis = axes[0]
        g.bodyAxis = [1,0,0]
        g.coneThreshold = self.coneThreshold
        g.tspan = tspan
        constraints.append(g)

        g = ik.WorldGazeDirConstraint()
        g.linkName = 'utorso'
        g.targetFrame = vtk.vtkTransform()
        g.targetAxis = [0,0,1]
        g.bodyAxis = [0,0,1]
        g.coneThreshold = self.coneThreshold
        g.tspan = tspan
        constraints.append(g)
        return constraints

def getTransformFromNumpy(mat):
    '''
    Given a numpy 4x4 array, return a vtkTransform.
    '''
    m = vtk.vtkMatrix4x4()
    for r in xrange(4):
        for c in xrange(4):
            m.SetElement(r, c, mat[r][c])

    t = vtk.vtkTransform()
    t.SetMatrix(m)
    return t

    def onStartMappingButton(self):
        msg = map_command_t()
        msg.timestamp = getUtime()
        msg.command = 0
        lcmUtils.publish('KINECT_MAP_COMMAND', msg)

        utime = self.queue.getCurrentImageTime('KINECT_RGB')
        self.cameraToLocalInit = vtk.vtkTransform()
        self.queue.getTransform('KINECT_RGB', 'local', utime, self.cameraToLocalInit)
        vis.updateFrame(self.cameraToLocalInit, 'initial cam' )
        print "starting mapping", utime
        print self.cameraToLocalInit.GetPosition()
        print self.cameraToLocalInit.GetOrientation()

def frameFromPositionAndRPY(position, rpy):
    '''
    rpy specified in degrees
    '''

    rpy = [math.radians(deg) for deg in rpy]

    angle, axis = botpy.roll_pitch_yaw_to_angle_axis(rpy)

    t = vtk.vtkTransform()
    t.PostMultiply()
    t.RotateWXYZ(math.degrees(angle), axis)
    t.Translate(position)
    return t

def getTransformFromAxes(xaxis, yaxis, zaxis):

    t = vtk.vtkTransform()
    m = vtk.vtkMatrix4x4()

    axes = np.array([xaxis, yaxis, zaxis]).transpose().copy()
    vtk.vtkMath.Orthogonalize3x3(axes, axes)

    for r in xrange(3):
        for c in xrange(3):
            m.SetElement(r, c, axes[r][c])

    t.SetMatrix(m)
    return t

def transformFromPose(position, quaternion):
    '''
    Returns a vtkTransform
    '''
    rotationMatrix = np.zeros((3,3))
    vtk.vtkMath.QuaternionToMatrix3x3(quaternion, rotationMatrix)

    mat = np.eye(4)
    mat[:3,:3] = rotationMatrix
    mat[:3,3] = position

    t = vtk.vtkTransform()
    t.SetMatrix(mat.flatten())
    return t

    def addTorus(self, radius, thickness, resolution=30):

        q = vtk.vtkSuperquadricSource()
        q.SetToroidal(1)
        q.SetSize(radius)
        q.SetThetaResolution(resolution)
        # thickness doesnt seem to match to Eucliean units. 0 is none. 1 is full. .1 is a good valve
        q.SetThickness(thickness)
        q.Update()

        # rotate Torus so that the hole axis (internally y), is set to be z, which we use for valves
        transform = vtk.vtkTransform()
        transform.RotateWXYZ(90,1,0,0)
        transformFilter=vtk.vtkTransformPolyDataFilter()
        transformFilter.SetTransform(transform)
        transformFilter.SetInputConnection(q.GetOutputPort())
        transformFilter.Update()
        self.addPolyData(transformFilter.GetOutput())

    def addEllipsoid(self, center, radii, color=[1,1,1], alpha=1.0, resolution=24):
        """
        Add an ellipsoid centered at [center] with x, y, and z principal axis radii given by
        radii = [x_scale, y_scale, z_scale]
        """
        sphere = vtk.vtkSphereSource()
        sphere.SetCenter([0,0,0])
        sphere.SetThetaResolution(resolution)
        sphere.SetPhiResolution(resolution)
        sphere.SetRadius(1.0)
        sphere.Update()

        transform = vtk.vtkTransform()
        transform.Translate(center)
        transform.Scale(radii)

        transformFilter = vtk.vtkTransformPolyDataFilter()
        transformFilter.SetTransform(transform)
        transformFilter.SetInputConnection(sphere.GetOutputPort())
        transformFilter.Update()
        self.addPolyData(transformFilter.GetOutput())

    def __init__(self):
        self.aprilTagSubsciber = lcmUtils.addSubscriber('APRIL_TAG_TO_CAMERA_LEFT', lcmbotcore.rigid_transform_t, self.onAprilTag)
        pose = transformUtils.poseFromTransform(vtk.vtkTransform())
        desc = dict(classname='MeshAffordanceItem', Name='polaris',
                    Filename='software/models/polaris/polaris_cropped.vtp', pose=pose)
        self.pointcloudAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
        self.originFrame = self.pointcloudAffordance.getChildFrame()
        self.originToAprilTransform = transformUtils.transformFromPose(np.array([-0.038508  , -0.00282131, -0.01000079]),
            np.array([  9.99997498e-01,  -2.10472556e-03,  -1.33815696e-04, 7.46246794e-04])) # offset for  . . . who knows why

        # t = transformUtils.transformFromPose(np.array([ 0.14376024,  0.95920689,  0.36655712]), np.array([ 0.28745842,  0.90741428, -0.28822068,  0.10438304]))

        t = transformUtils.transformFromPose(np.array([ 0.10873244,  0.93162364,  0.40509084]),
            np.array([ 0.32997378,  0.88498408, -0.31780588,  0.08318602]))
        self.leftFootEgressStartFrame  = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.pointcloudAffordance)


        t = transformUtils.transformFromPose(np.array([ 0.265,  0.874,  0.274]),
                                             np.array([ 0.35290731,  0.93443693, -0.04181263,  0.02314636]))
        self.leftFootEgressMidFrame  = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.pointcloudAffordance)

        t = transformUtils.transformFromPose(np.array([ 0.54339115,  0.89436275,  0.26681047]),
                                             np.array([ 0.34635985,  0.93680077, -0.04152008,  0.02674412]))
        self.leftFootEgressOutsideFrame  = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.pointcloudAffordance)


        # pose = [np.array([-0.78962299,  0.44284877, -0.29539116]), np.array([ 0.54812954,  0.44571517, -0.46063251,  0.53731713])] #old location
        # pose = [np.array([-0.78594663,  0.42026626, -0.23248139]), np.array([ 0.54812954,  0.44571517, -0.46063251,  0.53731713])] # updated location

        pose = [np.array([-0.78594663,  0.42026626, -0.23248139]), np.array([ 0.53047159,  0.46554963, -0.48086192,  0.52022615])] # update orientation

        desc = dict(classname='CapsuleRingAffordanceItem', Name='Steering Wheel', uuid=newUUID(), pose=pose,
                    Color=[1, 0, 0], Radius=float(0.18), Segments=20)
        self.steeringWheelAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)


        pose = [np.array([-0.05907324,  0.80460545,  0.45439687]), np.array([ 0.14288327,  0.685944  , -0.703969  ,  0.11615873])]

        desc = dict(classname='BoxAffordanceItem', Name='pedal', Dimensions=[0.12, 0.33, 0.04], pose=pose, Color=[0,1,0])
        self.pedalAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)


        # t = transformUtils.transformFromPose(np.array([ 0.04045136,  0.96565326,  0.25810111]),
        #     np.array([ 0.26484648,  0.88360091, -0.37065556, -0.10825996]))

        # t = transformUtils.transformFromPose(np.array([ -4.34908919e-04,   9.24901627e-01,   2.65614116e-01]),
        #     np.array([ 0.25022251,  0.913271  , -0.32136359, -0.00708626]))

        t = transformUtils.transformFromPose(np.array([ 0.0384547 ,  0.89273742,  0.24140762]),
            np.array([ 0.26331831,  0.915796  , -0.28055337,  0.11519963]))

        self.leftFootPedalSwingFrame = vis.updateFrame(t,'left foot pedal swing', scale=0.2, visible=True, parent=self.pointcloudAffordance)

        t = transformUtils.transformFromPose(np.array([-0.9012598 , -0.05709763,  0.34897024]),
            np.array([ 0.03879584,  0.98950919,  0.03820214,  0.13381721]))

        self.leftFootDrivingFrame = vis.updateFrame(t,'left foot driving', scale=0.2, visible=True, parent=self.pointcloudAffordance)
        # t = transformUtils.transformFromPose(np.array([-0.12702725,  0.92068409,  0.27209386]),
        #     np.array([ 0.2062255 ,  0.92155886, -0.30781119,  0.11598529]))


        # t = transformUtils.transformFromPose(np.array([-0.14940375,  0.90347275,  0.23812658]),
        #     np.array([ 0.27150909,  0.91398724, -0.28877386,  0.0867167 ]))

        # t = transformUtils.transformFromPose(np.array([-0.17331227,  0.87879312,  0.25645152]),
        #     np.array([ 0.26344489,  0.91567196, -0.28089824,  0.11505581]))
        # self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)


        t = transformUtils.transformFromPose(np.array([-0.12702725,  0.92068409,  0.27209386]),
            np.array([ 0.2062255 ,  0.92155886, -0.30781119,  0.11598529]))
        self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)

        t = transformUtils.transformFromPose(np.array([-0.13194951,  0.89871423,  0.24956246]),
            np.array([ 0.21589082,  0.91727326, -0.30088849,  0.14651633]))
        self.leftFootOnPedal = vis.updateFrame(t,'left foot on pedal', scale=0.2, visible=True, parent=self.pointcloudAffordance)

        t = transformUtils.transformFromPose(np.array([ 0.17712239,  0.87619935,  0.27001509]),
            np.array([ 0.33484372,  0.88280787, -0.31946488,  0.08044963]))

        self.leftFootUpFrame = vis.updateFrame(t,'left foot up frame', scale=0.2, visible=True, parent=self.pointcloudAffordance)


        t = transformUtils.transformFromPose(np.array([ 0.47214939, -0.04856998,  0.01375837]),
            np.array([  6.10521653e-03,   4.18621358e-04,   4.65520611e-01,
         8.85015882e-01]))
        self.rightHandGrabFrame = vis.updateFrame(t,'right hand grab bar', scale=0.2, visible=True, parent=self.pointcloudAffordance)

        self.frameSync = vis.FrameSync()
        self.frameSync.addFrame(self.originFrame)
        self.frameSync.addFrame(self.pointcloudAffordance.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.steeringWheelAffordance.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.pedalAffordance.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootPedalSwingFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootDrivingFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootDrivingKneeInFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootOnPedal, ignoreIncoming=True)
#.........这里部分代码省略.........

 def getThumbToPalmFrame(self):
     return vtk.vtkTransform()

 def createLeftFootPoseConstraint(self, targetFrame, tspan=[-np.inf,np.inf]):
     positionConstraint, orientationConstraint = self.robotSystem.ikPlanner.createPositionOrientationConstraint('l_foot', targetFrame, vtk.vtkTransform())
     positionConstraint.tspan = tspan
     orientationConstraint.tspan = tspan
     return positionConstraint, orientationConstraint

 def onAprilTag(self, msg):
     t = vtk.vtkTransform()
     cameraview.imageManager.queue.getTransform('april_tag_car_beam', 'local', msg.utime, t)
     self.originFrame.copyFrame(transformUtils.concatenateTransforms([self.originToAprilTransform, t]))