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 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 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)
示例#4
0
    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(), color)
示例#5
0
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)
示例#6
0
 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]))
示例#7
0
    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()
示例#8
0
    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 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)
示例#10
0
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)
示例#11
0
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
示例#12
0
 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 computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):

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

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

        footsteps = []

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

        lastBlock = blocks[-1]

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

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

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

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

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

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

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

        if not plan:
            return []

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

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

        return footsteps[2:]
    def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):

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

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

        footsteps = []

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

        lastBlock = blocks[-1]

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

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

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

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

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

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

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

        if not plan:
            return []

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

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

        return footsteps[2:]
示例#15
0
    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)
示例#16
0
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
示例#17
0
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
示例#18
0
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
示例#19
0
    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
示例#20
0
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
示例#21
0
    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()
示例#22
0
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
示例#23
0
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
示例#24
0
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
示例#25
0
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
示例#26
0
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
示例#27
0
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
示例#28
0
    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())
示例#29
0
    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())
示例#30
0
def copyFrame(transform):
    t = vtk.vtkTransform()
    t.PostMultiply()
    t.SetMatrix(transform.GetMatrix())
    return t
示例#31
0
    def __init__(self):
        self.aprilTagSubsciber = lcmUtils.addSubscriber('APRIL_TAG_TO_CAMERA_LEFT', lcmbotcore.rigid_transform_t, self.onAprilTag)
        pose = transformUtils.poseFromTransform(vtk.vtkTransform())
        desc = dict(classname='MeshAffordanceItem', Name='polaris',
                    Filename='software/models/polaris/polaris_cropped.vtp', pose=pose)
        self.pointcloudAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
        self.originFrame = self.pointcloudAffordance.getChildFrame()
        self.originToAprilTransform = transformUtils.transformFromPose(np.array([-0.038508  , -0.00282131, -0.01000079]),
            np.array([  9.99997498e-01,  -2.10472556e-03,  -1.33815696e-04, 7.46246794e-04])) # offset for  . . . who knows why

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

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


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

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


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

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

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


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

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


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

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

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

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

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

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


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

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


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

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

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

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


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

        self.frameSync = vis.FrameSync()
        self.frameSync.addFrame(self.originFrame)
        self.frameSync.addFrame(self.pointcloudAffordance.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.steeringWheelAffordance.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.pedalAffordance.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootPedalSwingFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootDrivingFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootDrivingKneeInFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootOnPedal, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootUpFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.rightHandGrabFrame, ignoreIncoming=True)
示例#32
0
 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
示例#33
0
 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]))
示例#34
0
def copyFrame(transform):
    t = vtk.vtkTransform()
    t.PostMultiply()
    t.SetMatrix(transform.GetMatrix())
    return t
示例#35
0
 def getThumbToPalmFrame(self):
     return vtk.vtkTransform()
示例#36
0
    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 = transformUtils.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)
示例#37
0
    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 = transformUtils.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)
示例#38
0
 def getThumbToPalmFrame(self):
     return vtk.vtkTransform()