def main():
global thetas
configureLeftHand()
planner = PathPlanner("right_arm")
grip = Gripper('right')
grip.calibrate()
raw_input("gripper calibrated")
grip.open()
table_size = np.array([3, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -5 - .112
#put cup
cup_pos = start_pos_xy
end_pos = goal_pos_xy
putCupObstacle(planner, cup_pos, "cup1")
putCupObstacle(planner, end_pos, "cup2")
planner.add_box_obstacle(table_size, "table", table_pose)
#move gripper to behind cup
position = cup_pos + np.array([-0.1, 0, -0.02])
orientation = np.array([0, np.cos(np.pi / 4), 0, np.cos(np.pi / 4)])
executePositions(planner, [position], [orientation])
raw_input("moved behind cup, press enter")
#move to cup and remove cup1 as obstacle since picking up
removeCup(planner, "cup1")
position = cup_pos + np.array([0, 0, -0.05])
executePositions(planner, [position], [orientation])
raw_input("moved to cup, press to close")
grip.close()
rospy.sleep(1)
raw_input("gripped")
moveAndPour(planner, end_pos)
raw_input("poured")
executePositions(planner, [position + np.array([0, 0, 0.02])],
[orientation])
grip.open()
removeCup(planner, "cup2")
def main():
"""
Main Script
"""
#===================================================
# Code to add gripper
#rospy.init_node('gripper_test')
# # Set up the right gripper
#right_gripper = robot_gripper.Gripper('right_gripper')
# #Calibrate the gripper (other commands won't work unless you do this first)
#print('Calibrating...')
#right_gripper.calibrate()
#rospy.sleep(2.0)
# #Close the right gripper to hold publisher
# # MIGHT NOT NEED THIS
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
#Tower
X = .075
Y = .15
Z = .0675
Xp = 0
Yp = 0
Zp = 0
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
# pose = PoseStamped()
# pose.header.stamp = rospy.Time.now()
# #TODO: Is this the correct frame name?
# pose.header.frame_id = "ar_marker_4"
# pose.pose.position.x = Xp
# pose.pose.position.y = Yp
# pose.pose.position.z = Zp
# pose.pose.orientation.x = Xpa
# pose.pose.orientation.y = Ypa
# pose.pose.orientation.z = Zpa
# pose.pose.orientation.w = Wpa
# planner.add_box_obstacle([X,Y,Z], "tower", pose)
#Table
X = 1
Y = 2
Z = .0675
Xp = 1.2
Yp = 0
Zp = -0.22
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
#TODO: Is this the correct frame name?
pose.header.frame_id = "base"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X, Y, Z], "table", pose)
try:
#right_gripper_tip
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_4"
#x, y, and z position
goal_1.pose.position.y = 0
goal_1.pose.position.z = .5
#removing the 6th layer
goal_1.pose.position.x = 0 #0.0825
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0.707
goal_1.pose.orientation.w = 0.707
plan = planner.plan_to_pose(goal_1, list())
if not planner.execute_plan(plan):
raise Exception("Execution failed0")
except Exception as e:
print e
def move(x, y, z, box_in):
planner = PathPlanner("right_arm")
#TODO: Add constraint restricting z > 0.
#Wait for the IK service to become available
#rospy.wait_for_service('compute_ik')
#rospy.init_node('service_query')
place_holder = {'images': [], 'camera_infos': []}
#rospy.Subscriber("/cameras/head_camera/image", Image, lambda x: place_holder['images'].append(x))
#rospy.Subscriber("/cameras/head_camera/camera_info", CameraInfo, lambda x: place_holder['camera_infos'].append(x))
limb = Limb("right")
#print(limb)
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
# while not rospy.is_shutdown() and count < 3:
# #calibration points 1-3, closest to robot right = origin, closest to robot left, id_card 3 3/8 inches forward from first point
# '''
# /cameras/head_camera/camera_info
# /cameras/head_camera/camera_info_std
# /cameras/head_camera/image
# '''
# count += 1
# raw_input("calibrate point " + str(count))
# pos = limb.endpoint_pose()
# calibration_points.append({})
# calibration_points[-1]['world_coordinates'] = (pos['position'].x, pos['position'].y, pos['position'].z)
# #calibration_points[-1]['camera_info'] = place_holder['camera_infos'][-1]
# #calibration_points[-1]['image'] = place_holder['images'][-1]
# for i in calibration_points:
# print(i['world_coordinates'])
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
## Add the controller here! IF you uncomment below, you should get the checkoff!
c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = box_in.pose.position.x
box.pose.position.y = box_in.pose.position.y
box.pose.position.z = box_in.pose.position.z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
planner.add_box_obstacle((3, 4, 0.10), "box", box)
# planner.add_box_obstacle((box_in.pose.position.x, box_in.pose.position.y, box_in.pose.position.z), "box", box)
# ree = input("some text")
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
_x, _y, _z = .67, -.2, .3
#x, y, z = .95, .11, .02
#x, y, z = .7, -.4, .2
print(0)
else:
raise Exception("not configured for sawyer yet.")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = _x
goal_1.pose.position.y = _y
goal_1.pose.position.z = _z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_1, [orien_const])
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move above point \n')
except Exception as e:
print(e)
traceback.print_exc()
else:
break
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
#x, y, z = .67, -.2, 0
# x, y, z = .79, .04, -.04
pass
else:
raise Exception("not configured for sawyer yet.")
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = x
goal_2.pose.position.y = y
goal_2.pose.position.z = z
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_2, [orien_const])
raw_input("Press <Enter> to move the right arm to goal pose 2: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move to point \n')
except Exception as e:
print(e)
traceback.print_exc()
else:
break
table_x_width = 1
table_y_length = 1.4
table_z_thick = 0.1
table_position_x = 0.6
table_position_y = 0.2
table_position_z = -0.35
table_pose = geometry_msgs.msg.PoseStamped()
table_pose.header.frame_id = "base"
table_pose.pose.orientation.w = 1.0
table_pose.pose.position.x = table_position_x
table_pose.pose.position.y = table_position_y
table_pose.pose.position.z = table_position_z
box_name = "table"
planner.add_box_obstacle(
np.array([table_x_width, table_y_length, table_z_thick]), box_name,
table_pose)
# kinect
center_of_table_plane = [table_position_x+table_x_width/2, \
table_position_y+table_y_length/2, \
table_position_z+table_z_thick/2]
kinect_pose = geometry_msgs.msg.PoseStamped()
kinect_pose.header.frame_id = "base"
kinect_pose.pose.orientation.w = 2.0
kinect_pose.pose.position.x = table_position_x + 0.55
kinect_pose.pose.position.y = table_position_y #+ table_y_length/2
kinect_pose.pose.position.z = 0.4
box_name = "kinect"
planner.add_box_obstacle(np.array([0.3, 0.3, 1]), box_name, kinect_pose)
def main(cube_pose, end_pose):
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 3.1415*2;
# orien_const.weight = 1.0;
start_pose = get_start(cube_pose, end_pose)
print("cube_pose", cube_pose)
print("start pose", start_pose)
#print(cube_pose)
# print(end_pose)
# size = np.array([0.01, 0.01, 0.01])
# planner.add_box_obstacle(size, "cube", cube_pose)
table_size = np.array([0.4, 1.2, 0.1])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
#x, y, and z position
table_pose.pose.position.x = 0.5
#Orientation as a quaternion
table_pose.pose.orientation.w = 1.0
planner.add_box_obstacle(table_size, "table", table_pose)
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(start_pose, [])
print(plan)
raw_input("Press <Enter> to move the right arm behind cube: ")
result = planner.execute_plan(plan)
if not result:
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
raw_input("Press <Enter> to remove cube")
# planner.remove_obstacle("cube")
# orien_const.absolute_z_axis_tolerance = 3.1415/4;
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(end_pose, [])
raw_input("Press <Enter> to move the cube to target: ")
result = planner.execute_plan(plan)
if not result:
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
global prev_msg
global sec_pre
plandraw = PathPlanner('right_arm')
# plandraw.grip?per_op
plandraw.start_position()
## BOX
box_size = np.array([2.4, 2.4, 0.1])
box_pose = PoseStamped()
box_pose.header.stamp = rospy.Time.now()
box_pose.header.frame_id = "base"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 0
box_pose.pose.position.z = -0.4
box_pose.pose.orientation.x = 0.00
box_pose.pose.orientation.y = 0.00
box_pose.pose.orientation.z = 0.00
box_pose.pose.orientation.w = 1.00
plandraw.add_box_obstacle(box_size, "box1", box_pose)
box_size2 = np.array([2.4, 2.4, 0.1])
box_pose2 = PoseStamped()
box_pose2.header.stamp = rospy.Time.now()
box_pose2.header.frame_id = "base"
box_pose2.pose.position.x = 0
box_pose2.pose.position.y = 0
box_pose2.pose.position.z = 1
box_pose2.pose.orientation.x = 0.00
box_pose2.pose.orientation.y = 0.00
box_pose2.pose.orientation.z = 0.00
box_pose2.pose.orientation.w = 1.00
plandraw.add_box_obstacle(box_size2, "box2", box_pose2)
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper_tip"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
def set_use_pen(pen_id, goal_1):
if pen_id == 0:
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -0.9848078
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = -0.1736482
if pen_id == 1:
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
if pen_id == 2:
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -0.9848078
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.1736482
waypoints = []
while not rospy.is_shutdown():
#raw_input("~~~~~~~~~~~~!!!!!!!!!!!!")
while not rospy.is_shutdown():
try:
while len(queue):
print(len(queue))
cur = queue.popleft()
x, y, z = cur.position_x, cur.position_y, cur.position_z
x -= 0.085 # ada different coordinate
z += 0.03
if cur.status_type != "edge_grad":
# ti bi !!!!! luo bi !!!!
if cur.status_type == "starting":
print("start")
# waypoints = []
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z + 0.12
#Orientation as a quaternion
# [0.766, -0.623, 0.139, -0.082]
# [-0.077408, 0.99027, -0.024714, ]
set_use_pen(cur.pen_type, goal_1)
waypoints.append(copy.deepcopy(goal_1.pose))
goal_1.pose.position.z -= 0.12
waypoints.append(copy.deepcopy(goal_1.pose))
# plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints)
# if not plandraw.execute_plan(plan):
# raise Exception("Starting execution failed")
# else:
# queue.pop(0)
elif cur.status_type == "next_point":
print("next")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
# goal_1.pose.orientation.x = 0.459962
# goal_1.pose.orientation.y = -0.7666033
# goal_1.pose.orientation.z = 0.0
# goal_1.pose.orientation.w = -0.4480562
set_use_pen(cur.pen_type, goal_1)
# waypoints = []
waypoints.append(copy.deepcopy(goal_1.pose))
# plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints)
# if not plandraw.execute_plan(plan):
# raise Exception("Execution failed, point is ", cur)
# else:
# queue.pop(0)
elif cur.status_type == "ending":
print("ppppppp ", cur)
# mmm = plandraw.get_cur_pos().pose
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z + 0.12
#Orientation as a quaternion
set_use_pen(1, goal_1)
# waypoints = []
waypoints.append(copy.deepcopy(goal_1.pose))
plan = plandraw.plan_to_pose(goal_1, [], waypoints)
waypoints = []
# queue.pop(0)
if not plandraw.execute_plan(plan):
raise Exception("Execution failed")
print("ti bi")
# rospy.sleep(5)
#raw_input("Press <Enter> to move next!!!")
except Exception as e:
print e
else:
#print("lllllllllllllllllllll")
break
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0
table_pose.pose.position.z = 0
table_pose.pose.orientation.x = 1
table_pose.pose.orientation.y = 0
table_pose.pose.orientation.z = 0
table_pose.pose.orientation.w = 0
table_size = [1, 1, 0.764]
planner_left.add_box_obstacle(table_size, "table", table_pose)
obstacles = list()
calibration = false
if calibration :
while not rospy.is_shutdown():
try:
#intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper
calib1 = PoseStamped()
calib1.header.frame_id = "base"
#x,y,z position
def main():
"""
Main Script
"""
#===================================================
# Code to add gripper
# rospy.init_node('gripper_test')
# # Set up the right gripper
# right_gripper = robot_gripper.Gripper('right_gripper')
# #Calibrate the gripper (other commands won't work unless you do this first)
# print('Calibrating...')
# right_gripper.calibrate()
# rospy.sleep(2.0)
# #Close the right gripper to hold publisher
# # MIGHT NOT NEED THIS
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = -1.1
pose.pose.position.y = 0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "left_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = -1.0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "right_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = 1.1
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "back_wall", pose)
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "ar_marker_1"
#orien_const.orientation.y = np.pi/2
#orien_const.orientation.w = 1;
orien_const.absolute_x_axis_tolerance = 1
orien_const.absolute_y_axis_tolerance = 1
orien_const.absolute_z_axis_tolerance = 1
orien_const.weight = .5
joint_const = JointConstraint()
# Constrain the position of a joint to be within a certain bound
joint_const.joint_name = "right_j6"
joint_const.position = 1.7314052734375
# the bound to be achieved is [position - tolerance_below, position + tolerance_above]
joint_const.tolerance_above = .2
joint_const.tolerance_below = .2
# A weighting factor for this constraint (denotes relative importance to other constraints. Closer to zero means less important)
joint_const.weight = .5
#for stage 2
# orien_const1 = OrientationConstraint()
# orien_const1.link_name = "right_gripper";
# orien_const1.header.frame_id = "ar_marker_1";
# #orien_const.orientation.y = np.pi/2
# #orien_const.orientation.w = 1;
# orien_const1.absolute_x_axis_tolerance = 0.5;
# orien_const1.absolute_y_axis_tolerance = 0.5;
# orien_const1.absolute_z_axis_tolerance = 0.5;
# orien_const1.weight = 1.0;
#rospy.sleep(1.0)
# while not rospy.is_shutdown():
# while not rospy.is_shutdown():
try:
#right_gripper_tip
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_1"
#x, y, and z position
goal_1.pose.position.x = -0.28
goal_1.pose.position.y = -0.05
#removing the 6th layer
goal_1.pose.position.z = .38 #0.0825
# z was .1
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 1
plan = planner.plan_to_pose_joint(goal_1, [joint_const])
if not planner.execute_plan(plan):
raise Exception("Execution failed0")
except Exception as e:
print e
def action(hand, m, n):
'''
Left hand or right hand
'''
left_gripper = robot_gripper.Gripper('left')
left_gripper.calibrate()
left_gripper.command_position(0)
p0x = 0.768
p0y = 0.220
p0z = -0.121
p1x = 0.378
p1y = -0.200
p1z = -0.121
arm = hand + "_arm"
planner = PathPlanner(arm)
if hand == "left":
tool = "marker"
else:
tool = "eraser"
init_px = p0x #0.795#
init_py = p0y
x_interval = (p1x - p0x) / 9 #-0.045
y_interval = (p1y - p0y) / 9 #-0.046
if hand == "left":
px = init_px + x_interval * m
py = init_py + y_interval * n
pz = -0.200
else:
px = init_px + (x_interval) * m - 0.02
py = init_py + (y_interval + 0.001) * n - 0.017
pz = -0.224
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
planner.remove_obstacle("low_table")
planner.remove_obstacle("high_table")
planner.remove_obstacle("front")
planner.remove_obstacle("leftmost")
planner.remove_obstacle("rightmost")
planner.remove_obstacle("eye")
def setObstacle(x, y, z, frame):
ob = PoseStamped()
ob.header.frame_id = frame
ob.pose.position.x = x
ob.pose.position.y = y
ob.pose.position.z = z
ob.pose.orientation.x = 0.0
ob.pose.orientation.y = 0.0
ob.pose.orientation.z = 0.0
ob.pose.orientation.w = 1.0
return ob
ob_low_table = setObstacle(px, py, pz - 0.018, "base")
planner.add_box_obstacle((1.50, 1.20, 0.01), "low_table", ob_low_table)
ob_leftmost = setObstacle(0.0, 1.1, 0.0, "base")
planner.add_box_obstacle((1.50, 0.01, 1.50), "leftmost", ob_leftmost)
ob_rightmost = setObstacle(0.0, -1.1, 0.0, "base")
planner.add_box_obstacle((1.50, 0.01, 1.50), "rightmost", ob_rightmost)
ob_front = setObstacle(1.2, 0.0, 0.0, "base")
planner.add_box_obstacle((0.01, 3.00, 1.50), "front", ob_front)
ob_back = setObstacle(-0.23, 0.0, 0.0, "base")
planner.add_box_obstacle((0.01, 3.00, 1.00), "back", ob_back)
def plan_move(x, y, z, px, py, pz, pw):
while not rospy.is_shutdown():
try:
goal = PoseStamped()
goal.header.frame_id = "base"
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
goal.pose.orientation.x = px
goal.pose.orientation.y = py
goal.pose.orientation.z = pz
goal.pose.orientation.w = pw
plan = planner.plan_to_pose(goal, list())
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
'''
Move to target
'''
print("move to target:{},{},{}".format(px, py, pz))
plan_move(px, py, pz, 0.0, -1.0, 0.0, 0.0)
'''
Rotate the hand with two circles and moves inward more and more
'''
shou = baxter_interface.Limb(hand)
joints = shou.joint_names()
joints_speed = dict()
for name in joints:
joints_speed[name] = 0
save_joint = hand + "_s1"
which_joint = hand + "_w2"
joints_speed[save_joint] = -0.018
if hand == "left":
round_num = 3
else:
round_num = 1
for i in range(round_num):
left_gripper.command_position(60 - 20 * i)
joints_speed[which_joint] = 4.6
start = time.time()
while time.time() - start < 1.8:
shou.set_joint_velocities(joints_speed)
print("{}: tick {}".format(tool, i))
joints_speed[which_joint] = -4.6
start = time.time()
while time.time() - start < 1.8:
shou.set_joint_velocities(joints_speed)
print("{}: tock {}".format(tool, i))
'''
Reset arms back to origin position
'''
print("Move to origin")
if hand == "left":
plan_move(0.300, 0.854, 0.116, 0.0, -1.0, 0.0, 0.0)
else:
plan_move(0.300, -0.854, 0.116, 0.0, -1.0, 0.0, 0.0)
class Actuator(object):
def __init__(self, sub_topic, pub_topic):
self.planner = None
self.limb = None
self.executed = False
self.planner = PathPlanner("right_arm")
self.limb = Limb("right")
# self.orien_const = OrientationConstraint()
# self.orien_const.link_name = "right_gripper"
# self.orien_const.header.frame_id = "base"
# self.orien_const.orientation.z = 0
# self.orien_const.orientation.y = 1
# self.orien_const.orientation.w = 0
# self.orien_const.absolute_x_axis_tolerance = 0.1
# self.orien_const.absolute_y_axis_tolerance = 0.1
# self.orien_const.absolute_z_axis_tolerance = 0.1
# self.orien_const.weight = 1.0
size = [1, 2, 2]
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = -1
obstacle_pose.pose.position.y = 0
obstacle_pose.pose.position.z = 0
obstacle_pose.pose.orientation.x = 0
obstacle_pose.pose.orientation.y = 0
obstacle_pose.pose.orientation.z = 0
obstacle_pose.pose.orientation.w = 1
self.planner.add_box_obstacle(size, "wall", obstacle_pose)
size = [.75, 1, 1]
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = 1
obstacle_pose.pose.position.y = 0
obstacle_pose.pose.position.z = -.4
obstacle_pose.pose.orientation.x = 0
obstacle_pose.pose.orientation.y = 0
obstacle_pose.pose.orientation.z = 0
obstacle_pose.pose.orientation.w = 1
self.planner.add_box_obstacle(size, "table", obstacle_pose)
self.pub = rospy.Publisher(pub_topic, Bool)
self.sub = rospy.Subscriber(sub_topic, PoseStamped, self.callback)
def callback(self, goal):
while not self.executed:
# if goal.pose.position.y <= 0:
# self.planner = PathPlanner("right_arm")
# self.limb = Limb("right")
# else:
# self.planner = PathPlanner("left_arm")
# self.limb = Limb("left")
try:
plan = self.planner.plan_to_pose(goal, [])
raw_input("Press <Enter> to execute plan")
if not self.planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
self.executed = True
print("Execution succeeded! Release the ball when ready!")
break
self.pub.publish(True)
def main():
"""
Main Script
"""
while not rospy.is_shutdown():
planner = PathPlanner("right_arm")
limb = Limb("right")
joint_angles = limb.joint_angles()
print(joint_angles)
camera_angle = {
'right_j6': 1.72249609375,
'right_j5': 0.31765625,
'right_j4': -0.069416015625,
'right_j3': 1.1111962890625,
'right_j2': 0.0664150390625,
'right_j1': -1.357775390625,
'right_j0': -0.0880478515625
}
limb.set_joint_positions(camera_angle)
limb.move_to_joint_positions(camera_angle,
timeout=15.0,
threshold=0.008726646,
test=None)
#drawing_angles = {'right_j6': -2.00561328125, 'right_j5': -1.9730205078125, 'right_j4': 1.5130146484375, 'right_j3': -1.0272568359375, 'right_j2': 1.24968359375, 'right_j1': -0.239498046875, 'right_j0': 0.4667275390625}
#print(joint_angles)
#drawing_angles = {'right_j6': -1.0133310546875, 'right_j5': -1.5432158203125, 'right_j4': 1.4599892578125, 'right_j3': -0.04361328125, 'right_j2': 1.6773486328125, 'right_j1': 0.019876953125, 'right_j0': 0.4214736328125}
drawing_angles = {
'right_j6': 1.9668515625,
'right_j5': 1.07505859375,
'right_j4': -0.2511611328125,
'right_j3': 0.782650390625,
'right_j2': 0.25496875,
'right_j1': -0.3268349609375,
'right_j0': 0.201146484375
}
raw_input("Press <Enter> to take picture: ")
waypoints_abstract = vision()
print(waypoints_abstract)
#ar coordinate :
x = 0.461067
y = -0.235305
#first get the solution of the maze
#solutionpoints = [(0,0),(-0.66,0.16), (-0.7, 0.4)]
# Make sure that you've looked at and understand path_planner.py before starting
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
r = rospy.Rate(10)
#find trans from
while not rospy.is_shutdown():
try:
trans = tfBuffer.lookup_transform('base', 'ar_marker_0',
rospy.Time()).transform
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
if tf2_ros.LookupException:
print("lookup")
elif tf2_ros.ConnectivityException:
print("connect")
elif tf2_ros.ExtrapolationException:
print("extra")
# print("not found")
pass
r.sleep()
mat = as_transformation_matrix(trans)
point_spaces = []
for point in waypoints_abstract:
# for point in solutionpoints:
point = np.array([point[0], point[1], 0, 1])
point_space = np.dot(mat, point)
point_spaces.append(point_space)
print(point_spaces)
length_of_points = len(point_spaces)
raw_input("Press <Enter> to move the right arm to drawing position: ")
limb.set_joint_positions(drawing_angles)
limb.move_to_joint_positions(drawing_angles,
timeout=15.0,
threshold=0.008726646,
test=None)
##
## Add the obstacle to the planning scene here
##
#add obstacle
size = [0.78, 1.0, 0.05]
name = "box"
pose = PoseStamped()
pose.header.frame_id = "base"
pose.pose.position.x = 0.77
pose.pose.position.y = 0.0
pose.pose.position.z = -0.18
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
planner.add_box_obstacle(size, name, pose)
#limb.set_joint_positions( drawing_angles)
#limb.move_to_joint_positions( drawing_angles, timeout=15.0, threshold=0.008726646, test=None)
#starting position
x, y, z = 0.67, 0.31, -0.107343
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
while not rospy.is_shutdown():
try:
waypoint = []
for point in point_spaces:
goal_1.pose.position.x = point[0]
goal_1.pose.position.y = point[1]
goal_1.pose.position.z = -0.113343 #set this value when put different marker
waypoint.append(copy.deepcopy(goal_1.pose))
plan, fraction = planner.plan_straight(waypoint, [])
print(fraction)
raw_input("Press <Enter> to move the right arm to draw: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
raw_input("Press <Enter> to start again: ")
cubes = message.pairs
# cubes = [cubes[0]]
# cubes[0].x = 630
# cubes[0].y = 442
cubes = process_cubes(cubes, tfBuffer, listener)
first_cube = cubes[0]
cube_size = np.array([0.02, 0.02, 0.02])
cube_pose = PoseStamped()
cube_pose.header.frame_id = "base"
cube_pose.pose.position.x = first_cube[0]
cube_pose.pose.position.z = -0.22 # for baxter
cube_pose.pose.orientation.w = 1.0
if(cnt > 0):
planner.remove_obstacle("cube")
planner.add_box_obstacle(cube_size, "cube", cube_pose)
cnt += 1
#print("found cubes", cubes)
table_height = -0.24
# cubes[0] = (0.41, -0.4, 100)
# print("first cube", cubes[0])
# cubes = [cubes[0]]
cube_path = get_cube_path_hue(cubes, table, table_height)
print("cube_path", cube_path)
manipulator_path = get_manipulator_path(cube_path, default_coords)
positions = [x.pose.position for x in manipulator_path]
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
# K values for Part 5
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3
]) # Borrowed from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Borrowed from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# Initialize the controller for Part 5
# controller = Controller( . . . )
#-----------------------------------------------------#
## Add any obstacles to the planning scene here
#-----------------------------------------------------#
posStamp1 = PoseStamped()
posStamp1.header.frame_id = "base"
posStamp1.pose.position.x = 0.9
posStamp1.pose.position.y = 0
posStamp1.pose.position.z = -0.3
posStamp1.pose.orientation.x = 0
posStamp1.pose.orientation.y = 0
posStamp1.pose.orientation.z = 0
posStamp1.pose.orientation.w = 1
planner.add_box_obstacle([0.913, 0.913, 0.04], "table1", posStamp1)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
def move_to_goal(x,
y,
z,
orien_const=[],
or_x=0.0,
or_y=-1.0,
or_z=0.0,
or_w=0.0):
while not rospy.is_shutdown():
try:
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = or_x
goal.pose.orientation.y = or_y
goal.pose.orientation.z = or_z
goal.pose.orientation.w = or_w
plan = planner.plan_to_pose(goal, orien_const)
raw_input("Press <Enter> to move the right arm to goal pose: ")
# Might have to edit this for part 5
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
while not rospy.is_shutdown():
# rospy.init_node('gripper_test')
move_to_goal(0.6, -0.3, 0.0)
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
right_gripper.calibrate()
rospy.sleep(2.0)
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
move_to_goal(0.4375, -0.10, -0.1)
move_to_goal(0.4375, -0.10, -0.14)
# move_to_goal(0.4225, -0.1265, -0.14)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
move_to_goal(0.6, -0.3, 0.0)
def main():
"""
Main Script
"""
# Setting up head camera
head_camera = CameraController("left_hand_camera")
head_camera.resolution = (1280, 800)
head_camera.open()
print("setting balance")
happy = False
while not happy:
e = head_camera.exposure
print("exposue value: " + str(e))
e_val = int(input("new exposure value"))
head_camera.exposure = e_val
satisfaction = str(raw_input("satified? y/n"))
happy = 'y' == satisfaction
planner = PathPlanner("right_arm")
listener = tf.TransformListener()
grip = Gripper('right')
grip.calibrate()
rospy.sleep(3)
grip.open()
# creating the table obstacle so that Baxter doesn't hit it
table_size = np.array([.5, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
thickness = 1
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -.112 - thickness / 2
table_size = np.array([.5, 1, thickness])
planner.add_box_obstacle(table_size, "table", table_pose)
raw_input("gripper close")
grip.close()
# ###load cup positions found using cup_detection.py ran in virtual environment
# start_pos_xy = np.load(POURING_CUP_PATH)
# goal_pos_xy = np.load(RECEIVING_CUP_PATH)
# start_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# goal_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# #### END LOADING CUP POSITIONS
camera_subscriber = rospy.Subscriber("cameras/left_hand_camera/image", Image, get_img)
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
cam_pos= [0.188, -0.012, 0.750]
##
## Add the obstacle to the planning scene here
##
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
horizontal = getQuaternion(np.array([0,1,0]), np.pi)
z_rot_pos = getQuaternion(np.array([0,0,1]), np.pi / 2)
orig = quatMult(z_rot_pos, horizontal)
orig = getQuaternion(np.array([0,1,0]), np.pi / 2)
#IN THE VIEW OF THE CAMERA
#CORNER1--------->CORNER2
# | |
# | |
# | |
#CORNER3 ------------|
width = 0.3
length = 0.6
CORNER1 = np.array([0.799, -0.524, -0.03])
CORNER2 = CORNER1 + np.array([-width, 0, 0])
CORNER3 = CORNER1 + np.array([0, length, 0])
#CREATE THE GRID
dir1 = CORNER2 - CORNER1
dir2 = CORNER3 - CORNER1
grid_vals = []
ret_vals = []
for i in range(0, 3):
for j in range(0, 4):
grid = CORNER1 + i * dir1 / 2 + j * dir2 / 3
grid_vals.append(grid)
ret_vals.append(np.array([grid[0], grid[1], OBJECT_HEIGHT]))
i = -1
while not rospy.is_shutdown():
for g in grid_vals:
i += 1
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = g[0]
goal_1.pose.position.y = g[1]
goal_1.pose.position.z = g[2]
y = - g[1] + cam_pos[1]
x = g[0] - cam_pos[0]
q = orig
#Orientation as a quaternion
goal_1.pose.orientation.x = q[1][0]
goal_1.pose.orientation.y = q[1][1]
goal_1.pose.orientation.z = q[1][2]
goal_1.pose.orientation.w = q[0]
plan = planner.plan_to_pose(goal_1, [])
if planner.execute_plan(plan):
# raise Exception("Execution failed")
rospy.sleep(1)
#grip.open()
raw_input("open")
rospy.sleep(1)
# plt.imshow(camera_image)
print("yay: " + str(i))
pos = listener.lookupTransform("/reference/right_gripper", "/reference/base", rospy.Time(0))[0]
print("move succesfully to " + str(pos))
fname = os.path.join(IMAGE_DIR, "calib_{}.jpg".format(i))
skimage.io.imsave(fname, camera_image)
print(e)
print("index: ", i)
else:
break
print(np.array(ret_vals))
# save the positions of the gripper so that the homography matrix can be calculated
np.save(POINTS_DIR, np.array(ret_vals))
print(np.load(POINTS_DIR))
break
def play_song(note_pos, note_names):
"""
Main Script
"""
planner = PathPlanner("right_arm")
if add_obstacle:
table_offset = 0.05
size = np.array([1, 1, (table_offset - .02) * 2.0])
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = 0.801
obstacle_pose.pose.position.y = 0.073
obstacle_pose.pose.position.z = -0.214 - table_offset #remember to calibrate based on ar marker
planner.add_box_obstacle(size, "table", obstacle_pose)
new_note_pos = []
num_waypoints_per_note = 4
# add waypoints above the note to ensure the note is struck properly
for note in note_pos:
waypoint1 = Vector3()
waypoint1.x = note.x
waypoint1.y = note.y
waypoint1.z = note.z + waypoint_offset
waypoint2 = Vector3()
waypoint2.x = note.x
waypoint2.y = note.y
waypoint2.z = note.z + waypoint_offset / 2.0
new_note_pos += [waypoint1, waypoint2, note, waypoint1]
# Iterate through all note positions for the song
for i, pos in enumerate(new_note_pos):
print(i, pos)
if i % num_waypoints_per_note == 0:
display_img(note_names[i // num_waypoints_per_note])
# Loop until that position is reached
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = pos.x
goal_1.pose.position.y = pos.y
goal_1.pose.position.z = pos.z
# #Orientation as a quaternion, facing straight down
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_1, list())
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main(cube_pose, end_pose):
planner = PathPlanner("right_arm")
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 10000;
orien_const.weight = 1.0;
# allow only 1/2 pi rotation so as to not lose the cube during transit
orien_path_const = OrientationConstraint()
orien_path_const.link_name = "right_gripper";
orien_path_const.header.frame_id = "base";
orien_path_const.orientation.y = -1.0;
orien_path_const.absolute_x_axis_tolerance = 0.1;
orien_path_const.absolute_y_axis_tolerance = 0.1;
orien_path_const.absolute_z_axis_tolerance = 1.57;
orien_path_const.weight = 1.0;
# quat = QuaternionStamped()
# quat.header.frame_id = "base"
# quat.quaternion.w = 1.0
start_pose = get_start(cube_pose, end_pose)
end_pose.pose.orientation = start_pose.pose.orientation
table_size = np.array([0.6, 1.2, 0.03])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
#x, y, and z position
table_pose.pose.position.x = 0.5
if ROBOT == "baxter":
table_pose.pose.position.z = -0.23 # for baxter
else:
table_pose.pose.position.z = -0.33
#Orientation as a quaternion
table_pose.pose.orientation.w = 1.0
planner.add_box_obstacle(table_size, "table", table_pose)
# cube_size = np.array([0.05, 0.05, 0.15])
default_state = get_pose(0.6, -0.5, -0.17, 0.0, -1.0, 0.0, 0.0)
# default_state = get_pose(0.94, 0.2, 0.5, 0.0, -1.0, 0.0, 0.0)
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(default_state, [])
print(type(plan))
raw_input("Press <Enter> to move the right arm to default pose: ")
if not planner.execute_plan(plan):
raise Exception("(ours) Execution failed")
except Exception as e:
print("caught!")
print e
else:
break
# planner.add_box_obstacle(cube_size, "cube", cube_pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(start_pose, [orien_const])
raw_input("Press <Enter> to move the right arm to cube: ")
if not planner.execute_plan(plan):
raise Exception("(ours) Execution failed")
except Exception as e:
print("caught!")
print e
else:
break
orien_path_const.orientation = start_pose.pose.orientation
# planner.remove_obstacle("cube")
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(end_pose, [orien_path_const])
raw_input("Press <Enter> to move the right arm to end position: ")
if not planner.execute_plan(plan):
raise Exception("(ours) Execution failed")
except Exception as e:
print("caught!")
print e
else:
break
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
controller = Controller(Kp, Kd, Ki, Kw, Limb('right'))
##
## Add the obstacle to the planning scene here
bagel = PoseStamped()
bagel.header.frame_id = 'base'
#x, y, and z position
bagel.pose.position.x = 0.5
bagel.pose.position.y = 0.0
bagel.pose.position.z = 0.0
#Orientation as a quaternion
bagel.pose.orientation.x = 0.0
bagel.pose.orientation.y = 0.0
bagel.pose.orientation.z = 0.0
bagel.pose.orientation.w = 1.0
planner.add_box_obstacle(np.array([0.4, 1.2, 0.1]), 'bagel', bagel)
#Create a path constraint for the arm
#UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.4
goal_1.pose.position.y = -0.3
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_1, list())
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
if not controller.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_2, list())
raw_input("Press <Enter> to move the right arm to goal pose 2: ")
if not controller.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_3, list())
raw_input("Press <Enter> to move the right arm to goal pose 3: ")
if not controller.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
class Mover:
def __init__(self, box_in):
self.box = box_in
self.conveyor_z = -0.038
self.mover_setup()
def mover_setup(self):
limb = Limb("right")
# Right arm planner
self.planner = PathPlanner("right_arm")
# Left arm planner
self.planner_left = PathPlanner("left_arm")
place_holder = {'images': [], 'camera_infos': []}
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
# Controller for right arm
self.c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
self.orien_const = OrientationConstraint()
self.orien_const.link_name = "right_gripper";
self.orien_const.header.frame_id = "base";
self.orien_const.orientation.y = -1.0;
self.orien_const.absolute_x_axis_tolerance = 0.1;
self.orien_const.absolute_y_axis_tolerance = 0.1;
self.orien_const.absolute_z_axis_tolerance = 0.1;
self.orien_const.weight = 1.0;
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = self.box.pose.position.x
box.pose.position.y = self.box.pose.position.y
# box.pose.position.z = self.box.pose.position.z
box.pose.position.z = self.conveyor_z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
self.planner.add_box_obstacle((100, 100, 0.00001), "box", box)
# Controller for left arm
self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left'))
def move(self, x, y, z):
try:
print("Moving\n\n\n")
print(x, y, z)
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = 0.0
goal.pose.orientation.y = -1.0
goal.pose.orientation.z = 0.0
goal.pose.orientation.w = 0.0
plan = self.planner.plan_to_pose(goal, [self.orien_const])
# TODO: Remove the raw_input for the demo.
# x = raw_input("Move?")
if not self.c.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print(e)
traceback.print_exc()
def delayed_move(self, x, y, z, delay, time1):
try:
print("Delayed Move\n\n")
print(x, y, z)
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = 0.0
goal.pose.orientation.y = -1.0
goal.pose.orientation.z = 0.0
goal.pose.orientation.w = 0.0
plan = self.planner.plan_to_pose(goal, [self.orien_const])
# Wait till delay has elapsed.
while rospy.Time.now().to_sec() < time1 + delay:
pass
if not self.c.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print(e)
traceback.print_exc()
# Move camera to set position (optional if cannot find a good intermediary pose)
def move_camera(self):
try:
goal = PoseStamped()
goal.header.frame_id = "base"
# TODO: Update position and orientation values
#x, y, and z position
goal.pose.position.x = .655
goal.pose.position.y = .21
goal.pose.position.z = .25
#Orientation as a quaternion
goal.pose.orientation.x = -.7
goal.pose.orientation.y = .71
goal.pose.orientation.z = -.03
goal.pose.orientation.w = -.02
plan = self.planner.plan_to_pose(goal)
# TODO: Remove the raw_input for the demo.
x = raw_input("Move?")
if not self.c.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print(e)
traceback.print_exc()
def main():
"""
Main Script
"""
use_orien_const = False
input = raw_input("use orientation constraint? y/n\n")
if input == 'y' or input == 'yes':
use_orien_const = True
print("using orientation constraint")
elif input == 'n' or input == 'no':
use_orien_const = False
print("not using orientation constraint")
else:
print("input error, not using orientation constraint as default")
add_box = False
input = raw_input("add_box? y/n\n")
if input == 'y' or input == 'yes':
add_box = True
print("add_box")
elif input == 'n' or input == 'no':
add_box = False
print("not add_box")
else:
print("input error, not add_box as default")
open_loop_contro = False
input = raw_input("using open loop controller? y/n\n")
if input == 'y' or input == 'yes':
open_loop_contro = True
print("using open loop controller")
elif input == 'n' or input == 'no':
open_loop_contro = False
print("not using open loop controller")
else:
print("input error, not using open loop controller as default")
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# joint_names = ['head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
my_controller = Controller(Kp, Kd, Ki, Kw, Limb("right"))
##
## Add the obstacle to the planning scene here
##
if add_box:
name = "obstacle_1"
size = np.array([0.4, 1.2, 0.1])
pose = PoseStamped()
pose.header.frame_id = "base"
#x, y, and z position
pose.pose.position.x = 0.5
pose.pose.position.y = 0.0
pose.pose.position.z = 0.0
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
planner.add_box_obstacle(size, name, pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.4
goal_1.pose.position.y = -0.3
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
if not use_orien_const:
plan = planner.plan_to_pose(goal_1, list())
else:
plan = planner.plan_to_pose(goal_1, [orien_const])
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
if open_loop_contro:
if not my_controller.execute_path(plan):
raise Exception("Execution failed")
else:
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
# plan = planner.plan_to_pose(goal_2, list())
if not use_orien_const:
plan = planner.plan_to_pose(goal_2, list())
else:
plan = planner.plan_to_pose(goal_2, [orien_const])
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
if open_loop_contro:
if not my_controller.execute_path(plan):
raise Exception("Execution failed")
else:
if not planner.execute_plan(plan):
raise Exception("Execution failed")
# if not my_controller.execute_path(plan):
# raise Exception("Execution failed")
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
# plan = planner.plan_to_pose(goal_3, list())
if not use_orien_const:
plan = planner.plan_to_pose(goal_3, list())
else:
plan = planner.plan_to_pose(goal_3, [orien_const])
raw_input(
"Press <Enter> to move the right arm to goal pose 3: ")
if open_loop_contro:
if not my_controller.execute_path(plan):
raise Exception("Execution failed")
else:
if not planner.execute_plan(plan):
raise Exception("Execution failed")
# if not my_controller.execute_path(plan):
# raise Exception("Execution failed")
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
# K values for Part 5
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3
]) # Borrowed from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Borrowed from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# Initialize the controller for Part 5
# controller = Controller( . . . )
#-----------------------------------------------------#
## Add any obstacles to the planning scene here
#-----------------------------------------------------#
size = np.array([0.4, 1.2, 0.1])
pose = PoseStamped()
pose.header.frame_id = "base"
pose.pose.position.x = 0.5
pose.pose.position.y = 0.0
pose.pose.position.z = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.z = 1.0
planner.add_box_obstacle(size, "box", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
def move_to_goal(x,
y,
z,
orien_const=[],
or_x=0.0,
or_y=-1.0,
or_z=0.0,
or_w=0.0):
while not rospy.is_shutdown():
try:
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = or_x
goal.pose.orientation.y = or_y
goal.pose.orientation.z = or_z
goal.pose.orientation.w = or_w
plan = planner.plan_to_pose(goal, orien_const)
print(plan.joint_trajectory.points[-1].positions)
raw_input("Press <Enter> to move the right arm to goal pose: ")
# # Might have to edit this for part 5
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
while not rospy.is_shutdown():
# Set your goal positions here
orient_const = OrientationConstraint()
# std_msgs/Header header
orient_const.orientation.x = 1.0
orient_const.orientation.y = 0.0
orient_const.orientation.z = 0.0
orient_const.orientation.w = 0.0
orient_const.header.seq = 0.0
orient_const.header.stamp = rospy.Time.now()
orient_const.header.frame_id = "right_gripper"
orient_const.link_name = "right_gripper"
orient_const.absolute_x_axis_tolerance = 0.05
orient_const.absolute_y_axis_tolerance = 0.05
orient_const.absolute_z_axis_tolerance = 0.05
orient_const.weight
move_to_goal(0.85, -0.3, 0.1, orien_const=[orient_const])
print("Done 1")
def main(list_of_class_objs, basket_coordinates, planner_left):
"""
Main Script
input:first argument: a list of classX_objs. Ex: [class1_objs, class2_objs] where class1_objs contains the same kind of fruits
second argument: a list of baskets coordinates
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner_left = PathPlanner("left_arm")
home_coordinates = [0.544, 0.306, 0.3]
home = PoseStamped()
home.header.frame_id = "base"
#home x,y,z position
home_x = home_coordinates[0]
home_y = home_coordinates[1]
home_z = home_coordinates[2]
home.pose.position.x = home_x
home.pose.position.y = home_y
home.pose.position.z = home_z
#Orientation as a quaternion
home.pose.orientation.x = 1.0
home.pose.orientation.y = 0.0
home.pose.orientation.z = 0.0
home.pose.orientation.w = 0.0
intermediate_obstacle = PoseStamped()
intermediate_obstacle.header.frame_id = "base"
intermediate_obstacle.pose.position.x = 0
intermediate_obstacle.pose.position.y = 0
intermediate_obstacle.pose.position.z = 0.764
intermediate_obstacle.pose.orientation.x = 1
intermediate_obstacle.pose.orientation.y = 0
intermediate_obstacle.pose.orientation.z = 0
intermediate_obstacle.pose.orientation.w = 0
intermediate_size = [1, 1, 0.2]
left_gripper = robot_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
while not rospy.is_shutdown():
try:
#GO HOME
plan = planner_left.plan_to_pose(home, list())
raw_input("Press <Enter> to move the left arm to home position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
for i, classi_objs in enumerate(list_of_class_objs):
#x, y,z, orientation of class(basket)
print("processing class: " + str(i))
classi_position = basket_coordinates[i]
classi = PoseStamped()
classi.header.frame_id = "base"
classi_x = classi_position[0]
classi_y = classi_position[1]
classi_z = classi_position[2]
classi.pose.position.x = classi_x
classi.pose.position.y = classi_y
classi.pose.position.z = classi_z +.1
classi.pose.orientation.x = 1.0
classi.pose.orientation.y = 0.0
classi.pose.orientation.z = 0.0
classi.pose.orientation.w = 0.0
for fruit in classi_objs:
gripper_yaw = fruit[3]
fruit_x = fruit[0]
fruit_y = fruit[1]
fruit_z = fruit[2]
gripper_orientation = euler_to_quaternion(gripper_yaw)
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
gripper_orientation_x = gripper_orientation[0]
gripper_orientation_y = gripper_orientation[1]
gripper_orientation_z = gripper_orientation[2]
gripper_orientation_w = gripper_orientation[3]
orien_const.orientation.x = gripper_orientation_x
orien_const.orientation.y = gripper_orientation_y
orien_const.orientation.z = gripper_orientation_z
orien_const.orientation.w = gripper_orientation_w
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose)
#intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper
intermidiate_to_fruit = PoseStamped()
intermidiate_to_fruit.header.frame_id = "base"
#x,y,z position
intermidiate_to_fruit.pose.position.x = fruit_x
intermidiate_to_fruit.pose.position.y = fruit_y
intermidiate_to_fruit.pose.position.z = home_z - .1
print("Trying to reach intermeidiate_to_fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z))
intermidiate_to_fruit.pose.orientation.x = gripper_orientation_x
intermidiate_to_fruit.pose.orientation.y = gripper_orientation_y
intermidiate_to_fruit.pose.orientation.z = gripper_orientation_z
intermidiate_to_fruit.pose.orientation.w = gripper_orientation_w
plan = planner_left.plan_to_pose(intermidiate_to_fruit, list())
raw_input("Press <Enter> to move the left arm to intermidiate_to_fruit position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
planner_left.remove_obstacle("intermediate")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
#go down to the actual height of the fruit and close gripper
fruitobs = generate_obstacle(fruit_x, fruit_y)
fruit = PoseStamped()
fruit.header.frame_id = "base"
#x,y,z position
fruit.pose.position.x = fruit_x
fruit.pose.position.y = fruit_y
fruit.pose.position.z = fruit_z
print("Trying to reach fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z))
#Orientation as a quaternion
fruit.pose.orientation.x = gripper_orientation_x
fruit.pose.orientation.y = gripper_orientation_y
fruit.pose.orientation.z = gripper_orientation_z
fruit.pose.orientation.w = gripper_orientation_w
plan = planner_left.plan_to_pose(fruit, [orien_const])
raw_input("Press <Enter> to move the left arm to fruit position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
fruitobs()
except Exception as e:
print e
else:
break
#close the gripper
print('Closing...')
left_gripper.close()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
fruitobs = generate_obstacle(fruit_x, fruit_y)
#intermidiate_to_basket stage1: Lift up to a height higher than the height of the basket
firt_intermidiate_to_class_i = PoseStamped()
firt_intermidiate_to_class_i.header.frame_id = "base"
#x, y, and z position
firt_intermidiate_to_class_i.pose.position.x = fruit_x
firt_intermidiate_to_class_i.pose.position.y = fruit_y
firt_intermidiate_to_class_i.pose.position.z = classi_z + 0.25
print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2))
#Orientation as a quaternion
firt_intermidiate_to_class_i.pose.orientation.x = 1.0
firt_intermidiate_to_class_i.pose.orientation.y = 0.0
firt_intermidiate_to_class_i.pose.orientation.z = 0.0
firt_intermidiate_to_class_i.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(firt_intermidiate_to_class_i, list())
raw_input("Press <Enter> to move the left arm to first_intermidiate_to_class_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
fruitobs()
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose)
#intermidiate_to_basket stage2: Move to the top of the basket
intermidiate_to_class_i = PoseStamped()
intermidiate_to_class_i.header.frame_id = "base"
#x, y, and z position
intermidiate_to_class_i.pose.position.x = classi_x
intermidiate_to_class_i.pose.position.y = classi_y
intermidiate_to_class_i.pose.position.z = classi_z + 0.2
print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2))
#Orientation as a quaternion
intermidiate_to_class_i.pose.orientation.x = 1.0
intermidiate_to_class_i.pose.orientation.y = 0.0
intermidiate_to_class_i.pose.orientation.z = 0.0
intermidiate_to_class_i.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(intermidiate_to_class_i, list())
raw_input("Press <Enter> to move the left arm to second_intermidiate_to_class_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
planner_left.remove_obstacle("intermediate")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
#basket stage: put the fruit in the basket
classi_obs = generate_obstacle(classi_x, class_y)
plan = planner_left.plan_to_pose(classi, list())
raw_input("Press <Enter> to move the left arm to sclass_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
classi_obs()
except Exception as e:
print e
else:
break
#Open the gripper
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
#intermidiate to home stage: lifting up to the home position height
intermidiate_to_home_1 = PoseStamped()
intermidiate_to_home_1.header.frame_id = "base"
#x, y, and z position
intermidiate_to_home_1.pose.position.x = classi_x
intermidiate_to_home_1.pose.position.y = classi_y
intermidiate_to_home_1.pose.position.z = home_z
#Orientation as a quaternion
intermidiate_to_home_1.pose.orientation.x = 1.0
intermidiate_to_home_1.pose.orientation.y = 0.0
intermidiate_to_home_1.pose.orientation.z = 0.0
intermidiate_to_home_1.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(intermidiate_to_home_1, list())
raw_input("Press <Enter> to move the right arm to intermidiate_to_home_1 position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
planner_left.shutdown()
def main():
planner = PathPlanner("left_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# #Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
filename = "test_cup.jpg"
table_height = 0
# get positions of cups
cups = find_cups(filename)
start_cup = cups[0]
goal_cup = cups[1]
cup_pos = [0.756, 0.300, -0.162] # position of cup
goal_pos = [0.756, 0.600, -0.162] # position of goal cup
init_pos = [cup_pos[0] - .2, cup_pos[1],
cup_pos[2] + .05] # initial position
start_pos = [init_pos[0] + .2, init_pos[1],
init_pos[2]] # start position, pick up cup
up_pos = [goal_pos[0], goal_pos[1] - .05,
goal_pos[2] + .1] # up position, ready to tilt
end_pos = start_pos # end position, put down cup
final_pos = init_pos # final position, away from cup
# add table obstacle
table_size = np.array([.4, .8, .1])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
#x, y, and z position
table_pose.pose.position.x = 0.5
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = 0.0
planner.add_box_obstacle(table_size, 'table', table_pose)
# add goal cup obstacle
goal_cup_size = np.array([.1, .1, .2])
goal_cup_pose = PoseStamped()
goal_cup_pose.header.frame_id = "base"
#x, y, and z position
goal_cup_pose.pose.position.x = goal_pos[0]
goal_cup_pose.pose.position.y = goal_pos[1]
goal_cup_pose.pose.position.z = goal_pos[2]
planner.add_box_obstacle(goal_cup_size, 'goal cup', goal_cup_pose)
# add first cup obstacle
cup_size = np.array([.1, .1, .2])
cup_pose = PoseStamped()
cup_pose.header.frame_id = "base"
#x, y, and z position
cup_pose.pose.position.x = cup_pos[0]
cup_pose.pose.position.y = cup_pos[1]
cup_pose.pose.position.z = cup_pos[2]
planner.add_box_obstacle(cup_size, 'cup', cup_pose)
while not rospy.is_shutdown():
left_gripper = robot_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
pose = PoseStamped()
pose.header.frame_id = "base"
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = -1.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = init_pos[0]
pose.pose.position.y = init_pos[1]
pose.pose.position.z = init_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to initial pose: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# grab cup
# need to make sure it doesn't knock it over
planner.remove_obstacle('cup')
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = start_pos[0]
pose.pose.position.y = start_pos[1]
pose.pose.position.z = start_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to grab the cup: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
print('Closing...')
left_gripper.close()
rospy.sleep(2)
except Exception as e:
print e
else:
break
# position to pour
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = up_pos[0]
pose.pose.position.y = up_pos[1]
pose.pose.position.z = up_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to above the goal cup: "
)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# pouring
raw_input("Press <Enter> to move the left arm to begin pouring: ")
for degree in range(180):
while not rospy.is_shutdown():
try:
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = -1.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
plan = planner.plan_to_pose(pose, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# move cup away from goal on table
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = end_pos[0]
pose.pose.position.y = end_pos[1]
pose.pose.position.z = end_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to away from the goal cup: "
)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# let go of cup on table
# need to make sure to not to hit cup
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = final_pos[0]
pose.pose.position.y = final_pos[1]
pose.pose.position.z = final_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to let go of the cup: "
)
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# get new cup position
new_cup_pos = []
# readd the cup obstacle
cup_pose.pose.position.x = new_cup_pos[0]
cup_pose.pose.position.y = new_cup_pos[1]
cup_pose.pose.position.z = new_cup_pos[2]
planner.add_box_obstacle(cup_size, 'cup', cup_pose)
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
plannerR = PathPlanner("right_arm")
plannerL = PathPlanner("left_arm")
right_gripper = robot_gripper.Gripper('right')
left_gripper = robot_gripper.Gripper('left')
# Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
# Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
# Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
# Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# right_arm = Limb("right")
# controller = Controller(Kp, Kd, Ki, Kw, right_arm)
##
## Add the obstacle to the planning scene here
##
obstacle1 = PoseStamped()
obstacle1.header.frame_id = "base"
#x, y, and z position
obstacle1.pose.position.x = 0.4
obstacle1.pose.position.y = 0
obstacle1.pose.position.z = -0.29
#Orientation as a quaternion
obstacle1.pose.orientation.x = 0.0
obstacle1.pose.orientation.y = 0.0
obstacle1.pose.orientation.z = 0.0
obstacle1.pose.orientation.w = 1
plannerR.add_box_obstacle(np.array([1.2, 1.2, .005]), "tableBox",
obstacle1)
# obstacle2 = PoseStamped()
# obstacle2.header.frame_id = "wall"
# #x, y, and z position
# obstacle2.pose.position.x = 0.466
# obstacle2.pose.position.y = -0.670
# obstacle2.pose.position.z = -0.005
# #Orientation as a quaternion
# obstacle2.pose.orientation.x = 0.694
# obstacle2.pose.orientation.y = -0.669
# obstacle2.pose.orientation.z = 0.251
# obstacle2.pose.orientation.w = -0.092
# planner.add_box_obstacle(np.array([0.01, 2, 2]), "wall", obstacle2)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.440
goal_3.pose.position.y = -0.012
goal_3.pose.position.z = 0.549
#Orientation as a quaternion
goal_3.pose.orientation.x = -0.314
goal_3.pose.orientation.y = -0.389
goal_3.pose.orientation.z = 0.432
goal_3.pose.orientation.w = 0.749
#plan = plannerR.plan_to_pose(goal_3, list())
raw_input(
"Press <Enter> to move the right arm to goal pose 3: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_3, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.448
goal_2.pose.position.y = -0.047
goal_2.pose.position.z = -0.245
#Orientation as a quaternion
goal_2.pose.orientation.x = 0
goal_2.pose.orientation.y = 1
goal_2.pose.orientation.z = 0
goal_2.pose.orientation.w = 0
#plan = plannerR.plan_to_pose(goal_2, list())
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_2, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.448
goal_1.pose.position.y = -0.047
goal_1.pose.position.z = -0.245
#Orientation as a quaternion
goal_1.pose.orientation.x = 1
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0
# Might have to edit this . . .
#plan = plannerR.plan_to_pose(goal_1, list()) # put orien_const here
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_1, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
"""
Main Script
"""
#===================================================
# Code to add gripper
# rospy.init_node('gripper_test')
# # Set up the right gripper
# right_gripper = robot_gripper.Gripper('right_gripper')
# #Calibrate the gripper (other commands won't work unless you do this first)
# print('Calibrating...')
# right_gripper.calibrate()
# rospy.sleep(2.0)
# #Close the right gripper to hold publisher
# # MIGHT NOT NEED THIS
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
## TF CODE
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
## TF CODE
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
#Tower
X = 0.075
Y = 0.075
Z = 0.0675
Xp = 0.0
Yp = -0.0425
Zp = 0.0225
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "ar_marker_1"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X,Y,Z], "tower", pose)
#-------------------------------walls
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = -2
pose.pose.position.y = 0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1,1,5], "left_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = -2
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1,1,5], "right_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = 2
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1,1,5], "back_wall", pose)
#-------------------------------walls
#Table
X = .2
Y = .2
Z = .01
Xp = 0
Yp = 0
Zp = 0
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
#TODO: Is this the correct frame name?
pose.header.frame_id = "ar_marker_1"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X,Y,Z], "table", pose)
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "ar_marker_1";
#orien_const.orientation.y = np.pi/2
#orien_const.orientation.w = 1;
orien_const.absolute_x_axis_tolerance = 1;
orien_const.absolute_y_axis_tolerance = 1;
orien_const.absolute_z_axis_tolerance = 1;
orien_const.weight = .5;
#for stage 2
# orien_const1 = OrientationConstraint()
# orien_const1.link_name = "right_gripper";
# orien_const1.header.frame_id = "ar_marker_1";
# #orien_const.orientation.y = np.pi/2
# #orien_const.orientation.w = 1;
# orien_const1.absolute_x_axis_tolerance = 0.5;
# orien_const1.absolute_y_axis_tolerance = 0.5;
# orien_const1.absolute_z_axis_tolerance = 0.5;
# orien_const1.weight = 1.0;
#rospy.sleep(1.0)
# while not rospy.is_shutdown():
# while not rospy.is_shutdown():
current_x = 0
current_y = 0
current_z = 0
offset = 0.14
i = 0
while(i == 0):
try:
trans = tfBuffer.lookup_transform('ar_marker_1', 'right_gripper_tip', rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
current_x = tf_px + offset
current_y = tf_py
current_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
continue
i =1
x = current_x
y = current_y
z = current_z
try:
#right_gripper_tip
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_1"
#x, y, and z position
goal_1.pose.position.x = -0.0175
goal_1.pose.position.y = -0.0018
goal_1.pose.position.z = 0.065 #0.0825
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = 0.707
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0.707
plan = planner.plan_to_pose(goal_1, list())#[orien_const])
if not planner.execute_plan(plan):
raise Exception("Execution failed0")
except Exception as e:
print e
