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 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 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 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)