예제 #1
0
    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()
예제 #2
0
파일: pfgrasp.py 프로젝트: rdeits/director
    def moveDrill(self,Pos=[0,0,0],RPY=[0,0,0],Style='Local'):
        linkMap = { 'left' : 'l_hand_face', 'right': 'r_hand_face'}
        linkName = linkMap[self.graspingHand]
        
        affordance = om.findObjectByName('drill')
        affordanceReach = om.findObjectByName('reach frame')
        frame = om.findObjectByName('drill frame')

        
        drillTransform = affordance.actor.GetUserTransform()
        reach = transformUtils.copyFrame(affordanceReach.actor.GetUserTransform())
        drillTransformCopy = transformUtils.copyFrame(affordance.actor.GetUserTransform())
        drillToReach = vtkTransform()
        drillToReach.Identity()
        drillToReach.PostMultiply()
        drillToReach.Concatenate(drillTransformCopy)
        drillToReach.Concatenate(reach.GetLinearInverse())
        
        handfaceToWorld = self.ikPlanner.getLinkFrameAtPose(linkName, self.getPlanningStartPose())
        
        # find a transform that move forward wrt hand palm
        delta = transformUtils.frameFromPositionAndRPY(Pos, RPY)
        drillTransform.Identity()
        drillTransform.PostMultiply()
        drillTransform.Concatenate(drillToReach)
        drillTransform.Concatenate(delta)
        drillTransform.Concatenate(handfaceToWorld)
예제 #3
0
 def computeLeftFootOverPlatformFrame(self, startPose, height):
     lFoot2World = transformUtils.copyFrame(self.polaris.leftFootEgressOutsideFrame.transform)
     rFoot2World = self.robotSystem.ikPlanner.getLinkFrameAtPose('r_foot', startPose)
     lFoot2World = transformUtils.copyFrame(rFoot2World)
     lFoot2World.PreMultiply()
     lFoot2World.Translate([0.05, 0.26, height])
     return lFoot2World
예제 #4
0
    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())
예제 #5
0
 def spawnHandAtCurrentLocation(side='left'):
     if (side is 'left'):
         tf = transformUtils.copyFrame( getLinkFrame( 'l_hand_face') )
         handFactory.placeHandModelWithTransform( tf , app.getCurrentView(), 'left')
     else:
         tf = transformUtils.copyFrame( getLinkFrame( 'right_pointer_tip') )
         handFactory.placeHandModelWithTransform( tf , app.getCurrentView(), 'right')
    def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):

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

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

        footsteps = []

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

        lastBlock = blocks[-1]

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

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

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

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

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

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

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

        if not plan:
            return []

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

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

        return footsteps[2:]
예제 #7
0
    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:]
예제 #8
0
 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())
예제 #9
0
    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())
예제 #10
0
    def computeRobotStanceFrame(self, objectTransform,
                                relativeStanceTransform):
        '''
        Given a robot model, determine the height of the ground
        using an XY and Yaw standoff, combined to determine the relative 6DOF standoff
        For a grasp or approach stance
        '''

        groundFrame = self.computeGroundFrame(self.robotModel)
        groundHeight = groundFrame.GetPosition()[2]

        graspPosition = np.array(objectTransform.GetPosition())
        graspYAxis = [0.0, 1.0, 0.0]
        graspZAxis = [0.0, 0.0, 1.0]
        objectTransform.TransformVector(graspYAxis, graspYAxis)
        objectTransform.TransformVector(graspZAxis, graspZAxis)

        xaxis = graspYAxis
        #xaxis = graspZAxis
        zaxis = [0, 0, 1]
        yaxis = np.cross(zaxis, xaxis)
        yaxis /= np.linalg.norm(yaxis)
        xaxis = np.cross(yaxis, zaxis)

        graspGroundTransform = transformUtils.getTransformFromAxes(
            xaxis, yaxis, zaxis)
        graspGroundTransform.PostMultiply()
        graspGroundTransform.Translate(graspPosition[0], graspPosition[1],
                                       groundHeight)

        robotStance = transformUtils.copyFrame(relativeStanceTransform)
        robotStance.Concatenate(graspGroundTransform)

        return robotStance
예제 #11
0
    def planReach(self):
        startPose = self.getPlanningStartPose()

        reachFrame = vtk.vtkTransform()
        reachFrame.Identity()
        reachFrame.PostMultiply()
        if self.graspingHand == 'right':
            reachFrame.Concatenate(
                transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 180, 0]))
        reachFrame.Concatenate(
            transformUtils.copyFrame(
                om.findObjectByName('reach frame').actor.GetUserTransform()))
        constraintSet = self.ikPlanner.planEndEffectorGoal(startPose,
                                                           self.graspingHand,
                                                           reachFrame,
                                                           lockBase=False,
                                                           lockBack=True)

        ## broken robot arm has a new joint limit
        if self.graspingHand == 'left':
            constraintSet.constraints.append(self.createBrokenArmConstraint())

        endPose, info = constraintSet.runIk()
        #print endPose

        if info > 10:
            self.log("in Target received: Bad movement")
            return
        reachPlan = constraintSet.runIkTraj()
예제 #12
0
    def computeStanceFrame(self, useIkTraj=False):
        objectTransform = transformUtils.copyFrame(
            self.computeGraspFrame().transform)
        if useIkTraj:
            startPose = self.getNominalPose()
            plan = self.planInsertTraj(self.speedLow,
                                       lockFeet=False,
                                       lockBase=False,
                                       resetPoses=True,
                                       startPose=startPose)
            stancePose = robotstate.convertStateMessageToDrakePose(
                plan.plan[0])
            stanceRobotModel = self.ikPlanner.getRobotModelAtPose(stancePose)
            self.nominalPelvisXYZ = stancePose[:3]
            robotStance = self.footstepPlanner.getFeetMidPoint(
                stanceRobotModel)
        else:
            robotStance = self.computeRobotStanceFrame(
                objectTransform, self.computeRelativeStanceTransform())

        stanceFrame = vis.updateFrame(robotStance,
                                      'valve grasp stance',
                                      parent=self.getValveAffordance(),
                                      visible=False,
                                      scale=0.2)
        stanceFrame.addToView(app.getDRCView())
        return stanceFrame
예제 #13
0
 def computeRelativeGraspTransform(self):
     t = transformUtils.copyFrame(transformUtils.frameFromPositionAndRPY(self.graspFrameXYZ,
                                                                         self.graspFrameRPY))
     t.PostMultiply()
     t.RotateX(180)
     t.RotateY(-90)
     return t
예제 #14
0
    def computeRobotStanceFrame(self, objectTransform, relativeStanceTransform):
        """
        Given a robot model, determine the height of the ground
        using an XY and Yaw standoff, combined to determine the relative 6DOF standoff
        For a grasp or approach stance
        """

        groundFrame = self.computeGroundFrame(self.robotModel)
        groundHeight = groundFrame.GetPosition()[2]

        graspPosition = np.array(objectTransform.GetPosition())
        graspYAxis = [0.0, 1.0, 0.0]
        graspZAxis = [0.0, 0.0, 1.0]
        objectTransform.TransformVector(graspYAxis, graspYAxis)
        objectTransform.TransformVector(graspZAxis, graspZAxis)

        xaxis = graspYAxis
        # xaxis = graspZAxis
        zaxis = [0, 0, 1]
        yaxis = np.cross(zaxis, xaxis)
        yaxis /= np.linalg.norm(yaxis)
        xaxis = np.cross(yaxis, zaxis)

        graspGroundTransform = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
        graspGroundTransform.PostMultiply()
        graspGroundTransform.Translate(graspPosition[0], graspPosition[1], groundHeight)

        robotStance = transformUtils.copyFrame(relativeStanceTransform)
        robotStance.Concatenate(graspGroundTransform)

        return robotStance
예제 #15
0
 def getFrameToOriginTransform(self, t):
     tCopy = transformUtils.copyFrame(t)
     tCopy.PostMultiply()
     tCopy.Concatenate(
         self.polaris.originFrame.transform.GetLinearInverse())
     print transformUtils.poseFromTransform(tCopy)
     return tCopy
예제 #16
0
    def flipHandSide():
        for obj in [pickedObj] + pickedObj.children():
            if not isGraspSeed(obj):
                continue
            side = 'right' if obj.side == 'left' else 'left'
            obj.side = side
            color = [1.0, 1.0, 0.0]
            if side == 'right':
                color = [0.33, 1.0, 0.0]
            obj.setProperty('Color', color)

            polyData = handFactory.getNewHandPolyData(side)
            obj.setPolyData(polyData)

            handFrame = obj.children()[0]
            t = transformUtils.copyFrame(handFrame.transform)
            t.PreMultiply()
            t.RotateY(180)
            handFrame.copyFrame(t)

            objName = obj.getProperty('Name')
            frameName = handFrame.getProperty('Name')
            if side == 'left':
                obj.setProperty('Name', objName.replace("right", "left"))
                handFrame.setProperty('Name', frameName.replace("right", "left"))
            else:
                obj.setProperty('Name', objName.replace("left", "right"))
                handFrame.setProperty('Name', frameName.replace("left", "right"))
            obj._renderAllViews()
예제 #17
0
    def updateObjectInCamera(self, obj, cameraObj):

        imageView = self.imageView

        objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform()
                                               or vtk.vtkTransform())

        localToCameraT = vtk.vtkTransform()
        self.imageQueue.getTransform('local', imageView.imageName,
                                     localToCameraT)

        t = vtk.vtkTransform()
        t.PostMultiply()
        t.Concatenate(objToLocalT)
        t.Concatenate(localToCameraT)

        pd = filterUtils.transformPolyData(obj.polyData, t)
        '''
        normals = pd.GetPointData().GetNormals()
        cameraToImageT = vtk.vtkTransform()
        imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
        pd = filterUtils.transformPolyData(pd, cameraToImageT)
        pts = vnp.getNumpyFromVtk(pd, 'Points')
        pts[:,0] /= pts[:,2]
        pts[:,1] /= pts[:,2]
        pd.GetPointData().SetNormals(normals)
        '''

        self.imageQueue.projectPoints(imageView.imageName, pd)

        cameraObj.setPolyData(pd)

        self.addActorToImageOverlay(cameraObj, imageView)
예제 #18
0
    def promotePolyDataItem(cls, obj):
        parent = obj.parent()
        view = obj.views[0]
        name = obj.getProperty('Name')
        polyData = obj.polyData
        props = obj.properties._properties
        childFrame = obj.getChildFrame()
        if childFrame:
            t = transformUtils.copyFrame(childFrame.transform)
        else:
            t = vtk.vtkTransform()
            t.PostMultiply()
            t.Translate(filterUtils.computeCentroid(polyData))
            polyData = filterUtils.transformPolyData(polyData,
                                                     t.GetLinearInverse())

        children = [c for c in obj.children() if c is not childFrame]

        meshId = cls.getMeshManager().add(polyData)

        om.removeFromObjectModel(obj)
        obj = MeshAffordanceItem(name, view)
        obj.setProperty('Filename', meshId)
        om.addToObjectModel(obj, parentObj=parent)
        frame = vis.addChildFrame(obj)
        frame.copyFrame(t)

        for child in children:
            om.addToObjectModel(child, parentObj=obj)

        obj.syncProperties(props)
        return obj
예제 #19
0
 def getFootstepRelativeTransform(self):
     self.footstepsToPlan = []
     for n in xrange(1,self.numFootsteps + 1):
         stepFrameName = 'step ' + str(n) + ' frame'
         fToWorld = transformUtils.copyFrame(om.findObjectByName(stepFrameName).transform)
         fToPlan = self.getTransformToPlanningFrame(fToWorld)
         self.footstepsToPlan.append(fToPlan)
예제 #20
0
    def run(self):

        self.ikPlanner = robotSystem.ikPlanner
        side = self.properties.getPropertyEnumValue('Side').lower()
        self.graspingHand = side

        targetPoints = self.getAnnotationInputPoints()
        gazeTargetFrame = self.getGazeTargetFrame()

        self.initGazeConstraintSet(gazeTargetFrame)

        numberOfSamples = len(targetPoints)

        for i in xrange(numberOfSamples):
            targetPos = targetPoints[i]
            targetFrame = transformUtils.copyFrame(gazeTargetFrame.transform)
            targetFrame.Translate(targetPos - np.array(targetFrame.GetPosition()))
            self.appendPositionConstraintForTargetFrame(targetFrame, i+1)

        gazeConstraint = self.constraintSet.constraints[0]
        assert isinstance(gazeConstraint, ikplanner.ik.WorldGazeDirConstraint)
        gazeConstraint.tspan = [1.0, numberOfSamples]


        plan = self.constraintSet.runIkTraj()

        _addPlanItem(plan, '%s gaze plan' % gazeTargetFrame.getProperty('Name'), ManipulationPlanItem)
예제 #21
0
파일: pfgrasp.py 프로젝트: rdeits/director
    def planGraspLineMotion(self):
        self.turnPointwiseOffSlow()
        startPose = self.getPlanningStartPose()

        graspFrame = vtk.vtkTransform()
        graspFrame.Identity()
        graspFrame.PostMultiply()
        if self.graspingHand == 'right':
            graspFrame.Concatenate(transformUtils.frameFromPositionAndRPY([0,0,0], [0,180,0]))
        graspFrame.Concatenate(transformUtils.copyFrame(om.findObjectByName('grasp frame').actor.GetUserTransform()))

        constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, graspFrame, \
                                                            lockBase=False, lockBack=True)
        # constraint orientation
        p,q = self.ikPlanner.createPositionOrientationGraspConstraints(self.graspingHand, graspFrame)
        q.tspan=[0.5,1]
        
        constraintSet.constraints.append(q)
        
        # constraint line axis 
        positionConstraint, orientationConstraint, axisConstraint = self.ikPlanner.createMoveOnLineConstraints(startPose, graspFrame)
        
        ## broken robot arm has a new joint limit
        if self.graspingHand == 'left':
            constraintSet.constraints.append(self.createBrokenArmConstraint())
        
        constraintSet.constraints.append(axisConstraint)
        constraintSet.constraints[-1].tspan = [0.5,np.inf]
        endPose, info = constraintSet.runIk()
        #print endPose
        if info > 10:
            self.log("in Target received: Bad movement")
            return
        graspPlan = constraintSet.runIkTraj()
예제 #22
0
    def getPinchToPalmFrame(self):
        pinchToPalm = transformUtils.copyFrame(self.getPinchToHandFrame())
        palmToHand = self.ikPlanner.getPalmToHandLink(side='right')
        pinchToPalm.PostMultiply()
        pinchToPalm.Concatenate(palmToHand.GetLinearInverse())

        return pinchToPalm
예제 #23
0
    def run(self):

        self.ikPlanner = robotSystem.ikPlanner
        side = self.properties.getPropertyEnumValue('Side').lower()
        self.graspingHand = side

        targetPoints = self.getAnnotationInputPoints()
        gazeTargetFrame = self.getGazeTargetFrame()

        self.initGazeConstraintSet(gazeTargetFrame)

        numberOfSamples = len(targetPoints)

        for i in xrange(numberOfSamples):
            targetPos = targetPoints[i]
            targetFrame = transformUtils.copyFrame(gazeTargetFrame.transform)
            targetFrame.Translate(targetPos -
                                  np.array(targetFrame.GetPosition()))
            self.appendPositionConstraintForTargetFrame(targetFrame, i + 1)

        gazeConstraint = self.constraintSet.constraints[0]
        assert isinstance(gazeConstraint, ikplanner.ik.WorldGazeDirConstraint)
        gazeConstraint.tspan = [1.0, numberOfSamples]

        plan = self.constraintSet.runIkTraj()

        _addPlanItem(plan,
                     '%s gaze plan' % gazeTargetFrame.getProperty('Name'),
                     ManipulationPlanItem)
예제 #24
0
    def promotePolyDataItem(cls, obj):
        parent = obj.parent()
        view = obj.views[0]
        name = obj.getProperty('Name')
        polyData = obj.polyData
        props = obj.properties._properties
        childFrame = obj.getChildFrame()
        if childFrame:
            t = transformUtils.copyFrame(childFrame.transform)
        else:
            t = vtk.vtkTransform()
            t.PostMultiply()
            t.Translate(filterUtils.computeCentroid(polyData))
            polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())

        children = [c for c in obj.children() if c is not childFrame]

        meshId = cls.getMeshManager().add(polyData)

        om.removeFromObjectModel(obj)
        obj = MeshAffordanceItem(name, view)
        obj.setProperty('Filename', meshId)
        om.addToObjectModel(obj, parentObj=parent)
        frame = vis.addChildFrame(obj)
        frame.copyFrame(t)

        for child in children:
            om.addToObjectModel(child, parentObj=obj)

        obj.syncProperties(props)
        return obj
예제 #25
0
    def fitSwitchBox(self, polyData, points):
        boxPosition = points[0]
        wallPoint = points[1]


        # find a frame that is aligned with wall
        searchRadius = 0.2
        planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0)

        obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False)
        obj.setProperty('Point Size', 7)

        viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
        if np.dot(normal, viewDirection) < 0:
            normal = -normal

        origin = segmentation.computeCentroid(planePoints)

        zaxis = [0,0,1]
        xaxis = normal
        yaxis = np.cross(zaxis, xaxis)
        yaxis /= np.linalg.norm(yaxis)
        zaxis = np.cross(xaxis, yaxis)
        zaxis /= np.linalg.norm(zaxis)

        t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)

        # translate that frame to the box position
        t.PostMultiply()
        t.Translate(boxPosition)

        boxFrame = transformUtils.copyFrame(t)
        self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
예제 #26
0
 def flipHandThumb():
     handFrame = pickedObj.children()[0]
     t = transformUtils.copyFrame(handFrame.transform)
     t.PreMultiply()
     t.RotateY(180)
     handFrame.copyFrame(t)
     pickedObj._renderAllViews()
예제 #27
0
    def getPinchToPalmFrame(self):
        pinchToPalm = transformUtils.copyFrame(self.getPinchToHandFrame())
        palmToHand = self.ikPlanner.getPalmToHandLink(side='right')
        pinchToPalm.PostMultiply()
        pinchToPalm.Concatenate(palmToHand.GetLinearInverse())

        return pinchToPalm
예제 #28
0
    def updateReachFrame(self):
        graspFrame = transformUtils.copyFrame(self.pinchToBox)
        boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform

        graspFrame.PostMultiply()
        graspFrame.Concatenate(boxFrame)
        vis.updateFrame(graspFrame, 'pinch reach frame', scale=0.2)
예제 #29
0
    def updateObjectInCamera(self, obj, cameraObj):

        imageView = self.imageView

        objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())

        localToCameraT = vtk.vtkTransform()
        self.imageQueue.getTransform('local', imageView.imageName, localToCameraT)

        t = vtk.vtkTransform()
        t.PostMultiply()
        t.Concatenate(objToLocalT)
        t.Concatenate(localToCameraT)

        pd = filterUtils.transformPolyData(obj.polyData, t)

        '''
        normals = pd.GetPointData().GetNormals()
        cameraToImageT = vtk.vtkTransform()
        imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
        pd = filterUtils.transformPolyData(pd, cameraToImageT)
        pts = vnp.getNumpyFromVtk(pd, 'Points')
        pts[:,0] /= pts[:,2]
        pts[:,1] /= pts[:,2]
        pd.GetPointData().SetNormals(normals)
        '''

        self.imageQueue.projectPoints(imageView.imageName, pd)

        cameraObj.setPolyData(pd)

        self.addActorToImageOverlay(cameraObj, imageView)
예제 #30
0
    def fitSwitchBox(self, polyData, points):
        boxPosition = points[0]
        wallPoint = points[1]


        # find a frame that is aligned with wall
        searchRadius = 0.2
        planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0)

        obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False)
        obj.setProperty('Point Size', 7)

        viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
        if np.dot(normal, viewDirection) < 0:
            normal = -normal

        origin = segmentation.computeCentroid(planePoints)

        zaxis = [0,0,1]
        xaxis = normal
        yaxis = np.cross(zaxis, xaxis)
        yaxis /= np.linalg.norm(yaxis)
        zaxis = np.cross(xaxis, yaxis)
        zaxis /= np.linalg.norm(zaxis)

        t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)

        # translate that frame to the box position
        t.PostMultiply()
        t.Translate(boxPosition)

        boxFrame = transformUtils.copyFrame(t)
        self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
예제 #31
0
    def makeStepMessages(self, stepFrames, leadingFoot, snapToTerrain=False):

        assert leadingFoot in ('left', 'right')
        isRightFootOffset = 0 if leadingFoot == 'left' else 1

        footOriginToSole = -np.mean(FootstepsDriver.getContactPts(), axis=0)

        stepMessages = []
        for i, stepFrame in enumerate(stepFrames):

            t = transformUtils.copyFrame(stepFrame)
            t.PreMultiply()
            t.Translate(footOriginToSole)

            step = lcmdrc.footstep_t()
            step.pos = transformUtils.positionMessageFromFrame(t)
            step.is_right_foot = (i + isRightFootOffset) % 2
            step.params = self.footstepsDriver.getDefaultStepParams()

            step.fixed_x = True
            step.fixed_y = True
            step.fixed_z = True
            step.fixed_roll = True
            step.fixed_pitch = True
            step.fixed_yaw = True

            if snapToTerrain:
                step.fixed_z = False
                step.fixed_roll = False
                step.fixed_pitch = False

            stepMessages.append(step)

        return stepMessages
예제 #32
0
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
예제 #33
0
    def updateReachFrame(self):
        graspFrame = transformUtils.copyFrame(self.pinchToBox)
        boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform

        graspFrame.PostMultiply()
        graspFrame.Concatenate(boxFrame)
        vis.updateFrame(graspFrame, 'pinch reach frame', scale=0.2)
예제 #34
0
 def computeRelativeGraspTransform(self):
     t = transformUtils.copyFrame(
         transformUtils.frameFromPositionAndRPY(self.graspFrameXYZ,
                                                self.graspFrameRPY))
     t.PostMultiply()
     t.RotateX(180)
     t.RotateY(-90)
     return t
예제 #35
0
 def getFootstepRelativeTransform(self):
     self.footstepsToPlan = []
     for n in xrange(1, self.numFootsteps + 1):
         stepFrameName = 'step ' + str(n) + ' frame'
         fToWorld = transformUtils.copyFrame(
             om.findObjectByName(stepFrameName).transform)
         fToPlan = self.getTransformToPlanningFrame(fToWorld)
         self.footstepsToPlan.append(fToPlan)
예제 #36
0
 def computeNextRoomFrame(self):
     assert self.targetAffordance
     t = transformUtils.frameFromPositionAndRPY(self.nextPosition,
                                                [0, 0, 0])
     self.faceTransformLocal = transformUtils.copyFrame(t)  # copy required
     t.Concatenate(self.targetFrame)
     self.faceFrameDesired = vis.showFrame(t,
                                           'target frame desired',
                                           parent=self.targetAffordance,
                                           visible=False,
                                           scale=0.2)
예제 #37
0
    def _computeBaseTransform(self, frame):

        currentDelta = None
        for frameId, frameData in self.frames.items():

            if frameData.ref() is None:
                self._removeFrameId(frameId)
            elif frameData.ref() is frame:
                continue
            else:
                currentDelta = transformUtils.copyFrame(frameData.baseTransform.GetLinearInverse())
                currentDelta.Concatenate(transformUtils.copyFrame(frameData.ref().transform))
                break

        t = transformUtils.copyFrame(frame.transform)
        t.PostMultiply()
        if currentDelta:
            t.Concatenate(currentDelta.GetLinearInverse())

        return t
예제 #38
0
 def cycleEndEffector(self):
     if len(self.endEffectorFrames) is not 0:
         self.clearKeyframePoses()
         self.endEffectorFrames[self.endEffectorIndex].setProperty('Edit', False)
         self.endEffectorIndex = (self.endEffectorIndex + 1) % len(self.endEffectorFrames)
         self.endEffectorFrames[self.endEffectorIndex].setProperty('Edit', True)
         om.setActiveObject(self.endEffectorFrames[self.endEffectorIndex])
         self.baseFrame = self.endEffectorFrames[self.endEffectorIndex]
         self.baseTransform = transformUtils.copyFrame(self.baseFrame.transform)
         self.resetDeltas()
         self.addKeyframePose()
예제 #39
0
    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)
예제 #40
0
 def addNewFrame():
     t = transformUtils.copyFrame(affordanceObj.getChildFrame().transform)
     t.PostMultiply()
     t.Translate(np.array(pickedPoint) - np.array(t.GetPosition()))
     newFrame = vis.showFrame(
         t,
         '%s frame %d' %
         (affordanceObj.getProperty('Name'), len(affordanceObj.children())),
         scale=0.2,
         parent=affordanceObj)
     affordanceObj.getChildFrame().getFrameSync().addFrame(
         newFrame, ignoreIncoming=True)
예제 #41
0
    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())
예제 #42
0
    def computeTableStanceFrame(self):
        assert self.tableData

        zGround = 0.0
        tableHeight = self.tableData.frame.GetPosition()[2] - zGround

        t = transformUtils.copyFrame(self.tableData.frame)
        t.PreMultiply()
        t1 = transformUtils.frameFromPositionAndRPY([-x/2 for x in self.tableData.dims],[0,0,0])
        t.Concatenate(t1)
        t2 = transformUtils.frameFromPositionAndRPY([-0.35, self.tableData.dims[1]*0.5, -tableHeight],[0,0,0])
        t.Concatenate(t2)

        self.tableStanceFrame = vis.showFrame(t, 'table stance frame', parent=self.tableObj, scale=0.2)
예제 #43
0
    def run(self):
        aff = self.getSelectedAffordance()
        affFrame = aff.getChildFrame().transform

        groundFrame = self.getGroundFrame().transform

        projectedXYZ = np.hstack([affFrame.GetPosition()[0:2], groundFrame.GetPosition()[2]])

        result = transformUtils.copyFrame(affFrame)
        result.Translate(projectedXYZ - np.array(result.GetPosition()))

        outputName = self.properties.getProperty('Frame output name')
        outputName = outputName or '%s ground frame' % aff.getProperty('Name')

        vis.updateFrame(result, outputName, scale=0.2)
예제 #44
0
 def cycleEndEffector(self):
     if len(self.endEffectorFrames) is not 0:
         self.clearKeyframePoses()
         self.endEffectorFrames[self.endEffectorIndex].setProperty(
             'Edit', False)
         self.endEffectorIndex = (self.endEffectorIndex + 1) % len(
             self.endEffectorFrames)
         self.endEffectorFrames[self.endEffectorIndex].setProperty(
             'Edit', True)
         om.setActiveObject(self.endEffectorFrames[self.endEffectorIndex])
         self.baseFrame = self.endEffectorFrames[self.endEffectorIndex]
         self.baseTransform = transformUtils.copyFrame(
             self.baseFrame.transform)
         self.resetDeltas()
         self.addKeyframePose()
예제 #45
0
    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)
예제 #46
0
    def spawnRunningBoardAffordance(self):

        boxDimensions = [0.4, 1.0, 0.05]
        stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)

        boxFrame = transformUtils.copyFrame(stanceFrame)
        boxFrame.PreMultiply()
        boxFrame.Translate(0.0, 0.0, -boxDimensions[2]/2.0)

        box = om.findObjectByName('running board')
        if not box:
            pose = transformUtils.poseFromTransform(boxFrame)
            desc = dict(classname='BoxAffordanceItem', Name='running board', Dimensions=boxDimensions, pose=pose)
            box = self.robotSystem.affordanceManager.newAffordanceFromDescription(desc)

        return box
예제 #47
0
    def drawObjectInCamera(objectName,visible=True):
        v = imageView.view
        q = cameraview.imageManager.queue
        localToCameraT = vtk.vtkTransform()
        q.getTransform('local', 'CAMERA_LEFT', localToCameraT)

        obj = om.findObjectByName(objectName)
        if obj is None:
            return
        objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
        objPolyDataOriginal = obj.polyData
        pd = objPolyDataOriginal
        pd = filterUtils.transformPolyData(pd, objToLocalT)
        pd = filterUtils.transformPolyData(pd, localToCameraT)
        q.projectPoints('CAMERA_LEFT', pd)
        vis.showPolyData(pd, ('overlay ' + objectName), view=v, color=[0,1,0],parent='camera overlay',visible=visible)
예제 #48
0
    def spawnFootstepFrame(self):
        # should snap this to
        boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform

        goalFrame = transformUtils.copyFrame(self.footToBox)
        goalFrame.PostMultiply()
        goalFrame.Concatenate(boxFrame)


        # translate goal frame to match current robot height
        stanceFrame = self.getStanceFrame()
        stanceHeight = stanceFrame.GetPosition()[2]
        goalHeight = goalFrame.GetPosition()[2]

        goalFrame.PreMultiply()
        goalFrame.Translate(0.0, 0.0, stanceHeight - goalHeight)
        vis.updateFrame(goalFrame, 'switch box stance frame', scale=0.2)