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
