def run(self):
pos_y_right = {
'right_s0': -0.4755340437011719,
'right_s1': -1.0722525695068361,
'right_w0': 0.13690778516235352,
'right_w1': 0.1334563283935547,
'right_w2': 2.6476508368652345,
'right_e0': 3.050704288421631,
'right_e1': -0.013422331878662111
}
pos_m_right = {
'right_s0': -0.08436894323730469,
'right_s1': -0.4878058899902344,
'right_w0': -0.3413107249145508,
'right_w1': 1.7521895529602052,
'right_w2': 0.20171847337646487,
'right_e0': 2.9437091285888672,
'right_e1': 1.2471263791259766
}
pos_c_right = {
'right_s0': -0.679553488256836,
'right_s1': -1.1017816996398926,
'right_w0': 0.37927674937133793,
'right_w1': 0.9223059476623536,
'right_w2': -0.22549517556152346,
'right_e0': 2.8574227093688966,
'right_e1': 0.900446721459961
}
pos_a_right = {
'right_s0': 0.03719903406372071,
'right_s1': -0.7248059214477539,
'right_w0': -0.2067039109313965,
'right_w1': 0.18983012228393556,
'right_w2': -0.1457281746826172,
'right_e0': 2.7358547320678714,
'right_e1': 1.200723460345459
}
self.arm.set_joint_position_speed(0.8)
move_list(neutral=False,
arm='right',
p_list=[pos_y_right, pos_m_right, pos_c_right],
threshold=0.2)
time.sleep(0.8)
move_list(neutral=False,
arm='right',
p_list=[pos_a_right],
threshold=0.1)
time.sleep(1.0)
self.arm.move_to_neutral(15.0)
def main():
rospy.init_node("salute")
left = baxter_interface.Limb('left')
position_1 = {'left_w0': -2.1360682446899415, 'left_w1': -0.11888351092529298, 'left_w2': -0.2795679982727051, 'left_e0': -2.3301168141357422, 'left_e1': 2.4382624595581057, 'left_s0': 0.004601942358398438, 'left_s1': 0.3413107249145508}
move_list(neutral=False, arm='left', p_list=[position_1], threshold=0.1, speed=0.8)
time.sleep(2.0)
left.move_to_neutral(15.0)
def wheel_moved(v):
print ("Wheel Increment: %d, New Value: %s" % (v, nav.wheel))
angles = left.joint_angles()
if v == 1:
temp = angles['left_w2'] - 0.5
elif v == -1:
temp = angles['left_w2'] + 0.5
pos = {'left_w2': temp}
move_list(arm='left', p_list=[pos], timeout=0.05)
def run(self):
pos_y_left = {
'left_w0': 0.030296120526123047,
'left_w1': 0.03758252926025391,
'left_w2': -2.852820767010498,
'left_e0': -3.0349809853637697,
'left_e1': -0.01457281746826172,
'left_s0': 0.49739326990356447,
'left_s1': -1.087208882171631
}
pos_m_left = {
'left_w0': 0.683388440222168,
'left_w1': 1.7817186830932619,
'left_w2': -0.6055389153259277,
'left_e0': -3.0250101102539064,
'left_e1': 1.2601652158081056,
'left_s0': 0.15148060263061525,
'left_s1': -0.5890486218750001
}
pos_c_left = {
'left_w0': -0.23661653626098633,
'left_w1': 0.3271214026428223,
'left_w2': -3.0587576875488285,
'left_e0': -3.040733413311768,
'left_e1': 1.0829904350097657,
'left_s0': 0.645422415765381,
'left_s1': 0.43334957208251956
}
pos_a_left = {
'left_w0': 0.27534955111083986,
'left_w1': 0.1326893380004883,
'left_w2': 0.12156797730102539,
'left_e0': -2.780340174865723,
'left_e1': 1.2011069555419922,
'left_s0': -0.011888351092529297,
'left_s1': -0.7712088402282715
}
self.arm.set_joint_position_speed(0.8)
move_list(neutral=False,
arm='left',
p_list=[pos_y_left, pos_m_left, pos_c_left, pos_a_left],
threshold=0.2)
time.sleep(1.0)
self.arm.move_to_neutral(15.0)
def main():
rospy.init_node("Str")
left = baxter_interface.Limb('left')
position_1 = ({'left_w0': -0.054,
'left_w1': 1.370,
'left_w2': -1.926,
'left_e0': 0.244,
'left_e1': -0.027,
'left_s0': -0.633,
'left_s1': 0.007})
position_2 = ({'left_w0': -0.054,
'left_w1': 1.104,
'left_w2': -2.095,
'left_e0': 0.243,
'left_e1': -0.049,
'left_s0': -0.668,
'left_s1': 0.018})
position_3 = ({'left_w0': -0.054,
'left_w1': 0.904,
'left_w2': -2.200,
'left_e0': 0.243,
'left_e1': -0.069,
'left_s0': -0.698,
'left_s1': 0.018})
position_4 = ({'left_w0': -0.054,
'left_w1': 1.104,
'left_w2': -2.095,
'left_e0': 0.243,
'left_e1': -0.049,
'left_s0': -0.668,
'left_s1': 0.018})
position_5 = ({'left_w0': -0.054,
'left_w1': 1.370,
'left_w2': -1.926,
'left_e0': 0.244,
'left_e1': -0.027,
'left_s0': -0.633,
'left_s1': 0.007})
left.set_joint_position_speed(0.75)
move_list(neutral=False, arm='left', p_list=[position_1], threshold=1.0)
"""
def main():
rospy.init_node("hand_wave")
left = baxter_interface.Limb('left')
position_1 = ({'left_w0': -3.0476363268493656, 'left_w1': -0.05867476506958008, 'left_w2': -3.0587576875488285, 'left_e0': -3.041500403704834, 'left_e1': 1.8821944245849611, 'left_s0': -0.7171360175170899, 'left_s1': 0.6043884297363281})
position_2 = ({'left_e0': -2.2})
left.set_joint_position_speed(0.75)
move_list(neutral=False, arm='left', p_list=[position_1], threshold=0.1)
for i in range(2):
move_list(neutral=False, arm='left', p_list=[position_2, position_1], timeout=0.8)
left.move_to_neutral(15.0)
def main():
rospy.init_node("hand_wave")
left = baxter_interface.Limb('left')
position_init = left.joint_angles()
position_1 = ({
'left_w0': -3.0476363268493656,
'left_w1': -0.05867476506958008,
'left_w2': -3.0587576875488285,
'left_e0': -3.041500403704834,
'left_e1': 1.8821944245849611,
'left_s0': -0.7171360175170899,
'left_s1': 0.6043884297363281
})
position_2 = ({'left_e0': -2.5})
move_list(arm='left', p_list=[position_1], threshold=0.05, speed=0.8)
for i in range(2):
move_list(arm='left',
p_list=[position_2, position_1],
threshold=0.05,
speed=0.8)
move_list(arm='left', p_list=[position_init], threshold=0.05, speed=0.5)
def move_arm():
pos_up = {
'left_w0': 3.044951860473633,
'left_w1': -1.176179767767334,
'left_w2': -0.09932525590209962,
'left_e0': 3.05415574519043,
'left_e1': 1.5941895319885255,
'left_s0': 0.7811797153381348,
'left_s1': 0.39960199478759767
}
pos_down = {
'left_w0': 3.0123547687683105,
'left_w1': -0.7198204838928223,
'left_w2': 0.0,
'left_e0': 3.0533887547973633,
'left_e1': 0.28838838779296877,
'left_s0': 0.5756262899963379,
'left_s1': 0.9200049764831544
}
move_list(arm='left', p_list=[pos_up], speed=0.1, threshold=0.1)
move_list(arm='left', p_list=[pos_down], timeout=1.0, speed=0.1)
def move_arm():
print("moving down")
move_list(arm='left', p_list=[pos_down], speed=0.1)
rospy.signal_shutdown("shutdown")
def __init__(self):
self.distance = {}
root_name = "/robot/range/"
sensor_name = ["left_hand_range/state"]
#sensor_name = ["left_hand_range/state","right_hand_range/state"]
self.__left_sensor = rospy.Subscriber(root_name + sensor_name[0],
Range,
callback=self.__sensorCallback,
callback_args="left",
queue_size=1)
#self.__right_sensor = rospy.Subscriber(root_name + sensor_name[1],Range, callback=self.__sensorCallback, callback_args="right",queue_size=1)
def __sensorCallback(self, msg, side):
global left_distance
left_distance = msg.range
if left_distance < 0.16:
print("Distance: {:.2f}".format(left_distance * 100))
rospy.signal_shutdown("shutdown")
print("Checking height...")
rospy.init_node("height_check")
left = baxter_interface.Limb('left')
left.set_joint_position_speed(0.1)
move_list(arm='left', p_list=[pos_up])
thread_move_arm = threading.Thread(name='move_arm', target=move_arm)
thread_height_sensor = threading.Thread(name='height_sensor',
target=def_height_sensor)
thread_move_arm.start()
thread_height_sensor.start()
'left_s1': -0.00
})
pos_3_2 = ({
'left_w0': -0.30,
'left_w1': 2.40,
'left_w2': -3.05,
'left_e0': -0.29,
'left_e1': -0.60,
'left_s0': -0.75,
'left_s1': -0.00
})
pos_a = ()
pos_b = ()
pos_c = ()
pos_d = ()
left.set_joint_position_speed(1.0)
move_list(arm='left', p_list=[pos_in])
for i in range(5):
left.set_joint_position_speed(1.0)
move_list(arm='left', p_list=[pos_in, pos_2_2, pos_3_2], timeout=0.3)
left.move_to_neutral(15.0)
time.sleep(1.0)
cartesian_move_rel(limb="left", y=0.04, smooth=False)
#shake()
time.sleep(1.0)
print(abs(start_Y_pos - get_cartesian_positions('left')['position y:']))
cartesian_move_rel(limb="left", x=x_diff, y=0.05, z=z_diff, smooth=False)
time.sleep(1.0)
def lower_hand():
cartesian_move_rel(limb="left", y=-0.05, threshold=0.005, smooth=False)
move_list(arm='left', p_list=[start_pos])
time.sleep(1.0)
start_Y_pos = get_cartesian_positions('left')['position y:']
pump_and_up(x_diff=0.1)
lower_hand()
#cartesian_move_rel(limb="left", x=0.1)
pump_and_up(z_diff=-0.1)
lower_hand()
#cartesian_move_rel(limb="left", z=-0.1)
pump_and_up(x_diff=-0.1)
lower_hand()
#cartesian_move_rel(limb="left", x=-0.1)
pos_above_beans = {'left_w0': 0.04371845240478516, 'left_w1': -0.634301055065918, 'left_w2': 0.005752427947998047, 'left_e0': -0.11274758778076173, 'left_e1': -0.04947088035278321, 'left_s0': -1.0009224629516602, 'left_s1': 0.5583690061523437}
pos_around_beans = {'left_w0': -0.01112136069946289, 'left_w1': -0.3290388786254883, 'left_w2': 0.06941263057250976, 'left_e0': -0.1457281746826172, 'left_e1': -0.04985437554931641, 'left_s0': -1.0016894533447267, 'left_s1': 0.5307573520019532}
pos_pour_beans_align = {'left_w0': -1.9504565695678713, 'left_w1': -0.9038981782287598, 'left_w2': 1.4983157328552248, 'left_e0': -0.13575729957275393, 'left_e1': -0.04640291878051758, 'left_s0': -1.1255584018249511, 'left_s1': 0.22779614674072268}
pos_pour_beans = {'left_w0': -1.8426944193420411, 'left_w1': -0.7374612629333497, 'left_w2': -0.2442864401916504, 'left_e0': -0.16682041049194338, 'left_e1': -0.0487038899597168, 'left_s0': -1.0365875162292482, 'left_s1': 0.28110197905883794}
pos_pour_beans_shake = {'left_s1': 0.23110197905883794}
print("Program started")
right.set_joint_position_speed(0.5)
grip_left.calibrate()
move_list(neutral=False, arm='left', p_list=[pos_above_beans, pos_around_beans])
grip_left.close()
time.sleep(0.5)
move_list(neutral=False, arm='left', p_list=[pos_pour_beans_align], threshold=0.05)
move_list(neutral=False, arm='left', p_list=[pos_pour_beans, pos_pour_beans_shake, pos_pour_beans])
time.sleep(0.5)
move_list(neutral=False, arm='left', p_list=[pos_above_beans, pos_around_beans])
grip_left.open()
time.sleep(0.5)
move_list(neutral=False, arm='left', p_list=[pos_above_beans, pos_above_glass], threshold=0.1)
move_list(neutral=False, arm='left', p_list=[pos_around_glass])
grip_left.close()
time.sleep(0.5)
move_list(neutral=False, arm='left', p_list=[pos_under_spout])
grip_left.open()
time.sleep(0.5)
