max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
6bde8d8688
robotics/new_main.py
Max Hunt 6bde8d8688 Update
2020-03-02 09:51:16 +00:00

387 lines
12 KiB
Python
Raw Blame History

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 time
import baxter_interface
import target_poses as tps
import tuck_arms
import target_angles as ta
brickstuff = tps.brick_directions_notf
global sumulation
simulation = True
debug = False
class PickAndPlace(object):
def __init__(self, limb, hover_distance = 0.10, verbose=True, speed=0.2, accuracy=baxter_interface.settings.JOINT_ANGLE_TOLERANCE):
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 _guarded_move_to_joint_position(self, joint_angles):
if joint_angles:
self._limb.set_joint_position_speed(0.1)
self._limb.move_to_joint_positions(joint_angles, timeout=20.0, threshold=self._accuracy)
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 send(self, angles):
self._guarded_move_to_joint_position(angles)
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
print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
print()
print()
print('Linb Joints:')
print(limb_joints)
print()
print()
print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
return limb_joints
def gripperPosition(self):
return self._gripper.position()#class that handles moving the robot, gripper ans IK
if simulation: #deals with gazebo objects, not useful in the real world
brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9']
with open ("brick/model.sdf", "r") as brick_file:brick_sdf=brick_file.read().replace('\n', '')
with open ("L3-table/model.sdf", "r") as table_file:table_sdf=table_file.read().replace('\n', '')
with open ("brick/static-b.sdf", "r") as table_file:static_brick=table_file.read().replace('\n', '')
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)
def cleanup(): #deals with gazebo objects, not useful in the real world
for obj in brick_ids:
delete_model(obj)
delete_model('t1')
delete_model('t2')
# Horrible hack that deals with gazebo objects, not useful in the real world
import numpy as np
def etq(roll, pitch, yaw):
qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
return [qx, qy, qz, qw]
# Horrible hack that deals with gazebo objects, not useful in the real world
def spawn_v_brick():#deals with gazebo objects(spawns a vertical brick), not useful in the real world
brick_pose = Pose()
brick_pose.position.x = 0.475
brick_pose.position.y = 0.759
brick_pose.position.z = 0.818
QUATS = etq(0, 1.57, 1.57)
brick_pose.orientation.x = QUATS[0]
brick_pose.orientation.y = QUATS[1]
brick_pose.orientation.z = QUATS[2]
brick_pose.orientation.w = QUATS[3]
brick_reference_frame = 'world'
brick_id = brick_ids.pop()
spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
def spawn_h_brick():#deals with gazebo objects(spawns a horizontal brick), not useful in the real world
brick_pose = Pose()
brick_pose.position.x = 0.4664
brick_pose.position.y = 0.8069
brick_pose.position.z = 0.7533
brick_pose.orientation.x = 0
brick_pose.orientation.y = 0
brick_pose.orientation.z = 0.707
brick_pose.orientation.w = 0.707
brick_reference_frame = 'world'
brick_id = brick_ids.pop()
spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
def spawn_tables():#deals with gazebo objects(spawns tables), not useful in the real world
table1 = Pose()
table1.position.x = 1.160
table1.position.y = 0.365
table1.position.z = -0.359
table1.orientation.x = 0
table1.orientation.y = 0
table1.orientation.z = 0
table1.orientation.w = 0
table2 = copy.deepcopy(table1)
table2.position.x = 0.996
table2.position.y = 1.018
table2.position.z = -0.003
table_reference_frame = 'world'
table1_id = 't1'
table2_id = 't2'
spawn_sdf(table1_id, table_sdf, "/", table1, table_reference_frame)
spawn_sdf(table2_id, table_sdf, "/", table2, table_reference_frame)
rospy.init_node("I_still_have_some_hope") # Am I wrong??
if simulation:
cleanup()
spawn_tables()
tuck_arms.init_arms() #Make sure the arms are where they meant to be
hover_distance = 0.2 #Used for IK, not useful to us anymore
left_pnp = PickAndPlace('left', hover_distance) #Initializer
def V_Routine(): #Spawns brick (if sim), sends gripper to brick position, asks if ready (if in debug),
# picks it up and checks that the brick is acc in the grippers
if simulation:
spawn_v_brick()
left_pnp.send(ta.V_approach)
if debug:
x = raw_input('Ready?')
left_pnp.send(ta.V_pickup)
left_pnp.gripper_close()
gripper_state = left_pnp.gripperPosition()
if gripper_state < 10:
command = raw_input('\n \nPROBLEM DETECTED!!!\n(C)ontinue, (A)bort, (O)pen gripper\n>_ ')
if command in {'C', 'c', 'continue'}:
pass
elif command in {'O', 'o', 'open'}:
open_and_wait()
else:
print('Exiting')
exit(0)
time.sleep(0.5)
left_pnp.send(ta.V_approach)
def open_and_wait(): #Self explanatory
left_pnp.gripper_open()
if debug:
x = raw_input('Close?')
left_pnp.gripper_close()
time.sleep(0.5)
def H_Routine():#Spawns brick (if sim), sends gripper to brick position, asks if ready (if in debug),
# picks it up and checks that the brick is acc in the grippers
if simulation:
spawn_h_brick()
left_pnp.send(ta.H_approach)
if debug:
x = raw_input('Ready?')
left_pnp.send(ta.H_pickup)
left_pnp.gripper_close()
gripper_state = left_pnp.gripperPosition()
if gripper_state < 10:
command = raw_input('\n \nPROBLEM DETECTED!!!\nGripper has nothing in it...\n(C)ontinue, (A)bort, (O)pen gripper\n>_ ')
if command == 'C':
pass
elif command == 'c':
pass
elif command == 'O':
open_and_wait()
elif command == 'o':
open_and_wait()
else:
exit(0)
time.sleep(0.5)
left_pnp.send(ta.H_approach)
left_pnp.gripper_open() #Duh
V_Routine()
left_pnp.send(ta.B_1_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_1_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_1_A)
V_Routine()
left_pnp.send(ta.B_2_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_2_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_2_A)
V_Routine()
left_pnp.send(ta.B_3_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_3_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_3_A)
H_Routine()
left_pnp.send(ta.B_4_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_4_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_4_A)
H_Routine()
left_pnp.send(ta.B_5_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_5_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_5_A)
V_Routine()
left_pnp.send(ta.B_6_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_6_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_6_A)
V_Routine()
left_pnp.send(ta.B_7_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_7_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_7_A)
H_Routine()
left_pnp.send(ta.B_8_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_8_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_8_A)
V_Routine()
left_pnp.send(ta.B_9_A)
if debug:
x = raw_input('Continue?: ')
left_pnp.send(ta.B_9_P)
if debug:
x = raw_input('Continue?: ')
left_pnp.gripper_open()
left_pnp.send(ta.B_9_za)
left_pnp.send(ta.B_9_zb)
left_pnp.send(ta.H_pickup)
time.sleep(1)
print("#################################################################################################################")
print("#...............................................................................................................#")
print("#...............................................................................................................#")
print("#...............................................................................................................#")
print("#.................. ____ _ _ _ _ ____ _ _ _ _ _.................#")
print("#................../ ___| ___ _ __ __| | (_)_ __ | |_| |__ ___ | __ )(_)_ __ __| | | | |................#")
print("#..................\\___ \\ / _ \\ '_ \\ / _` | | | '_ \\ | __| '_ \\ / _ \\ | _ \\| | '__/ _` | | | |................#")
print("#.................. ___) | __/ | | | (_| | | | | | | | |_| | | | __/ | |_) | | | | (_| |_|_|_|................#")
print("#..................|____/ \\___|_| |_|\\__,_| |_|_| |_| \\__|_| |_|\\___| |____/|_|_| \\__,_(_|_|_)................#")
print("#...............................................................................................................#")
print("#...............................................................................................................#")
print("#...............................................................................................................#")
print("#################################################################################################################")
Reference in New Issue View Git Blame Copy Permalink