From 8a242c32018d3e17e0215973384dac72c594a998 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Sat, 8 Feb 2020 17:11:00 +0000 Subject: [PATCH] Update --- BrickMachine.py | 427 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 427 insertions(+) create mode 100644 BrickMachine.py diff --git a/BrickMachine.py b/BrickMachine.py new file mode 100644 index 0000000..9378e76 --- /dev/null +++ b/BrickMachine.py @@ -0,0 +1,427 @@ +import argparse +import struct +import sys +import copy + +import rospy +import rospkg + +from math import radians + +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 + +from tf.transformations import quaternion_from_euler + +table = { + 'id':'t1', + 'rframe':'world', + 'x':0.85, + 'y':0.20, + 'z':0, + 'roll':0, + 'pitch':0, + 'yaw':radians(90) + } + +bricks_end =[{ + 'id':'a1', + 'rframe':'t1', + 'x':0.46, + 'y':-0.19, + 'z':0.822, + 'roll':0, + 'pitch':-1.570796, + 'yaw':radians(90) + }, + { + 'id':'a2', + 'rframe':'a1', + 'x':0, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'a3', + 'rframe':'a1', + 'x':0, + 'y':0, + 'z':-0.372, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'b1', + 'rframe':'a1', + 'x':0.127, + 'y':0, + 'z':-0.085, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'id':'b2', + 'rframe':'a1', + 'x':0.127, + 'y':0, + 'z':-0.287, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'id':'c1', + 'rframe':'a1', + 'x':0.254, + 'y':0, + 'z':-0.093, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'c2', + 'rframe':'a1', + 'x':0.254, + 'y':0, + 'z':-0.287, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'d1', + 'rframe':'a1', + 'x':0.381, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'id':'e1', + 'rframe':'a1', + 'x':0.508, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':0, + 'yaw':0 + }] + +bricks_start =[{ + 'id':'ia1', + 'rframe':'t1', + 'x':-0.519, + 'y':0.134, + 'z':0.770, + 'roll':radians(90), + 'pitch':0, + 'yaw':radians(90) + }, + { + 'id':'ia2', + 'rframe':'ia1', + 'x':0, + 'y':0, + 'z':0.192, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ia3', + 'rframe':'ia1', + 'x':0, + 'y':0, + 'z':0.384, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ib1', + 'rframe':'ia1', + 'x':0, + 'y':0.087, + 'z':0, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ib2', + 'rframe':'ia1', + 'x':0, + 'y':0.087, + 'z':0.192, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ic1', + 'rframe':'ia1', + 'x':0, + 'y':0.087, + 'z':0.384, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ic2', + 'rframe':'ia1', + 'x':0, + 'y':0.175, + 'z':0, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'id1', + 'rframe':'ia1', + 'x':0, + 'y':0.175, + 'z':0.192, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ie1', + 'rframe':'ia1', + 'x':0, + 'y':0.175, + 'z':0.384, + 'roll':0, + 'pitch':0, + 'yaw':0 + }] + + +class PickAndPlace(object): + def __init__(self, limb, hover_distance = 0.10, verbose=True): + 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(1.0) + 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.move_to_joint_positions(joint_angles) + else: + rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.") + + def gripper_open(self): + self._gripper.open() + rospy.sleep(1.0) + + def gripper_close(self): + self._gripper.close() + rospy.sleep(1.0) + + 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() + + + +def load_objects(): + # Load Table SDFs + with open ("models/L3-table/model.sdf", "r") as table_file:table_xml=table_file.read().replace('\n', '') + with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '') + + # Spawn Table SDF + rospy.wait_for_service('/gazebo/spawn_sdf_model') + + spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) + delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) + + table_pose = Pose() + table_pose.position = Point(x=table['x'], y=table['y'], z=table['z']) + tqo = quaternion_from_euler(table['roll'], table['pitch'], table['yaw']) + table_pose.orientation = Quaternion(tqo[0], tqo[1], tqo[2], tqo[3]) + table_reference_frame="world" + spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame) + + brick_ents = [] + + for brick in bricks: + ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation + brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z'])) + brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z']) + bqo = quaternion_from_euler(brick['roll'], brick['pitch'], brick['yaw']) + brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3]) + brick_reference_frame=brick['rframe'] + brick_id = brick['id'] + brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame)) + + + + + + + + +class debug_functions(): + with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '') + + def t(): + delete_model(table['id']) + tableLoader() + + def tableLoader(): + with open ("models/L3-table/model.sdf", "r") as table_file:table_xml=table_file.read().replace('\n', '') + table_pose = Pose() + table_pose.position = Point(x=table['x'], y=table['y'], z=table['z']) + tqo = quaternion_from_euler(table['roll'], table['pitch'], table['yaw']) + table_pose.orientation = Quaternion(tqo[0], tqo[1], tqo[2], tqo[3]) + table_reference_frame="world" + spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame) + + def make(num = 0, arr = bricks_i): + if num == 0: num = len(arr) + for ix in range(0, num):#brick in arr: + brick = arr[ix] + ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation + brick_pose = Pose() + brick_pose.position.x = brick['x'] + brick_pose.position.y = brick['y'] + brick_pose.position.z = brick['z'] + bqo = quaternion_from_euler(ber[0], ber[1], ber[2]) + brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3]) + brick_reference_frame=brick['rframe'] + brick_id = brick['id'] + brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame)) + + def remove(arr2 = bricks_i): + for i in arr2: + delete_model(i['id']) + + + def x_place_bricks(): + rospy.wait_for_service('/gazebo/spawn_sdf_model') + spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) + delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) + with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '') + for brick in bricks: + ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation + brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z'])) + brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z']) + bqo = quaternion_from_euler(brick['roll'], brick['pitch'], brick['yaw']) + brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3]) + spawn_sdf(brick['id'], brick_xml, "/", brick_pose, brick['rframe']) + +