def setKp(self, Kp, jointName=None):
if jointName is None:
self._Kp = Kp * np.ones(self._numPositions)
else:
idx = robotstate.getDrakePoseJointNames().index(jointName)
self._maxSpeed[idx] = Kp
self.updateKd()
def __init__(self, ikPlanner, affordanceMan,robotModel):
self.ikPlanner = ikPlanner
self.affordanceManager = affordanceMan
self.poses={}
self.robotModel=robotModel
self.jointLimits={}
for jointName in robotstate.getDrakePoseJointNames():
self.jointLimits[jointName]= [self.robotModel.model.getJointLimits(jointName)[0],self.robotModel.model.getJointLimits(jointName)[1]]
def setKp(self, Kp, jointName=None):
if jointName is None:
self._Kp = Kp*np.ones(self._numPositions)
else:
idx = robotstate.getDrakePoseJointNames().index(jointName)
self._maxSpeed[idx] = Kp
self.updateKd()
def plotPoses(self, poseTimes, poses):
import matplotlib.pyplot as plt
poses = np.array(poses)
if self.jointNameRegex:
jointIds = range(poses.shape[1])
else:
diffs = np.diff(poses, axis=0)
jointIds = np.unique(np.where(diffs != 0.0)[1])
jointNames = [
robotstate.getDrakePoseJointNames()[jointId]
for jointId in jointIds
]
jointTrajectories = [poses[:, jointId] for jointId in jointIds]
seriesNames = []
sampleResolutionInSeconds = 0.01
numberOfSamples = (poseTimes[-1] -
poseTimes[0]) / sampleResolutionInSeconds
xnew = np.linspace(poseTimes[0], poseTimes[-1], numberOfSamples)
fig = plt.figure()
ax = fig.add_subplot(111)
for jointId, jointName, jointTrajectory in zip(jointIds, jointNames,
jointTrajectories):
if self.jointNameRegex and not re.match(self.jointNameRegex,
jointName):
continue
x = poseTimes
y = jointTrajectory
y = np.rad2deg(y)
if self.interpolationMethod in ["slinear", "quadratic", "cubic"]:
f = scipy.interpolate.interp1d(x,
y,
kind=self.interpolationMethod)
elif self.interpolationMethod == "pchip":
f = scipy.interpolate.PchipInterpolator(x, y)
ax.plot(x, y, "ko")
seriesNames.append(jointName + " points")
ax.plot(xnew, f(xnew), "-")
seriesNames.append(jointName + " " + self.interpolationMethod)
ax.legend(seriesNames, loc="upper right").draggable()
ax.set_xlabel("time (s)")
ax.set_ylabel("joint angle (deg)")
ax.set_title("joint trajectories")
plt.show()
def getMovingJointNames(self, msg):
poseTimes, poses = self.getPlanPoses(msg)
diffs = np.diff(poses, axis=0)
jointIds = np.unique(np.where(diffs != 0.0)[1])
jointNames = [
robotstate.getDrakePoseJointNames()[jointId]
for jointId in jointIds
]
return jointNames
def __init__(self, models, poseCollection=None, jointNames=None):
self.jointNames = jointNames or robotstate.getDrakePoseJointNames()
self.numberOfJoints = len(self.jointNames)
self.models = list(models)
self.poses = {}
self.poseCollection = poseCollection
self.currentPoseName = None
self.lastRobotStateMessage = None
self.ignoreOldStateMessages = False
self.setPose('q_zero', np.zeros(self.numberOfJoints))
def __init__(self, models, poseCollection=None, jointNames=None):
self.jointNames = jointNames or robotstate.getDrakePoseJointNames()
self.numberOfJoints = len(self.jointNames)
self.models = list(models)
self.poses = {}
self.poseCollection = poseCollection
self.currentPoseName = None
self.lastRobotStateMessage = None
self.ignoreOldStateMessages = False
self.setPose('q_zero', np.zeros(self.numberOfJoints))
def __init__(self):
self.timer = TimerCallback(targetFps=10)
#self.timer.disableScheduledTimer()
self.app = ConsoleApp()
self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model')
self.fpsCounter = simpletimer.FPSCounter()
self.drakePoseJointNames = robotstate.getDrakePoseJointNames()
self.fpsCounter.printToConsole = True
self.timer.callback = self._tick
self._initialized = False
self.publishChannel = 'JOINT_POSITION_GOAL'
# self.lastCommandMessage = newAtlasCommandMessageAtZero()
self._numPositions = len(robotstate.getDrakePoseJointNames())
self._previousElapsedTime = 100
self._baseFlag = 0;
self.jointLimitsMin = np.array([self.robotModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames()])
self.jointLimitsMax = np.array([self.robotModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames()])
self.useControllerFlag = False
self.drivingGainsFlag = False
self.applyDefaults()
def __init__(self):
self.timer = TimerCallback(targetFps=10)
#self.timer.disableScheduledTimer()
self.app = ConsoleApp()
self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model')
self.fpsCounter = simpletimer.FPSCounter()
self.drakePoseJointNames = robotstate.getDrakePoseJointNames()
self.fpsCounter.printToConsole = True
self.timer.callback = self._tick
self._initialized = False
self.publishChannel = 'JOINT_POSITION_GOAL'
# self.lastCommandMessage = newAtlasCommandMessageAtZero()
self._numPositions = len(robotstate.getDrakePoseJointNames())
self._previousElapsedTime = 100
self._baseFlag = 0
self.jointLimitsMin = np.array([self.robotModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames()])
self.jointLimitsMax = np.array([self.robotModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames()])
self.useControllerFlag = False
self.drivingGainsFlag = False
self.applyDefaults()
def __init__(self, ikPlanner, affordanceMan, robotModel):
self.ikPlanner = ikPlanner
self.affordanceManager = affordanceMan
self.poses = {}
self.robotModel = robotModel
self.jointLimits = {}
for jointName in robotstate.getDrakePoseJointNames():
self.jointLimits[jointName] = [
self.robotModel.model.getJointLimits(jointName)[0],
self.robotModel.model.getJointLimits(jointName)[1]
]
def plotPoses(self, poseTimes, poses):
import matplotlib.pyplot as plt
poses = np.array(poses)
if self.jointNameRegex:
jointIds = range(poses.shape[1])
else:
diffs = np.diff(poses, axis=0)
jointIds = np.unique(np.where(diffs != 0.0)[1])
jointNames = [robotstate.getDrakePoseJointNames()[jointId] for jointId in jointIds]
jointTrajectories = [poses[:,jointId] for jointId in jointIds]
seriesNames = []
sampleResolutionInSeconds = 0.01
numberOfSamples = (poseTimes[-1] - poseTimes[0]) / sampleResolutionInSeconds
xnew = np.linspace(poseTimes[0], poseTimes[-1], numberOfSamples)
fig = plt.figure()
ax = fig.add_subplot(111)
for jointId, jointName, jointTrajectory in zip(jointIds, jointNames, jointTrajectories):
if self.jointNameRegex and not re.match(self.jointNameRegex, jointName):
continue
x = poseTimes
y = jointTrajectory
y = np.rad2deg(y)
if self.interpolationMethod in ['slinear', 'quadratic', 'cubic']:
f = scipy.interpolate.interp1d(x, y, kind=self.interpolationMethod)
elif self.interpolationMethod == 'pchip':
f = scipy.interpolate.pchip(x, y)
ax.plot(x, y, 'ko')
seriesNames.append(jointName + ' points')
ax.plot(xnew, f(xnew), '-')
seriesNames.append(jointName + ' ' + self.interpolationMethod)
ax.legend(seriesNames, loc='upper right').draggable()
ax.set_xlabel('time (s)')
ax.set_ylabel('joint angle (deg)')
ax.set_title('joint trajectories')
plt.show()
def onRobotStateMessage(msg):
if self.ignoreOldStateMessages and self.lastRobotStateMessage is not None and msg.utime < self.lastRobotStateMessage.utime:
return
poseName = channelName
pose = robotstate.convertStateMessageToDrakePose(msg)
self.lastRobotStateMessage = msg
# use joint name/positions from robot_state_t and append base_{x,y,z,roll,pitch,yaw}
jointPositions = np.hstack((msg.joint_position, pose[:6]))
jointNames = msg.joint_name + robotstate.getDrakePoseJointNames()[:6]
self.setPose(poseName, pose, pushToModel=False)
for model in self.models:
model.model.setJointPositions(jointPositions, jointNames)
def onRobotStateMessage(msg):
if self.ignoreOldStateMessages and self.lastRobotStateMessage is not None and msg.utime < self.lastRobotStateMessage.utime:
return
poseName = channelName
pose = robotstate.convertStateMessageToDrakePose(msg)
self.lastRobotStateMessage = msg
# use joint name/positions from robot_state_t and append base_{x,y,z,roll,pitch,yaw}
jointPositions = np.hstack((msg.joint_position, pose[:6]))
jointNames = msg.joint_name + robotstate.getDrakePoseJointNames()[:6]
self.setPose(poseName, pose, pushToModel=False)
for model in self.models:
model.model.setJointPositions(jointPositions, jointNames)
def __init__(self, robotSystem, jointGroups):
self.widget = QtGui.QTabWidget()
self.robotStateModel = robotSystem.robotStateModel
self.teleopRobotModel = robotSystem.teleopRobotModel
self.teleopJointController = robotSystem.teleopJointController
self.robotStateJointController = robotSystem.robotStateJointController
self.jointLimitsMin = np.array([
self.teleopRobotModel.model.getJointLimits(jointName)[0]
for jointName in robotstate.getDrakePoseJointNames()
])
self.jointLimitsMax = np.array([
self.teleopRobotModel.model.getJointLimits(jointName)[1]
for jointName in robotstate.getDrakePoseJointNames()
])
self.mirrorArms = False
self.mirrorLegs = False
self.buildTabWidget(jointGroups)
self.resetPoseToRobotState()
def initPerceptionDrivers(self, robotSystem):
from director import perception
from director import robotstate
multisenseDriver, mapServerSource = perception.init(robotSystem.view)
useNeckDriver = hasattr(robotSystem.ikPlanner, 'neckPitchJoint')
if useNeckDriver:
neckPitchJoint = robotSystem.ikPlanner.neckPitchJoint
neckPitchIndex = robotstate.getDrakePoseJointNames().index(
neckPitchJoint)
def getNeckPitch():
return robotSystem.robotStateJointController.q[neckPitchIndex]
neckDriver = perception.NeckDriver(robotSystem.view, getNeckPitch)
else:
neckDriver = None
spindleJoint = 'hokuyo_joint'
def getSpindleAngleFunction():
msg = robotSystem.robotStateJointController.lastRobotStateMessage
if msg and spindleJoint in msg.joint_name:
index = msg.joint_name.index(spindleJoint)
return (float(msg.utime) / (1e6), msg.joint_position[index])
else:
return (0, 0)
spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction)
robotSystem.robotStateModel.connectModelChanged(
spindleMonitor.onRobotStateChanged)
return FieldContainer(multisenseDriver=multisenseDriver,
mapServerSource=mapServerSource,
neckDriver=neckDriver,
spindleMonitor=spindleMonitor)
def initPerceptionDrivers(self, robotSystem):
from director import perception
from director import robotstate
multisenseDriver, mapServerSource = perception.init(robotSystem.view)
useNeckDriver = hasattr(robotSystem.ikPlanner, 'neckPitchJoint')
if useNeckDriver:
neckPitchJoint = robotSystem.ikPlanner.neckPitchJoint
neckPitchIndex = robotstate.getDrakePoseJointNames().index(neckPitchJoint)
def getNeckPitch():
return robotSystem.robotStateJointController.q[neckPitchIndex]
neckDriver = perception.NeckDriver(robotSystem.view, getNeckPitch)
else:
neckDriver = None
spindleJoint = 'hokuyo_joint'
def getSpindleAngleFunction():
msg = robotSystem.robotStateJointController.lastRobotStateMessage
if msg and spindleJoint in msg.joint_name:
index = msg.joint_name.index(spindleJoint)
return (float(msg.utime)/(1e6), msg.joint_position[index])
else:
return (0, 0)
spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction)
robotSystem.robotStateModel.connectModelChanged(spindleMonitor.onRobotStateChanged)
return FieldContainer(multisenseDriver=multisenseDriver,
mapServerSource=mapServerSource,
neckDriver=neckDriver,
spindleMonitor=spindleMonitor)
def atlasCommandToDrakePose(msg):
jointIndexMap = robotstate.getRobotStateToDrakePoseJointMap()
drakePose = np.zeros(len(robotstate.getDrakePoseJointNames()))
for jointIdx, drakeIdx in jointIndexMap.iteritems():
drakePose[drakeIdx] = msg.position[jointIdx]
return drakePose.tolist()
def getNeckPitch():
return robotStateJointController.q[robotstate.getDrakePoseJointNames().index( neckPitchJoint )]
def runTest():
side = 'left'
testTolerances = False
renderAllSamples = True
randomizeSamples = True
samplesPerJoint = 10
jointLimitPadding = np.radians(5)
ikPlanner = robotSystem.ikPlanner
jointController = robotSystem.robotStateJointController
robotModel = robotSystem.robotStateModel
if app.getTestingEnabled():
samplesPerJoint = 2
jointGroup = str('%s Arm' % side).title()
jointNames = ikPlanner.getJointGroup(jointGroup)
jointIndices = [robotstate.getDrakePoseJointNames().index(name) for name in jointNames]
jointLimits = np.array([robotModel.model.getJointLimits(jointName) for jointName in jointNames])
otherJoints = [name for name in robotstate.getDrakePoseJointNames() if name not in jointNames]
jointSamples = []
for name, limit in zip(jointNames, jointLimits):
jointMin = limit[0] + jointLimitPadding
jointMax = limit[1] - jointLimitPadding
samples, spacing = np.linspace(jointMin, jointMax, samplesPerJoint, retstep=True)
jointSamples.append(samples)
print 'joint name:', name
print 'joint range: [%.4f, %.4f]' % (limit[0], limit[1])
print 'joint number of samples:', samplesPerJoint
print 'joint sample spacing: %.4f' % spacing
totalSamples = np.product([len(x) for x in jointSamples])
print 'total number of samples:', totalSamples
allSamples = list(itertools.product(*jointSamples))
if randomizeSamples:
np.random.shuffle(allSamples)
if 'endEffectorConfig' in robotSystem.directorConfig:
linkName = robotSystem.directorConfig['endEffectorConfig']['endEffectorLinkNames'][0]
else:
linkName = ikPlanner.getHandLink(side)
linkFrame = robotModel.getLinkFrame(linkName)
constraints = []
constraints.append(ikPlanner.createPostureConstraint('q_nom', otherJoints))
constraints.extend(ikPlanner.createSixDofLinkConstraints(jointController.q, linkName))
def setTolerance(distance, angleInDegrees):
constraints[-1].angleToleranceInDegrees = angleInDegrees
constraints[-2].upperBound = np.ones(3)*distance
constraints[-2].lowerBound = np.ones(3)*-distance
setTolerance(0.005, 0.5)
ikParameters = ikplanner.IkParameters()
ikParameters.setToDefaults()
ikParameters.majorIterationsLimit = 10000
ikParameters.majorOptimalityTolerance = 1e-4
ikParameters.majorFeasibilityTolerance = 1e-6
#seedPoseName = 'q_nom'
#nominalPoseName = 'q_nom'
seedPoseName = 'sample_pose'
nominalPoseName = 'sample_pose'
print
print 'constraints:'
print
print constraints[-2]
print
print constraints[-1]
print
print ikParameters
print
print 'seed pose name:', seedPoseName
print 'nominal pose name:', nominalPoseName
print
ikPlanner.addPose(jointController.q, 'sample_pose')
endPose, info = computeIk(linkFrame, constraints, ikParameters, seedPoseName, nominalPoseName)
assert info == 1
assert np.allclose(endPose, jointController.q)
q = jointController.q.copy()
nom_sample = q[jointIndices].copy()
sampleCount = 0
totalSampleCount = 0
badSampleCount = 0
sampleL2NormAccum = 0.0
startTime = time.time()
for sample in allSamples:
sampleCount += 1
totalSampleCount += 1
dist = np.linalg.norm(sample - nom_sample)
sampleL2NormAccum += dist
q[jointIndices] = sample
jointController.setPose('sample_pose', q)
ikPlanner.addPose(q, 'sample_pose')
if renderAllSamples:
view.forceRender()
targetFrame = robotModel.getLinkFrame(linkName)
#pos, quat = transformUtils.poseFromTransform(frame)
endPose, info = computeIk(targetFrame, constraints, ikParameters, seedPoseName, nominalPoseName)
if info >= 10:
print
print 'bad info:', info
jointController.addPose('bad', endPose)
print 'sample num:', totalSampleCount
print 'sample:', sample
print
badSampleCount += 1
errorRate = badSampleCount/float(totalSampleCount)
print 'error rate: %.2f' % errorRate
print 'avg pose l2 norm:', sampleL2NormAccum/totalSampleCount
if testTolerances:
succeeded = False
for tol in [(0.01, 1), (0.01, 2), (0.02, 2), (0.02, 3), (0.03, 3), (0.03, 5), (0.04, 5), (0.05, 5), (0.1, 10), (0.2, 20)]:
print 'retry tolerance:', tol
setTolerance(tol[0], tol[1])
endPose, info = computeIk(frame, constraints, ikParameters, seedPoseName, nominalPoseName)
if info < 10:
succeeded = True
print 'Worked!'
break
setTolerance(0.005, 0.5)
if not succeeded:
print 'Giving up after retries.'
continue
timeNow = time.time()
elapsed = timeNow - startTime
if elapsed > 1.0:
view.forceRender()
print '%d samples/sec' % (sampleCount / elapsed), '%d total samples' % totalSampleCount
startTime = timeNow
sampleCount = 0
if app.getTestingEnabled():
assert badSampleCount == 0
app.quit()
def setMaxSpeed(self, speed, jointName=None):
if jointName is None:
self._maxSpeed = np.deg2rad(speed)*np.ones(self._numPositions)
else:
idx = robotstate.getDrakePoseJointNames().index(jointName)
self._maxSpeed[idx] = np.deg2rad(speed)
def atlasCommandToDrakePose(msg):
jointIndexMap = robotstate.getRobotStateToDrakePoseJointMap()
drakePose = np.zeros(len(robotstate.getDrakePoseJointNames()))
for jointIdx, drakeIdx in jointIndexMap.iteritems():
drakePose[drakeIdx] = msg.position[jointIdx]
return drakePose.tolist()
def getMovingJointNames(self, msg):
poseTimes, poses = self.getPlanPoses(msg)
diffs = np.diff(poses, axis=0)
jointIds = np.unique(np.where(diffs != 0.0)[1])
jointNames = [robotstate.getDrakePoseJointNames()[jointId] for jointId in jointIds]
return jointNames
def __init__(self, robotSystem, jointGroups):
self.widget = QtGui.QTabWidget()
self.robotStateModel = robotSystem.robotStateModel
self.teleopRobotModel = robotSystem.teleopRobotModel
self.teleopJointController = robotSystem.teleopJointController
self.robotStateJointController = robotSystem.robotStateJointController
self.jointLimitsMin = np.array([self.teleopRobotModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames()])
self.jointLimitsMax = np.array([self.teleopRobotModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames()])
self.mirrorArms = False
self.mirrorLegs = False
self.buildTabWidget(jointGroups)
self.resetPoseToRobotState()
def toJointName(self, jointIndex):
return robotstate.getDrakePoseJointNames()[jointIndex]
def toJointIndex(self, jointName):
return robotstate.getDrakePoseJointNames().index(jointName)
def __init__(self, robotStateModel, robotStateJointController):
self.robotStateModel = robotStateModel
self.robotStateJointController = robotStateJointController
# Get joint limits
self.jointLimitsLower = np.array([self.robotStateModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames()])
self.jointLimitsUpper = np.array([self.robotStateModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames()])
# Activate/Deactivate joint limit clamper
self.clampToJointLimits = False
def toJointIndex(self, jointName):
return robotstate.getDrakePoseJointNames().index(jointName)
def getNeckPitch():
return robotStateJointController.q[
robotstate.getDrakePoseJointNames().index(neckPitchJoint)]
def runTest():
side = 'left'
testTolerances = False
renderAllSamples = True
randomizeSamples = True
samplesPerJoint = 10
jointLimitPadding = np.radians(5)
ikPlanner = robotSystem.ikPlanner
jointController = robotSystem.robotStateJointController
robotModel = robotSystem.robotStateModel
if app.getTestingEnabled():
samplesPerJoint = 2
jointGroup = str('%s Arm' % side).title()
jointNames = ikPlanner.getJointGroup(jointGroup)
jointIndices = [robotstate.getDrakePoseJointNames().index(name) for name in jointNames]
jointLimits = np.array([robotModel.model.getJointLimits(jointName) for jointName in jointNames])
otherJoints = [name for name in robotstate.getDrakePoseJointNames() if name not in jointNames]
jointSamples = []
for name, limit in zip(jointNames, jointLimits):
jointMin = limit[0] + jointLimitPadding
jointMax = limit[1] - jointLimitPadding
samples, spacing = np.linspace(jointMin, jointMax, samplesPerJoint, retstep=True)
jointSamples.append(samples)
print('joint name:', name)
print('joint range: [%.4f, %.4f]' % (limit[0], limit[1]))
print('joint number of samples:', samplesPerJoint)
print('joint sample spacing: %.4f' % spacing)
totalSamples = np.product([len(x) for x in jointSamples])
print('total number of samples:', totalSamples)
allSamples = list(itertools.product(*jointSamples))
if randomizeSamples:
np.random.shuffle(allSamples)
if 'endEffectorConfig' in robotSystem.directorConfig:
linkName = robotSystem.directorConfig['endEffectorConfig']['endEffectorLinkNames'][0]
else:
linkName = ikPlanner.getHandLink(side)
linkFrame = robotModel.getLinkFrame(linkName)
constraints = []
constraints.append(ikPlanner.createPostureConstraint('q_nom', otherJoints))
constraints.extend(ikPlanner.createSixDofLinkConstraints(jointController.q, linkName))
def setTolerance(distance, angleInDegrees):
constraints[-1].angleToleranceInDegrees = angleInDegrees
constraints[-2].upperBound = np.ones(3)*distance
constraints[-2].lowerBound = np.ones(3)*-distance
setTolerance(0.005, 0.5)
ikParameters = ikplanner.IkParameters()
ikParameters.setToDefaults()
ikParameters.majorIterationsLimit = 10000
ikParameters.majorOptimalityTolerance = 1e-4
ikParameters.majorFeasibilityTolerance = 1e-6
#seedPoseName = 'q_nom'
#nominalPoseName = 'q_nom'
seedPoseName = 'sample_pose'
nominalPoseName = 'sample_pose'
print()
print('constraints:')
print()
print(constraints[-2])
print()
print(constraints[-1])
print()
print(ikParameters)
print()
print('seed pose name:', seedPoseName)
print('nominal pose name:', nominalPoseName)
print()
ikPlanner.addPose(jointController.q, 'sample_pose')
endPose, info = computeIk(linkFrame, constraints, ikParameters, seedPoseName, nominalPoseName)
assert info == 1
assert np.allclose(endPose, jointController.q)
q = jointController.q.copy()
nom_sample = q[jointIndices].copy()
sampleCount = 0
totalSampleCount = 0
badSampleCount = 0
sampleL2NormAccum = 0.0
startTime = time.time()
for sample in allSamples:
sampleCount += 1
totalSampleCount += 1
dist = np.linalg.norm(sample - nom_sample)
sampleL2NormAccum += dist
q[jointIndices] = sample
jointController.setPose('sample_pose', q)
ikPlanner.addPose(q, 'sample_pose')
if renderAllSamples:
view.forceRender()
targetFrame = robotModel.getLinkFrame(linkName)
#pos, quat = transformUtils.poseFromTransform(frame)
endPose, info = computeIk(targetFrame, constraints, ikParameters, seedPoseName, nominalPoseName)
if info >= 10:
print()
print('bad info:', info)
jointController.addPose('bad', endPose)
print('sample num:', totalSampleCount)
print('sample:', sample)
print()
badSampleCount += 1
errorRate = badSampleCount/float(totalSampleCount)
print('error rate: %.2f' % errorRate)
print('avg pose l2 norm:', sampleL2NormAccum/totalSampleCount)
if testTolerances:
succeeded = False
for tol in [(0.01, 1), (0.01, 2), (0.02, 2), (0.02, 3), (0.03, 3), (0.03, 5), (0.04, 5), (0.05, 5), (0.1, 10), (0.2, 20)]:
print('retry tolerance:', tol)
setTolerance(tol[0], tol[1])
endPose, info = computeIk(frame, constraints, ikParameters, seedPoseName, nominalPoseName)
if info < 10:
succeeded = True
print('Worked!')
break
setTolerance(0.005, 0.5)
if not succeeded:
print('Giving up after retries.')
continue
timeNow = time.time()
elapsed = timeNow - startTime
if elapsed > 1.0:
view.forceRender()
print('%d samples/sec' % (sampleCount / elapsed), '%d total samples' % totalSampleCount)
startTime = timeNow
sampleCount = 0
if app.getTestingEnabled():
assert badSampleCount == 0
app.quit()
def setMaxSpeed(self, speed, jointName=None):
if jointName is None:
self._maxSpeed = np.deg2rad(speed) * np.ones(self._numPositions)
else:
idx = robotstate.getDrakePoseJointNames().index(jointName)
self._maxSpeed[idx] = np.deg2rad(speed)
def toJointName(self, jointIndex):
return robotstate.getDrakePoseJointNames()[jointIndex]
