From c6528a40c5721e58f157cfb7c0b4f6df69951541 Mon Sep 17 00:00:00 2001 From: Ian Davis Date: Fri, 8 Dec 2017 21:21:46 -0600 Subject: [PATCH] Removed press_service.py script, since it wasn't being used. --- src/press_service.py | 199 ------------------------------------------- 1 file changed, 199 deletions(-) delete mode 100755 src/press_service.py diff --git a/src/press_service.py b/src/press_service.py deleted file mode 100755 index c28dd46..0000000 --- a/src/press_service.py +++ /dev/null @@ -1,199 +0,0 @@ -#!/usr/bin/env python -import rospy -import sys -import numpy as np -import baxter_interface -from geometry_msgs.msg import ( - Transform, - PoseStamped, - Pose, - Point, - Quaternion, -) -from std_msgs.msg import ( - Bool, - Header, -) -from baxter_core_msgs.srv import ( - SolvePositionIK, - SolvePositionIKRequest, -) -from starbax.srv import * - - -#To Do: -# 1.Replace stand in dTke with actuall desired offset based on actual frame. It's kind of in world coords right now? Don't know what I'm actually doing. -# 2.Replace dQke with a composition of quaternion rotations from base to keurig to desired end effector - - -def request_handler(req): - (Qk,pk) = posetonp(req) #Horizontal np.array([[w, x, y, z]]), vertical np.array([[x],[y],[z]]) - - - #Set variables - x1 = -.15 - x2 = -.03 - x3 = .0 - x4 = -.15 - y1 = -.06 - y2 = y1 - y3 = y2 - y4 = y3 - z1 = .0 - z2 = .0 - z3 = -.015 - z4 = .05 - ikreq = SolvePositionIKRequest() - lefthand = baxter_interface.Gripper('left') - left = baxter_interface.Limb('left') - # dQke1 = 1 #compose Qbk with Qke to get the desired Qbe - # dTke1 = np.array([[0,1,0,x1],[0,0,-1,y1],[-1.,0,0,z1],[0,0,0,1.]]) - # dQke2 = 1 - # dTke2 = np.array([[0,1,0,x2],[0,0,-1,y2],[-1.,0,0,z2],[0,0,0,1.]]) - # dQke3 = 1 - # dTke3 = np.array([[0,1,0,x3],[0,0,-1,y3],[-1.,0,0,z3],[0,0,0,1.]]) - # dQke4 = 1 - # dTke4 = np.array([[0,1,0,x4],[0,0,-1,y4],[-1.,0,0,z4],[0,0,0,1.]]) - #Changeme the placeholder rotations are incorrect - dQke1 = 1 - dTke1 = np.array([[0,.7071,-.7071,x1],[0,-.7071,-.7071,y1],[-1.,0,0,z1],[0,0,0,1.]]) - dQke2 = 1 - dTke2 = np.array([[0,.7071,-.7071,x2],[0,-.7071,-.7071,y2],[-1.,0,0,z2],[0,0,0,1]]) - dQke3 = 1 - dTke3 = np.array([[0,.7071,-.7071,x3],[0,-.7071,-.7071,y3],[-1.,0,0,z3],[0,0,0,1]]) - dQke4 = 1 - dTke4 = np.array([[0,.7071,-.7071,x4],[0,-.7071,-.7071,y4],[-1.,0,0,z4],[0,0,0,1]]) - - #Quaternion multiplication does represent composition of rotations. - #So if I translate a desired R for the EE in the keurig frame into a quaternion (or just figure out 180 about world z in quaternion?) then I should be able to multiply the keurig quaternion by the R quaternion to get a world quaternion for the end effector. - - - #Create list of poses to solve for - dpose = poser(Qk,pk,dQke1,dTke1) - print dpose - ikreq.pose_stamp.append(dpose) - - dpose = poser(Qk,pk,dQke2,dTke2) - print dpose - ikreq.pose_stamp.append(dpose) - - dpose = poser(Qk,pk,dQke3,dTke3) - print dpose - ikreq.pose_stamp.append(dpose) - - dpose = poser(Qk,pk,dQke4,dTke4) - print dpose - ikreq.pose_stamp.append(dpose) - #print ikreq - - #Send request message to ikcaller - sol_list = ikcaller(ikreq) - # print sol_list[0] - # print sol_list[1] - # print sol_list[2] - # print sol_list[3] - - - - - - - #Will need to index sol_list. It's a list of dictionaries - #Open the gripper - lefthand.open() - rospy.sleep(1) - left.move_to_joint_positions(sol_list[0]) - rospy.sleep(5) - left.move_to_joint_positions(sol_list[1]) - rospy.sleep(1) - left.move_to_joint_positions(sol_list[2]) - rospy.sleep(1) - left.move_to_joint_positions(sol_list[3]) - rospy.sleep(1) - - return Open2Response(True) - - -def ikcaller(poselist): - #This is using element zero, which may mean its designed for one joint solution solving. - iksvc = rospy.ServiceProxy("ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK) - - try: - rospy.wait_for_service("ExternalTools/left/PositionKinematicsNode/IKService", 5.0) - resp = iksvc(poselist) - except (rospy.ServiceException, rospy.ROSException), e: - rospy.logerr("Service call failed: %s" % (e,)) - return 1 - - #Be careful to respect the feasible movement limits - if (resp.isValid[0]): - print resp.isValid - print("SUCCESS - Valid Joint Solution Found:") - # Format solution into Limb API-compatible dictionary - limb_joints = [] - for i in range(0,4): - limb_joints.append(dict(zip(resp.joints[i].name, resp.joints[i].position))) - #print limb_joints - return limb_joints - else: - print resp.isValid - print("INVALID POSE - No Valid Joint Solution Found.") - return 1 - - - - -def poser(Qk,pk,dQke,dTke): - #Translate pk to dpw - Twk = np.array([[1,0,0,pk[0,0]],[0,1,0,pk[1,0]],[0,0,1,0.07+.15],[0,0,0,1]]) - dpw = Twk.dot(dTke.dot(np.array([[0],[0],[0],[1]]))) - - - #To start, I will just collect dQe from data. - #later, Qwk*Qke=Qwe - #Once I have any desired end effector orientation, I can rotate that about world z to get the one I want for a keurig rotation - dQe = np.array([[0.29591,0.59850,0.65811,0.34801]]) #w, x, y, z - - hdr = Header(stamp=rospy.Time.now(), frame_id='base') - dpose = PoseStamped( - header=hdr, - pose=Pose( - position=Point( - x=dpw[0,0], - y=dpw[1,0], - z=dpw[2,0], - ), - orientation=Quaternion( - x=dQe[0,1], - y=dQe[0,2], - z=dQe[0,3], - w=dQe[0,0], - ), - ), - ), - - return dpose[0] - - - -def posetonp(msg): - #Given a Pose message for the keurig, convert to numpy arrays p and Q. - Qk = np.array([[msg.pose.orientation.w,msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z]]) - pk = np.array([[msg.pose.position.x,msg.pose.position.y,msg.pose.position.z]]).T - return (Qk,pk) - - - - -def open_server(): - rospy.init_node('presser_node') - svc = rospy.Service('presser', Open2, request_handler) - try: - rospy.spin() - except KeyboardInterrupt: - print("Shutting down") - - - -if __name__ == '__main__': - open_server()