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_smooth(neutral=False,
arm='right',
p_list=[pos_y_right, pos_m_right, pos_c_right],
threshold=0.2,
speed=0.8)
time.sleep(0.8)
move_list_smooth(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('image_listener')
move_list_smooth(arm="left", p_list=[startPos], speed=0.8)
thread_get_image = threading.Thread(name='get_image', target=get_image)
thread_align_camera = threading.Thread(name='align_camera',
target=align_camera)
thread_get_image.start()
thread_align_camera.start()
def ik_test(limb):
rospy.init_node("rsdk_ik_service_client")
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
'left': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.558,
y=0.752,
z=-0.05,
),
orientation=Quaternion(
x=-0.866894936773,
y=0.885980397775,
z=0.008155782462,
w=0.262162481772,
),
),
),
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.656982770038,
y=-0.852598021641,
z=0.0388609422173,
),
orientation=Quaternion(
x=0.367048116303,
y=0.885911751787,
z=-0.108908281936,
w=0.261868353356,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except Exception as e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
if (resp.isValid[0]):
print("SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
print(limb_joints)
move_list_smooth(arm='left', p_list=[limb_joints])
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return 0
def align_camera():
global img_w, obj_w, img_h, obj_h
global boRun
boAlign = True
speed = 0.0005
while boRun and boAlign:
x_move = 0.0
z_move = 0.0
if abs((img_w * 0.5) - obj_w) > 20:
if (img_w * 0.5) < obj_w:
x_move = speed
#cartesian_move_rel(limb="left", x=0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
elif (img_w * 0.5) > obj_w:
x_move = -speed
#cartesian_move_rel(limb="left", x=-0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
if abs((img_h * 0.5) - obj_h) > 20:
if abs((img_h * 0.5) - obj_h) > 20:
if (img_h * 0.5) < obj_h:
z_move = -speed
# cartesian_move_rel(limb="left", x=0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
# cartesian_move_rel(limb="left", x=0.0, y=0.0, z=0.0)
elif (img_h * 0.5) > obj_h:
z_move = speed
# cartesian_move_rel(limb="left", x=-0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
# cartesian_move_rel(limb="left", x=0.0, y=0.0, z=0.0)
print(abs((img_w * 0.5) - obj_w))
if x_move == 0.0 and z_move == 0.0:
time.sleep(0.5)
if x_move == 0.0 and z_move == 0.0:
print("centered")
move_list_smooth(arm="left",
p_list=[{
"left_w2": 0.2,
"left_w0": -1.35
}],
speed=0.8)
cartesian_move_rel(limb="left", x=0.0, y=-0.15, z=0.0)
thread_open_hand = threading.Thread(name='open_hand',
target=open_hand)
thread_open_hand.start()
boAlign = False
else:
#cartesian_move_rel(limb="left", x=x_move, y=0.0, z=z_move, threshold=0.1)
cartesian_move_rel(limb="left",
x=x_move * abs((img_w * 0.5) - obj_w),
y=0.0,
z=z_move * abs((img_h * 0.5) - obj_h),
threshold=0.1)
obj_w = img_w * 0.5
obj_h = img_h * 0.5
time.sleep(0.19)
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_smooth(arm='left',
p_list=[position_1],
threshold=0.05,
speed=0.8)
for i in range(2):
move_list_smooth(arm='left',
p_list=[position_2, position_1],
threshold=0.05,
speed=0.5)
move_list_smooth(arm='left',
p_list=[position_init],
threshold=0.05,
speed=0.5)
def main():
rospy.init_node("salute")
left = baxter_interface.Limb('left')
position_1 = {
'left_w0': 0.6128253240600586,
'left_w1': 1.361024452496338,
'left_w2': -1.6409759459655764,
'left_e0': 0.053689327514648444,
'left_e1': 0.9679418760498048,
'left_s0': 1.7023351774108888,
'left_s1': -0.14994662184448243
}
move_list_smooth(neutral=False,
arm='left',
p_list=[position_1],
threshold=0.1)
time.sleep(2.0)
left.move_to_neutral(15.0)
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
positions_1 = ({
'right_w2': -2.6921,
'right_e1': 0.0422,
'right_w0': 2.6058,
'right_w1': 0.4249,
'right_s0': 0.8295,
'right_s1': 0.4533,
'right_e0': 0.0610
})
positions_2 = ({'right_s1': 0.15, 'right_s0': 1.1136, 'right_w0': -0.0912})
positions_3 = ({'right_s1': 0.3, 'right_s1': 0.3})
print("Program started")
move_list_smooth(neutral=False, arm='right', p_list=[positions_1], speed=0.5)
time.sleep(0.5)
move_list_smooth(neutral=False, arm='right', p_list=[positions_2], speed=0.5)
time.sleep(0.5)
move_list_smooth(neutral=False, arm='right', p_list=[positions_1], speed=0.5)
time.sleep(0.5)
move_list_smooth(neutral=False, arm='right', p_list=[positions_3], speed=0.5)
right.move_to_neutral(10)
print("Program executed")
from baxter_interface import CHECK_VERSION
pos_1 = {
'left_w0': 1.170427339819336,
'left_w1': -0.04947088035278321,
'left_w2': 0.04256796681518555,
'left_e0': -1.196888508380127,
'left_e1': 0.08091748646850587,
'left_s0': 0.5403447319152832,
'left_s1': 0.1710388576538086
}
pos_2 = {
'left_w0': 1.185383652484131,
'left_w1': 0.011504855895996095,
'left_w2': -0.14726215546875002,
'left_e0': -1.2030244315246583,
'left_e1': -0.050237870745849615,
'left_s0': 0.485121423614502,
'left_s1': -0.09357282795410157
}
rospy.init_node("hand_wave")
left = baxter_interface.Limb('left')
for i in range(3):
move_list_smooth(arm='left',
p_list=[pos_1, pos_2],
speed=0.5,
threshold=0.05)
def ik_move(limb, x=None, y=None, z=None):
def set_x(value):
poses[limb].pose.position.y = 1 - value
def set_y(value):
poses[limb].pose.position.z = value
def set_z(value):
poses[limb].pose.position.x = value
def get_x():
return 1 - poses[limb].pose.position.y
def get_y():
return poses[limb].pose.position.z
def get_z():
return poses[limb].pose.position.x
rospy.init_node("rsdk_ik_service_client")
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
limb_interface = baxter_interface.Limb(limb)
current_pose = limb_interface.endpoint_pose()
poses = {
'left':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(x=current_pose['position'].x,
y=current_pose['position'].y,
z=current_pose['position'].z),
orientation=Quaternion(
x=-0.866894936773,
y=0.885980397775,
z=0.008155782462,
w=0.262162481772,
),
),
),
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.656982770038,
y=-0.852598021641,
z=0.0388609422173,
),
orientation=Quaternion(
x=0.367048116303,
y=0.885911751787,
z=-0.108908281936,
w=0.261868353356,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
if x == None:
x = get_x()
if y == None:
y = get_y()
if z == None:
z = get_z()
set_x(x)
set_y(y)
set_z(z)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except Exception as e:
rospy.logerr("Service call failed: %s" % (e, ))
return 1
if (resp.isValid[0]):
print("SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
print(limb_joints)
move_list_smooth(arm='left', p_list=[limb_joints])
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return 0
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_smooth(
neutral=False,
arm='left',
p_list=[pos_y_left, pos_m_left, pos_c_left, pos_a_left],
threshold=0.2,
speed=0.8)
time.sleep(1.0)
self.arm.move_to_neutral(15.0)