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 planRetract(self, **kwargs):
startPose = self.getPlanningStartPose()
if self.graspingHand == 'left':
jointId = robotstate.getDrakePoseJointNames().index('l_arm_lwy')
wristAngleCW = np.radians(160) + startPose[jointId]
else:
jointId = robotstate.getDrakePoseJointNames().index('r_arm_lwy')
wristAngleCW = np.radians(160) - startPose[jointId]
plan = self.planInsertTraj(self.speedLow, retract=True, lockBase=True, lockFeet=True,
usePoses=True, planFromCurrentRobotState=True, resetPoses=False,
wristAngleCW=wristAngleCW, **kwargs)
self.addPlan(plan)
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 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.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 planRetract(self, **kwargs):
startPose = self.getPlanningStartPose()
if self.graspingHand == 'left':
jointId = robotstate.getDrakePoseJointNames().index('l_arm_lwy')
wristAngleCW = np.radians(160) + startPose[jointId]
else:
jointId = robotstate.getDrakePoseJointNames().index('r_arm_lwy')
wristAngleCW = np.radians(160) - startPose[jointId]
plan = self.planInsertTraj(self.speedLow,
retract=True,
lockBase=True,
lockFeet=True,
usePoses=True,
planFromCurrentRobotState=True,
resetPoses=False,
wristAngleCW=wristAngleCW,
**kwargs)
self.addPlan(plan)
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):
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, models, nominalPoseMatFile, 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.addPose('q_zero', [0.0 for i in xrange(self.numberOfJoints)])
self.addPose('q_nom', self.loadPoseFromFile(nominalPoseMatFile))
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 __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 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,
models,
nominalPoseMatFile,
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.addPose('q_zero', [0.0 for i in xrange(self.numberOfJoints)])
self.addPose('q_nom', self.loadPoseFromFile(nominalPoseMatFile))
def getNeckPitch():
return robotStateJointController.q[robotstate.getDrakePoseJointNames().index( neckPitchJoint )]
def getNeckPitch():
return robotStateJointController.q[
robotstate.getDrakePoseJointNames().index(neckPitchJoint)]
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 __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 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 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 toJointName(self, jointIndex):
return robotstate.getDrakePoseJointNames()[jointIndex]
def toJointIndex(self, jointName):
return robotstate.getDrakePoseJointNames().index(jointName)
def toJointIndex(self, jointName):
return robotstate.getDrakePoseJointNames().index(jointName)
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]
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()
