def __init__ (self, armName, listener=None):
self.armName = armName
if self.armName == MyConstants.Arm.Left:
self.toolframe = MyConstants.Frames.LeftTool
self.tooltopic = MyConstants.RavenTopics.LeftTool
self.baseframe = MyConstants.Frames.LeftBase
else:
self.toolframe = MyConstants.Frames.RightTool
self.tooltopic = MyConstants.RavenTopics.RightTool
self.baseframe = MyConstants.Frames.RightBase
if listener == None:
listener = tf.TransformListener()
self.listener = listener
self.player = MyTrajectoryPlayer(arms=self.armName)
self.jointStates = list()
rospy.Subscriber(MyConstants.RavenTopics.RavenState, RavenState, self.ravenStateCallback)
rospy.sleep(1)
class GripperControlClass:
"""
Class for controlling the end effectors of the Raven
"""
def __init__ (self, armName, listener=None):
self.armName = armName
if self.armName == MyConstants.Arm.Left:
self.toolframe = MyConstants.Frames.LeftTool
self.tooltopic = MyConstants.RavenTopics.LeftTool
self.baseframe = MyConstants.Frames.LeftBase
else:
self.toolframe = MyConstants.Frames.RightTool
self.tooltopic = MyConstants.RavenTopics.RightTool
self.baseframe = MyConstants.Frames.RightBase
if listener == None:
listener = tf.TransformListener()
self.listener = listener
self.player = MyTrajectoryPlayer(arms=self.armName)
self.jointStates = list()
rospy.Subscriber(MyConstants.RavenTopics.RavenState, RavenState, self.ravenStateCallback)
rospy.sleep(1)
def goToGripperPose(self, startPose, endPose, duration=None, speed=None, ignoreOrientation=False):
"""
Given a startPose, move to endPose
Both startPose and endPose are geometry_msgs.msg.Pose
startPose is the current pose of the gripper WITH RESPECT TO MyConstants.Frames.Link0
"""
startPose = tfx.pose(startPose)
endPose = tfx.pose(endPose)
# TEMP, until fix orientation issue
#endPose = tfx.pose(endPose.position, tfx.tb_angles(0,90,0))
#endPose = tfx.pose(endPosition, startPose.orientation)
if ignoreOrientation:
endPose.orientation = startPose.orientation
self.player.clear_stages()
self.player.add_pose_to_pose('goToGripperPose', startPose, endPose, duration=duration, speed=speed)
def goToGripperPoseDelta(self, startPose, deltaPose, duration=None, speed=None, ignoreOrientation=False):
"""
Given a startPose, move by deltaPose
Both startPose and deltaPose are geometry_msgs.msg.Pose
startPose is the pose of the gripper WITH RESPECT TO MyConstants.Frames.Link0
deltaPose is the difference between the current pose and the final pose
"""
# convert to tfx format
startPose = tfx.pose(startPose)
deltaPose = tfx.pose(deltaPose)
endPosition = startPose.position + deltaPose.position
endQuatMat = startPose.orientation.matrix * deltaPose.orientation.matrix
#endQuatMat = deltaPose.orientation.matrix * startPose.orientation.matrix
startQuat = tfx.tb_angles(startPose.orientation.matrix).quaternion
endQuat = tfx.tb_angles(endQuatMat).quaternion
if np.dot(startQuat, endQuat) < 0:
rospy.loginfo('QUAT IS NEGATIVE')
endQuat = list(endQuat)
endQuat[-1] = -endQuat[-1]
endPose = tfx.pose(endPosition, endQuat)
#endPose = tfx.pose(endPosition, endQuatMat)
if ignoreOrientation:
endPose.orientation = startPose.orientation
# TEMP, until fix orientation issue
#endPose = tfx.pose(endPosition, tfx.tb_angles(-90,90,0))
#endPose = tfx.pose(endPosition, startPose.orientation)
self.player.clear_stages()
self.player.add_pose_to_pose('goToGripperPose',startPose,endPose,duration=duration, speed=speed)
def goToGripperPoseUsingJoints(self, endPose, duration=None, speed=None):
self.player.clear_stages()
self.player.add_go_to_pose_using_joints('goToGripperPoseUsingJoints', self.jointStates, endPose, duration=duration, speed=speed)
def start(self):
# start running, no blocking
return self.player.play(False)
def pause(self):
# pauses by clearing stages
self.player.clear_stages()
def isPaused(self):
"""
True if no stages
"""
return len(self.player.stages) == 0
def stop(self):
return self.player.stop_playing()
def closeGripper(self,duration=5):
self.player.clear_stages()
self.player.add_close_gripper(duration=duration)
def openGripper(self,duration=5):
self.player.clear_stages()
self.player.add_open_gripper(duration=duration)
def setGripper(self, value, duration=5):
self.player.clear_stages()
self.player.add_set_gripper(value,duration=duration)
def getGripperPose(self,frame=MyConstants.Frames.World):
"""
Returns gripper pose w.r.t. frame
geometry_msgs.msg.Pose
"""
#self.listener.waitForTransform(frame, self.toolframe, rospy.Time.now(), rospy.Duration(5))
commonTime = self.listener.getLatestCommonTime(frame, self.toolframe)
pos, quat = self.listener.lookupTransform(frame, self.toolframe, commonTime)
return tfx.pose(pos,quat).msg.Pose()
def ravenStateCallback(self, msg):
self.jointStates = msg.arms[0].joints
def getJointStates(self):
return self.jointStates
