def __init__(self):
# Instance Variables
self.P_gain = 2e-3
self.I_gain = 0
self.D_gain = 0
self.int_err_x = 0
self.int_err_y = 0
self.x_bnd = [-0.27, 0.38]
self.y_bnd = [0.35, 0.85]
self.last_err_x = 0
self.last_err_y = 0
# Initialize Sawyer
rospy.init_node('sawyer_comm', anonymous=True)
self.limb = intera_interface.Limb('right')
self.kin = sawyer_kinematics('right')
joint_positions = go_to.joint_angle_arg(joint_angles=[
math.pi / 2, -math.pi / 3, 0, 2 * math.pi / 3, 0, -math.pi / 3,
0.57
],
speed_ratio=.225,
accel_ratio=0.1)
go_to.joint_angles(joint_positions)
self.start_pose = self.limb.endpoint_pose()
if VERBOSE:
self.lastTime = rospy.get_time()
'''
# Debug: move in a square
s = 0.1
self.limb.set_command_timeout(2)
rate = rospy.Rate(100)
x = np.array([s,0,0,0,0,0]);
while self.limb.endpoint_pose()['position'][0]<0.25:
self.set_endpoint_velocity(x)
rate.sleep()
x = np.array([0,-s,0,0,0,0]);
while self.limb.endpoint_pose()['position'][1]>0.46:
self.set_endpoint_velocity(x)
rate.sleep()
x = np.array([-s,0,0,0,0,0]);
while self.limb.endpoint_pose()['position'][0]>-0.1658:
self.set_endpoint_velocity(x)
rate.sleep()
x = np.array([0,s,0,0,0,0]);
while self.limb.endpoint_pose()['position'][1]<0.7206:
self.set_endpoint_velocity(x)
rate.sleep()
x = np.array([0,0,0,0,0,0]);
self.set_endpoint_velocity(x)
'''
# Subscribing topics
rospy.Subscriber('/ball_error',
Int32MultiArray,
callback=self.rx_pos_error,
queue_size=1)
def angleclass_to_anglearg(self, angleclass, anglearg):
anglearg = go_to.joint_angle_arg(join,
t_angles=[
angleclass.j0, angleclass.j1,
angleclass.j2, angleclass.j3,
angleclass.j4, angleclass.j5,
angleclass.j6
])
return anglearg
def __init__(self):
self.old_time = time.time()
self.learner_quotient = 0
self.interaction_counter = 0
self.total_interactions = 0
self.time_elapsed = 0
self.learner_interaction = False
self.old_time = time.time()
self.new_time = time.time()
self.waypoints = []
self.waypoints_back = []
self.waypoint_limit = 99
self.effort = []
self.new_angle = 0
self.old_angle = 0
self.changed_angle = 0
self.eventnumber = 0
self.iframe_sequence_pub = rospy.Publisher('/iframe_sequence',
Int32,
queue_size=10)
self.unique_input_pub = rospy.Publisher('/unique_input',
String,
queue_size=10)
self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10)
self.gripper_pub = rospy.Publisher('/gripper_control',
Int32,
queue_size=10)
self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10)
self.interaction_pub = rospy.Publisher('/interaction_topic',
Int32,
queue_size=10)
rospy.Subscriber('/upper_cuff_button', Int32,
self.upper_cuff_button_callback)
rospy.Subscriber('/lower_cuff_button', Int32,
self.lower_cuff_button_callback)
rospy.Subscriber('/bot_sequence', Int32, self.callback)
rospy.Subscriber('/save_info', String, self.save_info_callback)
rospy.Subscriber('/right_navigator_button', String,
self.navigator_callback)
rospy.init_node('Sawyer_Sparrow_comm_node', anonymous=True)
self.limb = intera_interface.Limb('right')
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
self.startPose = self.limb.endpoint_pose()
self.startPose_container = cartPose()
self.startPose_arg = go_to.cartesian_pose_arg()
self.startJointAngles = self.limb.joint_angles()
self.newJointAngles_arg = go_to.joint_angle_arg()
self.newCartPose_arg = go_to.cartesian_pose_arg()
self.newCartPose_arg2 = go_to.cartesian_pose_arg()
self.newCartPose = cartPose()
self.newJointAngles = jointAngles()
self.waypoint_init_container = cartPose()
self.waypoint_init_arg = go_to.cartesian_pose_arg()
#Check if gripper is attached properly
self.grip = gripper_cuff.GripperConnect()
self.savedLocations = []
#Open gripper
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
#set constrained mode
zeroG.constrained(zeroG_all_constraints)
# self.startPose = self.limb.endpoint_pose()
# print(self.limb.endpoint_pose())
# self.startPose_container = self.pose_to_cartclass(pose = self.startPose, cartclass = self.startPose_container)
# print(self.startPose_container.position_z)
# print(self.startPose_container.position_z)
# self.startPose_arg = self.cartclass_to_cartarg(cartclass = self.startPose_container, cartarg = self.startPose_arg)
# print(self.startPose_container.position_z)
# go_to.cartesian_pose(self.startPose_arg)
#Check files and set text file to save to
if (file_save):
self.home = os.path.expanduser("~")
self.dir_path = self.home + '/Learner_Responses/module' + str(
module_number) + '/'
if not os.path.exists(self.dir_path):
os.makedirs(self.dir_path)
self.file_number = 0
self.file_path = self.dir_path + 'test_' + str(
self.file_number) + '.txt'
while (os.path.exists(self.file_path)):
self.file_number += 1
self.file_path = self.dir_path + 'test_' + str(
self.file_number) + '.txt'
self.file = open(self.file_path, 'w')
self.file.write('pretest \r\n' + str(self.old_time) + '\r\n')
self.file.close()
go_to.joint_angles(empty_angle_arg)
# #Complete grip
# grip_msg.data = 0
# self.gripper_pub.publish(grip_msg)
#Test code here
#effort testing
# self.waypoints = []
# self.waypoint_limit = 5
#initialize navigator for user inputs
navigator.right()
def callback(self, data):
self.new_time = time.time()
self.time_elapsed = self.new_time - self.old_time
self.file = open(self.file_path, 'a')
self.file.write('time stamp: ' + str(self.time_elapsed) + '\r\n' +
'callback: ' + str(data.data) + '\r\n')
self.file.close()
rospy.loginfo(rospy.get_caller_id() + "I heard" + str(data.data))
if (data.data == 0):
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
go_to.joint_angles(empty_angle_arg)
int_msg.data = 0
self.iframe_sequence_pub.publish(int_msg)
if (data.data == 1):
#To begin with, let me try to grab it with my hands
#Fail to grip the object
#Open Gripper
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
go_to.joint_angles(empty_angle_arg)
#Initial move
go_to.joint_angles(init_joint_arg)
#Go to gripping position
go_to.joint_angles(fail_init_joint_arg)
go_to.cartesian_pose(fail_pickup_cart_arg)
#Grip
grip_msg.data = 0
self.gripper_pub.publish(grip_msg)
#fail and then lift up
#Open gripper
rospy.sleep(1)
go_to.cartesian_pose(fail_above_pickup_cart_arg)
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
go_to.joint_angles(init_joint_arg)
go_to.joint_angles(joint_buttons)
int_msg.data = 1
self.iframe_sequence_pub.publish(int_msg)
self.learner_interaction = True
elif (data.data == 2):
#The first lesson of the day
#Properly grip the object
go_to.joint_angles(empty_angle_arg)
go_to.joint_angles(init_joint_arg)
go_to.joint_angles(fail_init_joint_arg)
go_to.joint_angles(success_init_joint_arg)
int_msg.data = 2
self.iframe_sequence_pub.publish(int_msg)
go_to.cartesian_pose(success_pickup_cart_arg)
#Complete grip
grip_msg.data = 0
self.gripper_pub.publish(grip_msg)
#Show off the proper gripping
go_to.cartesian_pose(success_above_pickup_cart_arg)
go_to.joint_angles(success_init_joint_arg)
go_to.joint_angles(init_joint_arg)
#put it back down
go_to.joint_angles(success_init_joint_arg)
go_to.cartesian_pose(success_pickup_cart_arg)
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
go_to.cartesian_pose(success_above_pickup_cart_arg)
#Get ready for next exercise
go_to.joint_angles(success_init_joint_arg)
elif (data.data == 3):
#To get a feeling for what I mean
self.learner_interaction = True
elif (data.data == 4):
#Let's try an example.
go_to.joint_angles(success_init_joint_arg)
self.learner_interaction = True
if int_msg.data != 4:
int_msg.data = 3
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 5):
#alright let's see
self.check_pickup()
if (self.compare_efforts(effort1=self.effort[0],
effort2=self.effort[1])):
#did not lift the shoe. Efforts are the same.
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
int_msg.data = 4
self.iframe_sequence_pub.publish(int_msg)
rospy.sleep(1)
else:
#did lift the shoe.
int_msg.data = 5
self.iframe_sequence_pub.publish(int_msg)
go_to.cartesian_pose(self.startPose_arg)
elif (data.data == 6):
#For example, now that I've picked up the shoe
go_to.cartesian_pose(above_bin_cart_arg)
self.startPose = self.limb.endpoint_pose()
self.newCartPose = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.newCartPose)
self.newCartPose.position_z = z_table + bin_height + box2_height - gripper_width
self.newCartPose_arg = self.newCartPose_arg = self.cartclass_to_cartarg(
cartclass=self.newCartPose, cartarg=self.newCartPose_arg)
go_to.cartesian_pose(self.newCartPose_arg)
rospy.sleep(1)
go_to.cartesian_pose(above_bin_cart_arg)
int_msg.data = 6
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 7):
#... it won't fit. So, before I can put the shoe in the shoebox...
self.startJointAngles = self.limb.joint_angles()
self.newJointAngles = self.limbangles_to_angleclass(
limbangles=self.startJointAngles,
angleclass=self.newJointAngles)
self.newJointAngles.j6 = self.startJointAngles['right_j6'] + 1.57
self.newJointAngles_arg = self.angleclass_to_anglearg(
angleclass=self.newJointAngles,
anglearg=self.newJointAngles_arg)
go_to.joint_angles(self.newJointAngles_arg)
int_msg.data = 7
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 8):
#Now I can package the box
#Update current pose
self.startPose = self.limb.endpoint_pose()
self.startPose_container = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.startPose_container)
self.startPose_arg = self.cartclass_to_cartarg(
cartclass=self.startPose_container, cartarg=self.startPose_arg)
self.newCartPose = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.newCartPose)
self.newCartPose.position_z = z_table + bin_depth + box2_height - gripper_width
self.newCartPose_arg = self.cartclass_to_cartarg(
cartclass=self.newCartPose, cartarg=self.newCartPose_arg)
go_to.cartesian_pose(self.newCartPose_arg)
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
go_to.cartesian_pose(self.startPose_arg)
go_to.joint_angles(init_joint_arg)
int_msg.data = 8
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 9):
#Now let's try moving this 3rd box into the 2nd bin.
go_to.joint_angles(success_init_joint_arg)
self.learner_interaction = True
elif (data.data == 10):
self.check_pickup()
if (self.compare_efforts(effort1=self.effort[0],
effort2=self.effort[1])):
#did not lift the shoe. Efforts are the same.
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
int_msg.data = 9
self.iframe_sequence_pub.publish(int_msg)
rospy.sleep(1)
else:
#did lift the shoe.
int_msg.data = 10
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 11):
#correct answer
int_msg.data = 11
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 12):
#correct answer continued
self.startPose = self.limb.endpoint_pose()
self.startPose_container = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.startPose_container)
self.startPose_arg = self.cartclass_to_cartarg(
cartclass=self.startPose_container, cartarg=self.startPose_arg)
grip_msg.data = 0
self.gripper_pub.publish(grip_msg)
go_to.cartesian_pose(into_second_bin_cart_arg)
go_to.cartesian_pose(self.startPose_arg)
int_msg.data = 12
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 13):
self.startPose = self.limb.endpoint_pose()
self.newCartPose = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.startPose_container)
self.newCartPose.position_z = z_table + box2_height - gripper_width + bin_height + .2
self.newCartPose_arg = self.cartclass_to_cartarg(
cartclass=self.startPose_container, cartarg=self.startPose_arg)
go_to.cartesian_pose(self.newCartPose_arg)
int_msg.data = 13
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 14):
go_to.cartesian_pose(above_second_bin_cart_arg)
int_msg.data = 14
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 15):
self.startPose = self.limb.endpoint_pose()
self.newCartPose = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.startPose_container)
self.newCartPose.position_z = z_table + box2_height + bin_depth - gripper_width
self.newCartPose_arg = self.cartclass_to_cartarg(
cartclass=self.newCartPose, cartarg=self.newCartPose_arg)
go_to.cartesian_pose(self.newCartPose_arg)
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
go_to.cartesian_pose(above_second_bin_cart_arg)
#skip incorrect answer
int_msg.data = 17
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 16):
#incorrect answer
int_msg.data = 15
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 17):
#same as 12, moving box into the bin.
self.startPose = self.limb.endpoint_pose()
self.startPose_container = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.startPose_container)
self.startPose_arg = self.cartclass_to_cartarg(
cartclass=self.startPose_container, cartarg=self.startPose_arg)
grip_msg.data = 0
self.gripper_pub.publish(grip_msg)
go_to.cartesian_pose(into_second_bin_cart_arg)
go_to.cartesian_pose(self.startPose_arg)
int_msg.data = 12
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 18):
#go back and grip the box
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
self.startPose = self.limb.endpoint_pose()
self.startPose_container = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.startPose_container)
self.startPose_container.position_z = z_table + box2_height - gripper_width + 0.01
self.startPose_arg = self.cartclass_to_cartarg(
cartclass=self.startPose_container, cartarg=self.startPose_arg)
go_to.cartesian_pose(self.startPose_arg)
grip_msg.data = 0
self.gripper_pub.publish(grip_msg)
int_msg.data = 18
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 19):
#Lift the box and then move back and then set it down
self.startPose = self.limb.endpoint_pose()
self.startPose_container = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.startPose_container)
self.startPose_container.position_z = self.startPose_container.position_z + box2_height + 0.01
self.startPose_arg = self.cartclass_to_cartarg(
cartclass=self.startPose_container, cartarg=self.startPose_arg)
go_to.cartesian_pose(self.startPose_arg)
go_to.cartesian_pose(above_third_box_cart_arg)
self.startPose = self.limb.endpoint_pose()
self.startPose_container = self.pose_to_cartclass(
pose=self.startPose, cartclass=self.startPose_container)
self.startPose_container.position_z = z_table + box2_height - gripper_width
self.newCartPose_arg = self.cartclass_to_cartarg(
cartclass=self.startPose_container,
cartarg=self.newCartPose_arg)
go_to.cartesian_pose(self.newCartPose_arg)
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
int_msg.data = 19
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 20):
go_to.joint_angles(success_init_joint_arg)
self.learner_interaction = True
elif (data.data == 21):
self.check_pickup()
if (self.compare_efforts(effort1=self.effort[0],
effort2=self.effort[1])):
#did not lift the shoe. Efforts are the same.
int_msg.data = 20
self.iframe_sequence_pub.publish(int_msg)
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
rospy.sleep(1)
else:
#did lift the shoe.
int_msg.data = 31
self.iframe_sequence_pub.publish(int_msg)
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
go_to.cartesian_pose(self.startPose_arg)
go_to.joint_angles(empty_angle_arg)
elif (data.data == 32):
go_to.joint_angles(empty_angle_arg)
zeroG.constrained(zeroG_truly_no_constraints)
int_msg.data = 32
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 33):
zeroG.constrained(zeroG_all_constraints)
rospy.sleep(0.1)
int_msg.data = 33
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 34):
go_to.joint_angles(self.savedLocations[0])
int_msg.data = 34
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 35):
go_to.joint_angles(self.savedLocations[1])
int_msg.data = 35
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 36):
go_to.joint_angles(self.savedLocations[2])
int_msg.data = 36
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 37):
go_to.joint_angles(self.savedLocations[3])
int_msg.data = 37
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 38):
go_to.joint_angles(self.savedLocations[4])
int_msg.data = 38
self.iframe_sequence_pub.publish(int_msg)
elif (data.data == 100):
JointAnglesLimb = self.limb.joint_angles()
JointAnglesClass = jointAngles()
JointAnglesClass = self.limbangles_to_angleclass(
JointAnglesLimb, JointAnglesClass)
JointAngleArg = go_to.joint_angle_arg()
JointAngleArg = self.angleclass_to_anglearg(
JointAnglesClass, JointAngleArg)
self.savedLocations.append(JointAngleArg)
# elif(data.data == 39):
# #Wrong waypoints were sent.
# for x in range(0, int(unique_msg.data[0])+1):
# #goes one additional waypoint more than the waypoint of intersection
# go_to.cartesian_pose(self.waypoints[x])
# del self.waypoints[:]
# int_msg.data = 39
# self.iframe_sequence_pub.publish(int_msg)
# elif(data.data == 40):
# rospy.sleep(10)
if __name__ == '__main__':
empty_angle_arg = go_to.joint_angle_arg()
empty_cartesian_arg = go_to.cartesian_pose_arg()
child = pexpect.spawn(
'rostopic pub -r 10 /robot/limb/right/suppress_cuff_interaction std_msgs/Empty'
)
#variables to be used
init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
init_joint_arg = go_to.joint_angle_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
fail_init_joint_arg = go_to.joint_angle_arg(joint_angles=[
# rospy.sleep(0.1)
# int_msg.data = 3
# self.iframe_sequence_pub.publish(int_msg)
# elif(data.data == 4):
# rospy.sleep(0.1)
# self.slideSequence()
# self.slideSequence() #calls 6
# self.slideSequence()
# rospy.sleep(0.1)
# int_msg.data = 4
# self.iframe_sequence_pub.publish(int_msg)
if __name__ == '__main__':
empty_angle_arg = go_to.joint_angle_arg()
empty_cartesian_arg = go_to.cartesian_pose_arg()
#variables to be used
init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
init_joint_arg = go_to.joint_angle_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
joint_buttons = go_to.joint_angle_arg(joint_angles=[
-0.0393427734375, -0.71621875, 0.022359375, 1.811921875,
-0.116017578125, 2.77036035156, -4.48708789063
def __init__(self):
self.slideEvent = True
self.slideClose = False
self.movedBoxes = 0
self.ready = False
self.last_msg = Int32()
self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10)
self.gripper_pub = rospy.Publisher('/gripper_control',
Int32,
queue_size=10)
self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10)
self.return_pub = rospy.Publisher('/sawyer_slider_return',
Int32,
queue_size=2)
rospy.Subscriber('/sawyer_sequence', Int32, self.callback)
#rospy.Subscriber('/sawyer_mode', Int32, self.sawyerMode_callback)
#rospy.Subscriber('/sawyer_time', Float32, self.sawyerTime_callback)
rospy.init_node('Sawyer_node', anonymous=True)
self.limb = intera_interface.Limb('right')
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
self.startPose = self.limb.endpoint_pose()
self.startPose_container = cartPose()
self.startPose_arg = go_to.cartesian_pose_arg()
self.startJointAngles = self.limb.joint_angles()
self.newCartPose_container = cartPose()
self.newJointAngles_arg = go_to.joint_angle_arg()
self.newCartPose_arg = go_to.cartesian_pose_arg()
self.newCartPose_arg2 = go_to.cartesian_pose_arg()
self.newCartPose = cartPose()
self.newJointAngles = jointAngles()
self.waypoint_init_container = cartPose()
self.waypoint_init_arg = go_to.cartesian_pose_arg()
#Check if gripper is attached properly
self.grip = gripper_cuff.GripperConnect()
#Open gripper
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
if (go_to.joint_angles(empty_angle_arg)):
int_msg.data = 1
self.return_pub.publish(int_msg)
# go_to.joint_angles(step2_conveyor_arg)
# self.startPose = self.limb.endpoint_pose()
# self.startPose_container = self.pose_to_cartclass(self.startPose, self.startPose_container)
# self.startPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.startPose_arg)
# self.startPose_container.position_z = z_conveyor + box_height - gripper_width
# self.newCartPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.newCartPose_arg)
# if(go_to.cartesian_pose(self.newCartPose_arg)):
# grip_msg.data = 0
# self.gripper_pub.publish(grip_msg)
# rospy.sleep(1)
# go_to.cartesian_pose(self.startPose_arg)
# go_to.cartesian_pose(self.newCartPose_arg)
# grip_msg.data = 1
# self.gripper_pub.publish(grip_msg)
# go_to.cartesian_pose(self.startPose_arg)
navigator.right()
# rospy.sleep(0.1)
# elif(data.data == 6):
# rospy.sleep(0.1)
# status_msg.data = 0
# self.return_pub.publish(status_msg)
# go_to.cartesian_pose(pickup_carriage_cart_arg)
# grip_msg.data = 0
# self.gripper_pub.publish(grip_msg)
# go_to.cartesian_pose(above_carriage_cart_arg)
# status_msg.data = 1
# self.return_pub.publish(status_msg)
if __name__ == '__main__':
empty_angle_arg = go_to.joint_angle_arg()
empty_cartesian_arg = go_to.cartesian_pose_arg()
#variables to be used
init_cartesian_arg = go_to.cartesian_pose_arg(joint_angles=[
-0.438147460938, -0.678481445312, -0.433721679687, 1.33986621094,
-0.763953125, -1.132484375, 0.959416015625
])
init_joint_arg = go_to.joint_angle_arg(joint_angles=[
-0.33585546875, -0.379596679687, -1.60570117188, 1.12274511719,
-1.9501328125, -1.73130761719, 1.83596582031
])
joint_buttons = go_to.joint_angle_arg(joint_angles=[
-0.0393427734375, -0.71621875, 0.022359375, 1.811921875,
-0.116017578125, 2.77036035156, -4.48708789063