Update

new_main.py

@@ -0,0 +1,156 @@
+import argparse
+import struct
+import sys
+import copy
+
+import rospy
+import rospkg
+
+from gazebo_msgs.srv import (SpawnModel, DeleteModel)
+from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion)
+from std_msgs.msg import (Header, Empty)
+
+from baxter_core_msgs.srv import (SolvePositionIK, SolvePositionIKRequest)
+
+import baxter_interface
+
+import target_poses as tps
+
+brickstuff = tps.brick_directions_notf
+
+class PickAndPlace(object):
+    def __init__(self, limb, hover_distance = 0.10, verbose=True, speed=0.9, accuracy=baxter_interface.settings.JOINT_ANGLE_TOLERANCE):
+        self._speed = speed
+        self._accuracy = accuracy
+        self._limb_name = limb # string
+        self._hover_distance = hover_distance # in meters
+        self._verbose = verbose # bool
+        self._limb = baxter_interface.Limb(limb)
+        self._gripper = baxter_interface.Gripper(limb)
+        ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
+        self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
+        rospy.wait_for_service(ns, 5.0)
+        # verify robot is enabled
+        print("Getting robot state... ")
+        self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
+        self._init_state = self._rs.state().enabled
+        print("Enabling robot... ")
+        self._rs.enable()
+
+    def move_to_start(self, start_angles=None):
+        print("Moving the {0} arm to start pose...".format(self._limb_name))
+        if not start_angles:
+            start_angles = dict(zip(self._joint_names, [0]*7))
+        self._guarded_move_to_joint_position(start_angles)
+        self.gripper_open()
+        rospy.sleep(0.2)
+        print("Running. Ctrl-c to quit")
+
+    def ik_request(self, pose):
+        hdr = Header(stamp=rospy.Time.now(), frame_id='base')
+        ikreq = SolvePositionIKRequest()
+        ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
+        try:
+            resp = self._iksvc(ikreq)
+        except (rospy.ServiceException, rospy.ROSException), e:
+            rospy.logerr("Service call failed: %s" % (e,))
+            return False
+        # Check if result valid, and type of seed ultimately used to get solution
+        # convert rospy's string representation of uint8[]'s to int's
+        resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type)
+        limb_joints = {}
+        if (resp_seeds[0] != resp.RESULT_INVALID):
+            seed_str = {
+                        ikreq.SEED_USER: 'User Provided Seed',
+                        ikreq.SEED_CURRENT: 'Current Joint Angles',
+                        ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
+                       }.get(resp_seeds[0], 'None')
+            if self._verbose:
+                print("IK Solution SUCCESS - Valid Joint Solution Found from Seed Type: {0}".format(
+                         (seed_str)))
+            # Format solution into Limb API-compatible dictionary
+            limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
+            if self._verbose:
+                print("IK Joint Solution:\n{0}".format(limb_joints))
+                print("------------------")
+        else:
+            rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
+            return False
+        return limb_joints
+
+    def _guarded_move_to_joint_position(self, joint_angles):
+        if joint_angles:
+            self._limb.set_joint_position_speed(self._speed)
+            self._limb.move_to_joint_positions(joint_angles, timeout=20.0, threshold=self._accuracy)
+            self._limb.set_joint_position_speed(0.3)
+        else:
+            rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")
+
+    def gripper_open(self):
+        self._gripper.open()
+        rospy.sleep(0.2)
+
+    def gripper_close(self):
+        self._gripper.close()
+        rospy.sleep(0.2)
+
+    def _approach(self, pose):
+        approach = copy.deepcopy(pose)
+        # approach with a pose the hover-distance above the requested pose
+        approach.position.z = approach.position.z + self._hover_distance
+        joint_angles = self.ik_request(approach)
+        self._guarded_move_to_joint_position(joint_angles)
+
+    def _retract(self):
+        # retrieve current pose from endpoint
+        current_pose = self._limb.endpoint_pose()
+        ik_pose = Pose()
+        ik_pose.position.x = current_pose['position'].x
+        ik_pose.position.y = current_pose['position'].y
+        ik_pose.position.z = current_pose['position'].z + self._hover_distance
+        ik_pose.orientation.x = current_pose['orientation'].x
+        ik_pose.orientation.y = current_pose['orientation'].y
+        ik_pose.orientation.z = current_pose['orientation'].z
+        ik_pose.orientation.w = current_pose['orientation'].w
+        joint_angles = self.ik_request(ik_pose)
+        # servo up from current pose
+        self._guarded_move_to_joint_position(joint_angles)
+
+    def _servo_to_pose(self, pose):
+        # servo down to release
+        joint_angles = self.ik_request(pose)
+        self._guarded_move_to_joint_position(joint_angles)
+
+    def pick(self, pose):
+        # open the gripper
+        self.gripper_open()
+        # servo above pose
+        print('Approaching')
+        self._approach(pose)
+        # servo to pose
+        self._servo_to_pose(pose)
+        print('Ready to grip')
+        # close gripper
+        self.gripper_close()
+        print('grip')
+        # retract to clear object
+        self._retract()
+
+    def place(self, pose):
+        # servo above pose
+        self._approach(pose)
+        # servo to pose
+        self._servo_to_pose(pose)
+        # open the gripper
+        self.gripper_open()
+        # retract to clear object
+        self._retract()
+
+
+print(brickstuff[1]['pose'].position.x)
+
+rospy.init_node("I_still_have_some_hope")  # Am I wrong??
+
+left_pnp = PickAndPlace('left', hover_distance)
+
+print(brickstuff[0]['pose'])

target_poses.py

@@ -1,4 +1,29 @@
+from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion)
+# ##DELETE THIS
+# class Position():
+# 	def __init__(self):
+# 		self.x = 0
+# 		self.y = 0
+# 		self.z = 0
 
+# class Orientation():
+# 	def __init__(self):
+# 		self.x = 0
+# 		self.y = 0
+# 		self.z = 0
+# 		self.w = 0
+
+# class EAngle():
+# 	def __init__(self):
+# 		self.roll = 0
+# 		self.pitch = 0
+# 		self.yaw = 0
+
+# class Pose():
+# 	def __init__(self):
+# 		self.position = Position()
+# 		self.orientation = Orientation()
+# ##DELETE THIS
 
 startv = Pose()
 startv.position.x = 0.474