Beispiel #1
0
 def __init__(self, robotModel, imageManager, cameraName):
     self.robotModel = robotModel
     self.cameraName = cameraName
     self.imageManager = imageManager
     self.rayLength = 2.0
     self.headLink = drcargs.getDirectorConfig()['headLink']
     robotModel.connectModelChanged(self.update)
     self.update(robotModel)
Beispiel #2
0
 def __init__(self, robotModel, imageManager, cameraName):
     self.robotModel = robotModel
     self.cameraName = cameraName
     self.imageManager = imageManager
     self.rayLength = 2.0
     self.headLink = drcargs.getDirectorConfig()['headLink']
     robotModel.connectModelChanged(self.update)
     self.update(robotModel)
    def sliderChanged(self, jointName):


        slider = self.slidersMap[jointName]
        jointIndex = self.toJointIndex(jointName)
        jointValue = self.toJointValue(jointIndex, slider.value / float(self.sliderMax))

        if ('arm' in jointName and self.mirrorArms) or ('leg' in jointName and self.mirrorLegs):
            mirrorJoint = jointName.replace('l_', 'r_') if jointName.startswith('l_') else jointName.replace('r_', 'l_')
            mirrorIndex = self.toJointIndex(mirrorJoint)
            mirrorValue = jointValue
            if mirrorJoint in drcargs.getDirectorConfig()['mirrorJointSignFlips']:
                mirrorValue = -mirrorValue
            self.endPose[mirrorIndex] = mirrorValue

        self.endPose[jointIndex] = jointValue
        self.updateLabel(jointName, jointValue)
        self.showPose(self.endPose)
        self.updateSliders()
Beispiel #4
0
    def sliderChanged(self, jointName):


        slider = self.slidersMap[jointName]
        jointIndex = self.toJointIndex(jointName)
        jointValue = self.toJointValue(jointIndex, slider.value / float(self.sliderMax))

        if ('arm' in jointName and self.mirrorArms) or ('leg' in jointName and self.mirrorLegs):
            mirrorJoint = jointName.replace('l_', 'r_') if jointName.startswith('l_') else jointName.replace('r_', 'l_')
            mirrorIndex = self.toJointIndex(mirrorJoint)
            mirrorValue = jointValue
            if mirrorJoint in drcargs.getDirectorConfig()['mirrorJointSignFlips']:
                mirrorValue = -mirrorValue
            self.endPose[mirrorIndex] = mirrorValue

        self.endPose[jointIndex] = jointValue
        self.updateLabel(jointName, jointValue)
        self.showPose(self.endPose)
        self.updateSliders()
Beispiel #5
0
    def searchFinalPose(self, constraints, side, eeName, eePose, nominalPoseName, capabilityMapFile, ikParameters):
        commands = []
        commands.append('default_shrink_factor = %s;' % ikParameters.quasiStaticShrinkFactor)
        constraintNames = []
        for constraintId, constraint in enumerate(constraints):
            if not constraint.enabled:
                continue
            constraint.getCommands(commands, constraintNames, suffix='_%d' % constraintId)
            commands.append('\n')
        commands.append('eeId = r.findLinkId(\'{:s}\');'.format(eeName))
        commands.append('additional_constraints = {};')
        commands.append('goal_constraints = {};')
        commands.append('capability_map = CapabilityMap([\'{:s}\', \'/{:s}\']);'.format(os.path.dirname(drcargs.args().directorConfigFile), drcargs.getDirectorConfig()['capabilityMapFile']))
        for constraint in constraintNames:
            commands.append('if isa({0:s}, \'Point2PointDistanceConstraint\') && {0:s}.body_a.idx == eeId '
                            '|| isa({0:s}, \'EulerConstraint\') && {0:s}.body == eeId '
                            'goal_constraints = {{goal_constraints{{:}}, {0:s}}}; else '
                            'additional_constraints = {{additional_constraints{{:}}, {0:s}}};end'.format(constraint))
        commands.append('cost = Point(r.getPositionFrame(),10);')
        commands.append('for i = r.getNumBodies():-1:1 '
                        'if all(r.getBody(i).parent > 0) && all(r.getBody(r.getBody(i).parent).position_num > 0) '
                        'cost(r.getBody(r.getBody(i).parent).position_num) = '
                        'cost(r.getBody(r.getBody(i).parent).position_num) + cost(r.getBody(i).position_num);end;end')
        commands.append('cost(1:6) = max(cost(7:end))/2;')
        commands.append('cost = cost/min(cost);')
        commands.append('Q = diag(cost);')
        commands.append('ikoptions = IKoptions(r);')
        commands.append('ikoptions = ikoptions.setMajorIterationsLimit({:d});'.format(ikParameters.majorIterationsLimit))
        commands.append('ikoptions = ikoptions.setQ(Q);')
        commands.append('ikoptions = ikoptions.setMajorOptimalityTolerance({:f});' .format(ikParameters.majorOptimalityTolerance))
#        commands.append('{:s}'.format(ConstraintBase.toColumnVectorString(xGoal)))
        commands.append('fpp = FinalPoseProblem(r, eeId, reach_start, {:s}, additional_constraints,'
                        '{:s}, \'capabilitymap\', capability_map, \'ikoptions\', ikoptions, \'graspinghand\', \'{:s}\');'.format(ConstraintBase.toColumnVectorString(eePose), nominalPoseName, side))
        commands.append('[x_goal, info] = fpp.findFinalPose();')
        self.comm.sendCommands(commands)
        
        info = self.comm.getFloatArray('info')[0]
        if info == 1:
            endPose = self.comm.getFloatArray('x_goal(8:end)')
        else:
            endPose = []

        return endPose, info
Beispiel #6
0
    postureShortcuts = teleoppanel.PosturePlanShortcuts(robotStateJointController, ikPlanner)


    def drillTrackerOn():
        om.findObjectByName('Multisense').model.showRevolutionCallback = fitDrillMultisense

    def drillTrackerOff():
        om.findObjectByName('Multisense').model.showRevolutionCallback = None

    def fitPosts():
        segmentation.fitVerticalPosts(segmentation.getCurrentRevolutionData())
        affordancePanel.onGetRaycastTerrain()

    ikPlanner.addPostureGoalListener(robotStateJointController)

    if 'fixedBaseArm' in drcargs.getDirectorConfig()['userConfig']:
        ikPlanner.fixedBaseArm = True

    playbackPanel = playbackpanel.init(planPlayback, playbackRobotModel, playbackJointController,
                                      robotStateModel, robotStateJointController, manipPlanner)

    footstepsDriver.walkingPlanCallback = playbackPanel.setPlan
    manipPlanner.connectPlanReceived(playbackPanel.setPlan)

    teleopPanel = teleoppanel.init(robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController,
                     ikPlanner, manipPlanner, affordanceManager, playbackPanel.setPlan, playbackPanel.hidePlan)

    if useGamepad:
        gamePad = gamepad.Gamepad(teleopPanel, teleopJointController, ikPlanner, view)

    if useBlackoutText:
Beispiel #7
0
 def isCompatibleWithConfig():
     return 'headLink' in drcargs.getDirectorConfig()
Beispiel #8
0
    def searchFinalPose(self, constraints, side, eeName, eePose,
                        nominalPoseName, capabilityMapFile, ikParameters):
        commands = []
        commands.append('default_shrink_factor = %s;' %
                        ikParameters.quasiStaticShrinkFactor)
        constraintNames = []
        for constraintId, constraint in enumerate(constraints):
            if not constraint.enabled:
                continue
            constraint.getCommands(commands,
                                   constraintNames,
                                   suffix='_%d' % constraintId)
            commands.append('\n')
        commands.append('eeId = r.findLinkId(\'{:s}\');'.format(eeName))
        commands.append('additional_constraints = {};')
        commands.append('goal_constraints = {};')
        commands.append(
            'capability_map = CapabilityMap([\'{:s}\', \'/{:s}\']);'.format(
                os.path.dirname(drcargs.args().directorConfigFile),
                drcargs.getDirectorConfig()['capabilityMapFile']))
        for constraint in constraintNames:
            commands.append(
                'if isa({0:s}, \'Point2PointDistanceConstraint\') && {0:s}.body_a.idx == eeId '
                '|| isa({0:s}, \'EulerConstraint\') && {0:s}.body == eeId '
                'goal_constraints = {{goal_constraints{{:}}, {0:s}}}; else '
                'additional_constraints = {{additional_constraints{{:}}, {0:s}}};end'
                .format(constraint))
        commands.append('cost = Point(r.getPositionFrame(),10);')
        commands.append(
            'for i = r.getNumBodies():-1:1 '
            'if all(r.getBody(i).parent > 0) && all(r.getBody(r.getBody(i).parent).position_num > 0) '
            'cost(r.getBody(r.getBody(i).parent).position_num) = '
            'cost(r.getBody(r.getBody(i).parent).position_num) + cost(r.getBody(i).position_num);end;end'
        )
        commands.append('cost(1:6) = max(cost(7:end))/2;')
        commands.append('cost = cost/min(cost);')
        commands.append('Q = diag(cost);')
        commands.append('ikoptions = IKoptions(r);')
        commands.append(
            'ikoptions = ikoptions.setMajorIterationsLimit({:d});'.format(
                ikParameters.majorIterationsLimit))
        commands.append('ikoptions = ikoptions.setQ(Q);')
        commands.append(
            'ikoptions = ikoptions.setMajorOptimalityTolerance({:f});'.format(
                ikParameters.majorOptimalityTolerance))
        #        commands.append('{:s}'.format(ConstraintBase.toColumnVectorString(xGoal)))
        commands.append(
            'fpp = FinalPoseProblem(r, eeId, reach_start, {:s}, additional_constraints,'
            '{:s}, \'capabilitymap\', capability_map, \'ikoptions\', ikoptions, \'graspinghand\', \'{:s}\');'
            .format(ConstraintBase.toColumnVectorString(eePose),
                    nominalPoseName, side))
        commands.append('[x_goal, info] = fpp.findFinalPose();')
        self.comm.sendCommands(commands)

        info = self.comm.getFloatArray('info')[0]
        if info == 1:
            endPose = self.comm.getFloatArray('x_goal(8:end)')
        else:
            endPose = []

        return endPose, info
Beispiel #9
0
 def isCompatibleWithConfig():
     return 'headLink' in drcargs.getDirectorConfig()