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