def __init__ (self, armName, closedGraspValue=0.,defaultPoseSpeed=.01): self.armName = armName if armName == raven_constants.Arm.Left: self.toolFrame = raven_constants.Frames.LeftTool else: self.toolFrame = raven_constants.Frames.RightTool self.commandFrame = raven_constants.Frames.Link0 self.ravenController = RavenController(self.armName, closedGraspValue=closedGraspValue, defaultPoseSpeed=defaultPoseSpeed)
class RavenArm: """ Class for controlling the end effectors of the Raven """ def __init__ (self, armName, closedGraspValue=0.,defaultPoseSpeed=.01): self.armName = armName if armName == raven_constants.Arm.Left: self.toolFrame = raven_constants.Frames.LeftTool else: self.toolFrame = raven_constants.Frames.RightTool self.commandFrame = raven_constants.Frames.Link0 self.ravenController = RavenController(self.armName, closedGraspValue=closedGraspValue, defaultPoseSpeed=defaultPoseSpeed) ############################# # start, pause, and stop #### ############################# def start(self): """ Starts the raven arm (releases the e-brake) (does nothing if raven arm is already running) """ print 'starting raven arm ', self.armName return self.ravenController.start() def pause(self): """ Pauses by clearing the stages """ self.ravenController.clearStages() def isPaused(self): """ True if no stages """ return len(self.ravenController.stages) == 0 def stop(self): return self.ravenController.stop() ####################### # trajectory commands # ####################### def executePoseTrajectory(self, poseTraj, block=True, duration=None, speed=None, ignoreOrientation=False): """ poseTraj is a tuple/list of poses Can set either duration or speed (not both) duration is either the time of the whole trajectory or a list of the duration of each segment of the trajectory speed is the joint speed """ if duration != None: if type(duration) != tuple: duration = [duration/len(poseTraj) for _ in range(len(poseTraj))] if len(duration) != len(poseTraj): return prevPose = None for pose in poseTraj: self.goToGripperPose(tfx.pose(pose), startPose=prevPose, block=False, duration=duration, speed=speed, ignoreOrientation=ignoreOrientation) prevPose = pose if block: return self.blockUntilPaused() return True def executeStampedPoseTrajectory(self, stampedPoses, block=True): """ stampedPoses is a list of tfx poses with time stamps """ prevTime = rospy.Time.now() prevPose = None for stampedPose in stampedPoses: duration = (stampedPose.stamp - prevTime).to_sec() pose = tfx.pose(stampedPose.position, stampedPose.orientation) self.goToGripperPose(pose, startPose=prevPose, block=False, duration=duration) prevTime = stampedPose.stamp prevPose = pose if block: return self.blockUntilPaused() return True def executeDeltaPoseTrajectory(self, deltaPoses, startPose=None, block=True, speed=None, ignoreOrientation=False): """ Each deltaPose in deltaPoses is with respect to the startPose Assuming deltaPoses and startPose in same frame (0_link) """ if startPose is None: startPose = self.getGripperPose() if startPose is None: return endPoses = [raven_util.endPose(startPose, deltaPose, self.commandFrame) for deltaPose in deltaPoses] #IPython.embed() print print 'START', startPose print 'END', endPoses[-1] print '''IPython.embed() ''' return self.executePoseTrajectory(endPoses, block=block, speed=speed, ignoreOrientation=ignoreOrientation) def executeStampedDeltaPoseTrajectory(self, stampedDeltaPoses, startPose=None, block=True): """ stampedDeltaPoses is a list of tfx poses with time stamps each stampedDeltaPose is endPose - startPose, where each endPose is different but the startPose is the same This function is intended to be used where each endPose is from the vision system and the one startPose is the gripper position according to vision """ durations = [] prevTime = rospy.Time.now() for stampedDeltaPose in stampedDeltaPoses: durations.append(stampedDeltaPose.stamp - prevTime) prevTime = stampedDeltaPose.stamp if startPose == None: startPose = self.getGripperPose() if startPose == None: return startPose = raven_util.convertToFrame(tfx.pose(startPose), self.commandFrame) endPoses = [] for deltaPose in stampedDeltaPoses: endPoses.append(raven_util.endPose(startPose, deltaPose, self.commandFrame)) prevPose = None for duration, endPose in zip(duration, endPoses): self.goToGripperPose(endPose, startPose=prevPose, block=False, duration=duration) prevPose = endPose if block: return self.blockUntilPaused() return True def executeJointTrajectory(self, jointTraj, block=True, duration=None, speed=None): """ jointTraj is a tuple/list of joints dictionaries joints is a dictionary of {jointType : jointPos} jointType is from raven_2_msgs.msg.Constants jointPos is position in radians Can set either duration or speed (not both) duration is either the time of the whole trajectory or a list of the duration of each segment of the trajectory speed is a scalar factor (compared to default joint speed) """ if duration != None: if type(duration) != tuple: duration = [duration/len(jointTraj) for _ in range(len(jointTraj))] if len(duration) != len(jointTraj): return prevJoints = None for joints in jointTraj: joints = dict(joints) self.goToJoints(joints, startJoints=prevJoints, block=False, duration=duration, speed=speed) prevJoints = joints if block: self.blockUntilPaused() return True def executeStampedJointTrajectory(self, stampedJoints, block=True): """ stampedJoints is a tuple of ((stamp, joints), ....) joints is dictionary of {jointType : jointPos} jointType is from raven_2_msgs.msg.Constants jointPos is position in radians """ prevTime = rospy.Time.now() prevJoints = None for stamp, joints in stampedJoints: duration = (stamp - prevTime).to_sec() joints = dict(joints) # so we don't clobber, just in case self.goToJoints(joints, startJoints=prevJoints, block=False, duration=duration) prevTime = stamp prevJoints = joints if block: return self.blockUntilPaused() return True ##################### # command methods # ##################### def goToGripperPose(self, endPose, startPose=None, block=True, duration=None, speed=None, ignoreOrientation=False): """ Move to endPose If startPose is not specified, default to current pose according to tf (w.r.t. Link0) """ if startPose == None: startPose = self.getGripperPose() if startPose == None: return startPose = raven_util.convertToFrame(tfx.pose(startPose), self.commandFrame) endPose = raven_util.convertToFrame(tfx.pose(endPose), self.commandFrame) if ignoreOrientation: endPose.orientation = startPose.orientation self.ravenController.goToPose(endPose, start=startPose, duration=duration, speed=speed) if block: return self.blockUntilPaused() return True def goToGripperPoseDelta(self, deltaPose, startPose=None, block=True, duration=None, speed=None, ignoreOrientation=False): """ Move by deltaPose If startPose is not specified, default to current pose according to tf (w.r.t. Link0) """ if startPose == None: startPose = self.getGripperPose() if startPose == None: return endPose = raven_util.endPose(startPose, deltaPose, self.commandFrame) self.goToGripperPose(endPose, startPose=startPose, block=block, duration=duration, speed=speed, ignoreOrientation=ignoreOrientation) def goToJoints(self, desJoints, startJoints=None, block=True, duration=None, speed=None): """ desJoints is dictionary of {jointType : jointPos} jointType is from raven_2_msgs.msg.Constants jointPos is position in radians speed is a factor gain compared to default speeds """ self.ravenController.goToJoints(desJoints, startJoints=startJoints, duration=duration, speed=speed) if block: return self.blockUntilPaused() return True def closeGripper(self,duration=2.5, block=True): self.setGripper(0.,duration=duration,block=block) def openGripper(self,duration=2.5, block=True): self.setGripper(1., duration=duration, block=block) def setGripper(self, grasp,duration=2.5, block=True): self.ravenController.clearStages() self.ravenController.setGripper(grasp,duration=duration) if block: rospy.sleep(duration) ####################### # state info methods # ####################### def getGripperPose(self,frame=raven_constants.Frames.Link0): """ Returns gripper pose w.r.t. frame geometry_msgs.msg.Pose """ pose = tfx.pose([0,0,0],frame=self.toolFrame) return tfx.convertToFrame(pose, self.commandFrame, pose.frame, wait=10) gripperPose = self.ravenController.currentPose if gripperPose != None: gripperPose = raven_util.convertToFrame(tfx.pose(gripperPose), frame) return gripperPose def getCurrentJoints(self): """ Returns is dictionary of {jointType : jointPos} jointType is from raven_2_msgs.msg.Constants jointPos is position in radians """ return self.ravenController.getCurrentJoints() ################# # other methods # ################# @property def name(self): return self.armName def blockUntilPaused(self, timeoutTime=999999): timeout = raven_util.Timeout(timeoutTime) timeout.start() while not self.isPaused(): if rospy.is_shutdown() or timeout.hasTimedOut(): return False rospy.sleep(.05) return True