Beispiel #1
0
    def getNominalPose(self):
        axes = transformUtils.getAxesFromTransform(
            self.computeGraspFrame().transform)
        yaxis = axes[1]
        yawDesired = np.arctan2(yaxis[1], yaxis[0])
        seedDistance = 1

        nominalPose = self.ikPlanner.jointController.getPose('q_nom')
        nominalPose[0] = (self.computeGraspFrame().transform.GetPosition()[0] -
                          seedDistance * yaxis[0])
        nominalPose[1] = (self.computeGraspFrame().transform.GetPosition()[1] -
                          seedDistance * yaxis[1])
        nominalPose[5] = yawDesired
        if self.scribeDirection == 1:  # Clockwise
            nominalPose = self.ikPlanner.getMergedPostureFromDatabase(
                nominalPose,
                'valve',
                'reach-nominal-cw',
                side=self.graspingHand)
        else:  # Counter-clockwise
            nominalPose = self.ikPlanner.getMergedPostureFromDatabase(
                nominalPose,
                'valve',
                'reach-nominal-ccw',
                side=self.graspingHand)
        return nominalPose
Beispiel #2
0
    def planInsertTraj(self, speed, lockFeet=True, lockBase=None, resetBase=False, wristAngleCW=0,
                       startPose=None, verticalOffset=0.01, usePoses=False, resetPoses=True,
                       planFromCurrentRobotState=False, retract=False):

        ikParameters = IkParameters(usePointwise=False, maxDegreesPerSecond=speed,
                                    numberOfAddedKnots=1,
                                    quasiStaticShrinkFactor=self.quasiStaticShrinkFactor,
                                    fixInitialState=planFromCurrentRobotState)

        _, yaxis, _ = transformUtils.getAxesFromTransform(self.computeGraspFrame().transform)
        yawDesired = np.arctan2(yaxis[1], yaxis[0])

        if startPose is None:
            startPose = self.getPlanningStartPose()

        nominalPose = self.getNominalPose()

        self.ikPlanner.addPose(nominalPose, self.nominalPoseName)
        self.ikPlanner.addPose(startPose, self.startPoseName)

        self.ikPlanner.reachingSide = self.graspingHand

        constraints = []
        constraints.extend(self.createBaseConstraints(resetBase, lockBase, lockFeet, yawDesired))
        constraints.append(self.createBackPostureConstraint())
        constraints.append(self.ikPlanner.createQuasiStaticConstraint())
        constraints.extend(self.createFootConstraints(lockFeet))
        constraints.append(self.ikPlanner.createLockedArmPostureConstraint(self.startPoseName))
        constraints.append(self.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
        constraints.append(self.createElbowPostureConstraint())
        constraints.append(self.createStaticTorqueConstraint())
        constraints.append(self.createHandGazeConstraint())
        constraints.append(self.createHandFixedOrientConstraint())
        constraints.append(self.createWristAngleConstraint(wristAngleCW,
                                                           planFromCurrentRobotState))
        constraints.extend(self.createAllHandPositionConstraints(self.coaxialTol, retract))

        if retract:
            startPoseName = self.getStartPoseName(planFromCurrentRobotState, True, usePoses)
            endPoseName = self.getEndPoseName(True, usePoses)
            endPose = self.ikPlanner.jointController.getPose(endPoseName)
            endPose = self.ikPlanner.mergePostures(endPose, robotstate.matchJoints('lwy'), startPose)
            endPoseName = 'q_retract'
            self.ikPlanner.addPose(endPose, endPoseName)
        else:
            startPoseName = self.getStartPoseName(planFromCurrentRobotState, retract, usePoses)
            endPoseName = self.getEndPoseName(retract, usePoses)

        plan = self.ikPlanner.runIkTraj(constraints, startPoseName, endPoseName, self.nominalPoseName, ikParameters=ikParameters)

        if resetPoses and not retract and max(plan.plan_info) <= 10:
            self.setReachAndTouchPoses(plan)

        return plan
def updateCamera(msg):
    T = transformUtils.frameFromPositionAndRPY(msg.q[:3], np.degrees(msg.q[3:]))
    axes = transformUtils.getAxesFromTransform(T)

    # vis.updateFrame(T, 'vicon camera')
    # return

    camera = app.view.camera()
    camera.SetPosition(T.GetPosition())
    camera.SetFocalPoint(np.array(T.GetPosition()) + axes[0])
    camera.SetViewUp(axes[2])
    camera.SetViewAngle(122.6)
    app.view.render()
def updateCamera(msg):
    T = transformUtils.frameFromPositionAndRPY(msg.q[:3],np.degrees(msg.q[3:]))
    axes = transformUtils.getAxesFromTransform(T)
   
    #vis.updateFrame(T, 'vicon camera')
    #return

    camera = app.view.camera()
    camera.SetPosition(T.GetPosition())
    camera.SetFocalPoint(np.array(T.GetPosition())+axes[0])
    camera.SetViewUp(axes[2])
    camera.SetViewAngle(122.6)
    app.view.render()
Beispiel #5
0
    def getNominalPose(self):
        axes = transformUtils.getAxesFromTransform(self.computeGraspFrame().transform)
        yaxis = axes[1]
        yawDesired = np.arctan2(yaxis[1], yaxis[0])
        seedDistance = 1

        nominalPose = self.ikPlanner.jointController.getPose('q_nom')
        nominalPose[0] = (self.computeGraspFrame().transform.GetPosition()[0] -
                          seedDistance*yaxis[0])
        nominalPose[1] = (self.computeGraspFrame().transform.GetPosition()[1] -
                          seedDistance*yaxis[1])
        nominalPose[5] = yawDesired
        if self.scribeDirection == 1: # Clockwise
            nominalPose = self.ikPlanner.getMergedPostureFromDatabase(nominalPose, 'valve', 'reach-nominal-cw', side=self.graspingHand)
        else: # Counter-clockwise
            nominalPose = self.ikPlanner.getMergedPostureFromDatabase(nominalPose, 'valve', 'reach-nominal-ccw', side=self.graspingHand)
        return nominalPose
def testTransform():
    '''
    test transformFromPose --> getAxesFromTransform is same as quat --> matrix
    '''

    quat = transformations.random_quaternion()
    pos = np.random.rand(3)

    frame = transformUtils.transformFromPose(pos, quat)
    axes = transformUtils.getAxesFromTransform(frame)
    mat = transformUtils.getNumpyFromTransform(frame)

    assert np.allclose(mat[:3, :3], np.array(axes).transpose())

    mat2 = transformations.quaternion_matrix(quat)
    mat2[:3, 3] = pos

    print mat
    print mat2
    assert np.allclose(mat, mat2)
Beispiel #7
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
Beispiel #8
0
    def addCollisionObject(self, obj):
        if om.getOrCreateContainer('affordances').findChild(obj.getProperty('Name') + ' affordance'):
            return # Affordance has been created previously

        frame = obj.findChild(obj.getProperty('Name') + ' frame')
        (origin, quat) = transformUtils.poseFromTransform(frame.transform)
        (xaxis, yaxis, zaxis) = transformUtils.getAxesFromTransform(frame.transform)

        # TODO: move this into transformUtils as getAxisDimensions or so
        box = obj.findChild(obj.getProperty('Name') + ' box')
        box_np = vtkNumpy.getNumpyFromVtk(box.polyData, 'Points')
        box_min = np.amin(box_np, 0)
        box_max = np.amax(box_np, 0)
        xwidth = np.linalg.norm(box_max[0]-box_min[0])
        ywidth = np.linalg.norm(box_max[1]-box_min[1])
        zwidth = np.linalg.norm(box_max[2]-box_min[2])
        name = obj.getProperty('Name') + ' affordance'

        boxAffordance = segmentation.createBlockAffordance(origin, xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, name, parent='affordances')
        boxAffordance.setSolidColor(obj.getProperty('Color'))
        boxAffordance.setProperty('Alpha', 0.3)
    def getCorners(self):
        '''
        Return a 4x3 numpy array representing the world xyz positions of the
        four corners of the block top.  Corners are listed clockwise from far right.
        '''


        width = self.rectWidth
        depth = self.rectDepth

        width = max(width, 0.39)
        #depth = max(depth, 0.38)

        xaxis, yaxis, zaxis = transformUtils.getAxesFromTransform(self.cornerTransform)
        xedge = np.array(xaxis)*depth
        yedge = np.array(yaxis)*width

        c1 = np.array(self.cornerTransform.GetPosition()) + (np.array(yaxis)*self.rectWidth*0.5) - yedge*0.5
        c2 = c1 - xedge
        c3 = c1 - xedge + yedge
        c4 = c1 + yedge

        return np.array([c3, c4, c1, c2])
    def getCorners(self):
        '''
        Return a 4x3 numpy array representing the world xyz positions of the
        four corners of the block top.  Corners are listed clockwise from far right.
        '''


        width = self.rectWidth
        depth = self.rectDepth

        width = max(width, 0.39)
        #depth = max(depth, 0.38)

        xaxis, yaxis, zaxis = transformUtils.getAxesFromTransform(self.cornerTransform)
        xedge = np.array(xaxis)*depth
        yedge = np.array(yaxis)*width

        c1 = np.array(self.cornerTransform.GetPosition()) + (np.array(yaxis)*self.rectWidth*0.5) - yedge*0.5
        c2 = c1 - xedge
        c3 = c1 - xedge + yedge
        c4 = c1 + yedge

        return np.array([c3, c4, c1, c2])
    def fitRunningBoardAtFeet(self):

        # get stance frame
        startPose = self.getPlanningStartPose()
        stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
        stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)

        # get pointcloud and extract search region covering the running board
        polyData = segmentation.getCurrentRevolutionData()
        polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
        _, polyData = segmentation.removeGround(polyData)
        polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1])

        if not polyData.GetNumberOfPoints():
            print 'empty search region point cloud'
            return

        vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False)

        # extract maximal points along the stance x axis
        perpAxis = stanceFrameAxes[0]
        edgeAxis = stanceFrameAxes[1]
        edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
        edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
        vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True)

        # ransac fit a line to the edge points
        linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints)
        if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
            lineDirection = -lineDirection

        linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0])
        dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection)
        p1 = linePoint + lineDirection*np.min(dists)
        p2 = linePoint + lineDirection*np.max(dists)
        vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False)


        # compute a new frame that is in plane with the stance frame
        # and matches the orientation and position of the detected edge
        origin = np.array(stanceFrame.GetPosition())
        normal = np.array(stanceFrameAxes[2])

        # project stance origin to edge, then back to foot frame
        originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection)
        originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal)
        zaxis = np.array(stanceFrameAxes[2])
        yaxis = np.array(lineDirection)
        xaxis = np.cross(yaxis, zaxis)
        xaxis /= np.linalg.norm(xaxis)
        yaxis = np.cross(zaxis, xaxis)
        yaxis /= np.linalg.norm(yaxis)

        d = DebugData()
        d.addSphere(p1, radius=0.005)
        d.addSphere(p2, radius=0.005)
        d.addLine(p1, p2)
        d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0])
        d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0])
        d.addLine(originProjectedToPlane, origin, color=[0,1,0])
        d.addLine(originProjectedToEdge, origin, color=[1,0,0])
        vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False)

        # update the running board box affordance position and orientation to
        # fit the detected edge
        box = self.spawnRunningBoardAffordance()
        boxDimensions = box.getProperty('Dimensions')
        t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane)
        t.PreMultiply()
        t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0)
        box.getChildFrame().copyFrame(t)

        self.initialize()
Beispiel #12
0
    def planInsertTraj(self,
                       speed,
                       lockFeet=True,
                       lockBase=None,
                       resetBase=False,
                       wristAngleCW=0,
                       startPose=None,
                       verticalOffset=0.01,
                       usePoses=False,
                       resetPoses=True,
                       planFromCurrentRobotState=False,
                       retract=False):

        ikParameters = IkParameters(
            usePointwise=False,
            maxDegreesPerSecond=speed,
            numberOfAddedKnots=1,
            quasiStaticShrinkFactor=self.quasiStaticShrinkFactor,
            fixInitialState=planFromCurrentRobotState)

        _, yaxis, _ = transformUtils.getAxesFromTransform(
            self.computeGraspFrame().transform)
        yawDesired = np.arctan2(yaxis[1], yaxis[0])

        if startPose is None:
            startPose = self.getPlanningStartPose()

        nominalPose = self.getNominalPose()

        self.ikPlanner.addPose(nominalPose, self.nominalPoseName)
        self.ikPlanner.addPose(startPose, self.startPoseName)

        self.ikPlanner.reachingSide = self.graspingHand

        constraints = []
        constraints.extend(
            self.createBaseConstraints(resetBase, lockBase, lockFeet,
                                       yawDesired))
        constraints.append(self.createBackPostureConstraint())
        constraints.append(self.ikPlanner.createQuasiStaticConstraint())
        constraints.extend(self.createFootConstraints(lockFeet))
        constraints.append(
            self.ikPlanner.createLockedArmPostureConstraint(
                self.startPoseName))
        constraints.append(
            self.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
        constraints.append(self.createElbowPostureConstraint())
        constraints.append(self.createStaticTorqueConstraint())
        constraints.append(self.createHandGazeConstraint())
        constraints.append(self.createHandFixedOrientConstraint())
        constraints.append(
            self.createWristAngleConstraint(wristAngleCW,
                                            planFromCurrentRobotState))
        constraints.extend(
            self.createAllHandPositionConstraints(self.coaxialTol, retract))

        if retract:
            startPoseName = self.getStartPoseName(planFromCurrentRobotState,
                                                  True, usePoses)
            endPoseName = self.getEndPoseName(True, usePoses)
            endPose = self.ikPlanner.jointController.getPose(endPoseName)
            endPose = self.ikPlanner.mergePostures(
                endPose, robotstate.matchJoints('lwy'), startPose)
            endPoseName = 'q_retract'
            self.ikPlanner.addPose(endPose, endPoseName)
        else:
            startPoseName = self.getStartPoseName(planFromCurrentRobotState,
                                                  retract, usePoses)
            endPoseName = self.getEndPoseName(retract, usePoses)

        plan = self.ikPlanner.runIkTraj(constraints,
                                        startPoseName,
                                        endPoseName,
                                        self.nominalPoseName,
                                        ikParameters=ikParameters)

        if resetPoses and not retract and max(plan.plan_info) <= 10:
            self.setReachAndTouchPoses(plan)

        return plan
    def fitRunningBoardAtFeet(self):

        # get stance frame
        startPose = self.getPlanningStartPose()
        stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
            self.robotSystem.robotStateModel, useWorldZ=False)
        stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)

        # get pointcloud and extract search region covering the running board
        polyData = segmentation.getCurrentRevolutionData()
        polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
        _, polyData = segmentation.removeGround(polyData)
        polyData = segmentation.cropToBox(polyData, stanceFrame,
                                          [1.0, 1.0, 0.1])

        if not polyData.GetNumberOfPoints():
            print 'empty search region point cloud'
            return

        vis.updatePolyData(polyData,
                           'running board search points',
                           parent=segmentation.getDebugFolder(),
                           color=[0, 1, 0],
                           visible=False)

        # extract maximal points along the stance x axis
        perpAxis = stanceFrameAxes[0]
        edgeAxis = stanceFrameAxes[1]
        edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
        edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
        vis.updatePolyData(edgePoints,
                           'edge points',
                           parent=segmentation.getDebugFolder(),
                           visible=True)

        # ransac fit a line to the edge points
        linePoint, lineDirection, fitPoints = segmentation.applyLineFit(
            edgePoints)
        if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
            lineDirection = -lineDirection

        linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels',
                                                  [1.0, 1.0])
        dists = np.dot(
            vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint,
            lineDirection)
        p1 = linePoint + lineDirection * np.min(dists)
        p2 = linePoint + lineDirection * np.max(dists)
        vis.updatePolyData(fitPoints,
                           'line fit points',
                           parent=segmentation.getDebugFolder(),
                           colorByName='ransac_labels',
                           visible=False)

        # compute a new frame that is in plane with the stance frame
        # and matches the orientation and position of the detected edge
        origin = np.array(stanceFrame.GetPosition())
        normal = np.array(stanceFrameAxes[2])

        # project stance origin to edge, then back to foot frame
        originProjectedToEdge = linePoint + lineDirection * np.dot(
            origin - linePoint, lineDirection)
        originProjectedToPlane = segmentation.projectPointToPlane(
            originProjectedToEdge, origin, normal)
        zaxis = np.array(stanceFrameAxes[2])
        yaxis = np.array(lineDirection)
        xaxis = np.cross(yaxis, zaxis)
        xaxis /= np.linalg.norm(xaxis)
        yaxis = np.cross(zaxis, xaxis)
        yaxis /= np.linalg.norm(yaxis)

        d = DebugData()
        d.addSphere(p1, radius=0.005)
        d.addSphere(p2, radius=0.005)
        d.addLine(p1, p2)
        d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0])
        d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0])
        d.addLine(originProjectedToPlane, origin, color=[0, 1, 0])
        d.addLine(originProjectedToEdge, origin, color=[1, 0, 0])
        vis.updatePolyData(d.getPolyData(),
                           'running board edge',
                           parent=segmentation.getDebugFolder(),
                           colorByName='RGB255',
                           visible=False)

        # update the running board box affordance position and orientation to
        # fit the detected edge
        box = self.spawnRunningBoardAffordance()
        boxDimensions = box.getProperty('Dimensions')
        t = transformUtils.getTransformFromAxesAndOrigin(
            xaxis, yaxis, zaxis, originProjectedToPlane)
        t.PreMultiply()
        t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0)
        box.getChildFrame().copyFrame(t)

        self.initialize()