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