From 54646e206c6c69d02676dd004c32d978603dd1f8 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Fri, 14 Feb 2020 23:49:23 +0000 Subject: [PATCH] Update --- new_main.py | 156 ++++++++++++++++++++++++++++++++++++++++++++++++ target_poses.py | 25 ++++++++ 2 files changed, 181 insertions(+) create mode 100644 new_main.py diff --git a/new_main.py b/new_main.py new file mode 100644 index 0000000..9237902 --- /dev/null +++ b/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']) \ No newline at end of file diff --git a/target_poses.py b/target_poses.py index e948d85..876cb00 100644 --- a/target_poses.py +++ b/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