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()
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
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:
def isCompatibleWithConfig(): return 'headLink' in drcargs.getDirectorConfig()
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