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 __init__(self, gsl, sil, vil):
self.LOG.info('init')
self._sil = sil
self.tc = ThrottleControl(gsl)
self.sc = SteeringControl(gsl)
self.tlc = TopLevelControl(gsl, sil, self.tc, self.sc)
self.pp = PathPlanner(gsl, sil, vil, self.tc, self.sc)
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():
get_top_left_AR_tag()
# print("Here are the marker rotations: " + str(marker_rotations))
# print("Here are the marker translations: " + str(marker_translations))
global top_left_x
global top_left_y
global top_left_z
assert len(marker_translations) == 1
R = marker_rotations[0]
t = marker_translations[0]
top_left_x, top_left_y, top_left_z = t
sawyer = Robot(PathPlanner("right_arm"), None, "right_gripper", 0.095)
# Adding environment obstacles for the robot to avoid during path planning
# add_block_obstacle(sawyer, position=[0.67452, -0.4455, 0.916557], name='right_wall', size=[5, 0, 5], orient=[1, 0, 0, 0])
add_block_obstacle(sawyer,
position=[0.460625, 0.5444, 0.916557],
name='left_wall',
size=[5, 0, 5],
orient=[1, 0, 0, 0])
# add_block_obstacle(sawyer, position=[, , TABLE_HEIGHT - sawyer.gripper_length], name='table', size=[3.5, 3.5, 0], orient=[0, 1, 0, 0])
# add_block_obstacle(planner, [0,0,0], 'ceiling', [10, 5, 1])
add_block_obstacle(
sawyer,
position=[0.81, -0.26, -0.1 + (0.5 * BLOCK_TOTAL_HEIGHT)],
name='baseplate',
size=[
5 * BLOCK_SIDE_LENGTH, 6 * BLOCK_SIDE_LENGTH,
0.5 * BLOCK_TOTAL_HEIGHT
])
add_block_obstacle(sawyer,
position=[0.81, -0.26, -0.1 - (BLOCK_TOTAL_HEIGHT / 2)],
name='table',
size=[5, 5, BLOCK_TOTAL_HEIGHT / 2])
#TODO: Finish adding as many environment constraints as possible without compromising on path planning.
dest_x, dest_y = block_coord_to_sawyer((4, 5))
print('Top left:', top_left_x, top_left_y)
start_x3 = top_left_x - 0.08 - (BLOCK_SIDE_LENGTH / 2.0)
start_y3 = top_left_y + 0.0335
print(start_x3, start_y3)
start_x3, start_y3 = start_num_to_sawyer(3)
print(start_x3, start_y3)
print()
for i in range(8):
print(start_num_to_sawyer(i))
while 1 < 2:
pick_and_place_block(sawyer, start_x3, start_y3, -0.0320774, dest_x,
dest_y, top_left_z + (0.5 * BLOCK_SQUARE_HEIGHT),
'block_test')
def execute_rendezvous(
self, master_coords, drone_coords
): # calculates rendezvous path and sets nav goal for robots at the provided poses
if self.executing_rendezvous or not self.map_initialized:
return False
# init and run the path planner
path_planner = PathPlanner(self.occupancy_grid, self.map_width,
self.map_height, self.map_resolution,
self.map_origin)
path_planner.set_start(master_coords)
path_planner.set_goal(drone_coords)
rendezvous_path = path_planner.a_star()
# abort if no path found
if len(rendezvous_path) == 0:
return False
# parse the result and start nav to goal for both robots
grid_goal = rendezvous_path[int(math.floor(len(rendezvous_path) / 2))]
world_goal = path_planner.grid_to_world(grid_goal)
print 'executing rendezvous at ' + str(world_goal)
self.master_node.start_navigation(
world_goal[0], world_goal[1], 0,
(master_coords[0], master_coords[1], 0))
self.drone_node.start_navigation(world_goal[0], world_goal[1], 0,
(drone_coords[0], drone_coords[1], 0))
self.executing_rendezvous = True
return True
def configureLeftHand():
planner = PathPlanner("left_arm")
position = np.array([0.516, 0.485, 0.362])
orientation = [0.647, 0.655, -0.281, 0.271]
executePositions(planner, [position], [orientation])
def grasp(key,
x,
y,
z,
x_threshhold=0.02,
y_threshhold=0.02,
hoverDist=0.020,
zoffset=.92,
orien_const=[],
or_x=0.0,
or_y=-1.0,
or_z=0.0,
or_w=0.0):
#while not rospy.is_shutdown():
try:
left_arm_planner = PathPlanner("left_arm")
right_arm_planner = PathPlanner("right_arm")
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x + x_threshhold
goal.pose.position.y = y + y_threshhold
goal.pose.position.z = z - zoffset + hoverDist
#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
if key == 'left_arm':
plan = left_arm_planner.plan_to_pose(goal, orien_const)
openLeftGripper()
left_arm_planner.execute_plan(plan)
closeLeftGripper()
else:
plan = right_arm_planner.plan_to_pose(goal, orien_const)
openRightGripper()
right_arm_planner.execute_plan(plan)
closeRightGripper()
except Exception as e:
print e
traceback.print_exc()
def on_start(self, goal, frame):
self.goal = goal
filtered_image = self.obstacleDetector.applyFilter(frame)
planner = PathPlanner()
self.path, expanded, found_goal = planner.planPath(
filtered_image, self.marbleStateManager.get_position(), goal)
print("Got a path of length: " + str(len(self.path)))
def __init__(self):
self.path_planner = PathPlanner()
# will need a map:
map_maker = MapMaker()
map_maker.set_map()
map_maker.calibrate_map()
lowres_map = map_maker.get_lowres_map()
self.path_planner.set_map(lowres_map)
def __init__(self):
if not self.load_parameters(): sys.exit(1)
self._planner = PathPlanner('{}_arm'.format(self._arm))
rospy.on_shutdown(self.shutdown)
if not self.register_callbacks(): sys.exit(1)
def main():
# rospy.spin()
left = baxter_interface.Limb('left')
lj = left.joint_names()
left_gripper = robot_gripper.Gripper('left')
right = baxter_interface.Limb('right')
rj = right.joint_names()
right_gripper = robot_gripper.Gripper('right')
print(right_gripper.force())
left.set_joint_position_speed(0.8)
right.set_joint_position_speed(0.8)
print("Initializing planner")
planner_right = PathPlanner("right_arm")
planner_left = PathPlanner("left_arm")
print("Calling function")
# move_at_angle(left, right, lj, rj, planner_left, planner_right, 0.4)
return_to_neutral(left, right, lj, rj, planner_left, planner_right)
def setUp(self):
self.path_planner = PathPlanner()
# will need a map:
map_maker = MapMaker()
map_maker.set_map()
map_maker.calibrate_map()
lowres_map = map_maker.get_lowres_map()
self.path_planner.set_map(lowres_map)
self.path_planner.set_start_node()
self.path_planner.set_end_node()
def __init__(self):
print("Starting init")
self.left_arm = baxter_interface.Limb('left')
print("Left arm obtrained")
self.lj = self.left_arm.joint_names()
self.left_gripper = robot_gripper.Gripper('left')
self.left_joint_neutral = self.left_arm.joint_angles()
self.right_arm = baxter_interface.Limb('right')
self.rj = self.right_arm.joint_names()
self.right_gripper = robot_gripper.Gripper('right')
self.right_joint_neutral = self.right_arm.joint_angles()
self.planner_left = PathPlanner('left_arm')
self.planner_right = PathPlanner('right_arm')
self.orien_const_left_vert = OrientationConstraint()
self.orien_const_left_vert.link_name = "left_gripper"
self.orien_const_left_vert.header.frame_id = "base"
self.orien_const_left_vert.orientation.y = -1.0
self.orien_const_left_vert.absolute_x_axis_tolerance = 0.1
self.orien_const_left_vert.absolute_y_axis_tolerance = 0.1
self.orien_const_left_vert.absolute_z_axis_tolerance = 0.1
self.orien_const_left_vert.weight = 1.0
self.orien_const_right_vert = copy.deepcopy(self.orien_const_left_vert)
self.orien_const_right_vert.link_name = "right_gripper"
self.previous_deviation = (None, None)
self.setting_to_neutral = False
self.tilt_after_neutral = True
self.pub = rospy.Publisher("/board_controller_log",
std_msgs.msg.String,
queue_size=10)
rospy.Subscriber("/physics_inference", std_msgs.msg.String,
self.perform_tilt)
rospy.Subscriber("/control/neutral_set", std_msgs.msg.Bool,
self.neutral_listener)
rospy.Subscriber("control/tilt_set", std_msgs.msg.Bool,
self.tilt_listener)
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 __init__(self, map_name='campus'):
self.map = MapLoader.load(path='resources/campus.json')
self.pt = PositionTracker(osmap=self.map)
self.pf = PositionFetcher(self.pt)
self.planner = PathPlanner(self.pt, on_wp_reached=self.remove_waypoint)
self.vis = Visualizer(map_name=map_name,
pt=self.pt,
add_wp=self.add_waypoint,
osmap=self.map)
self.waypoints_lock = Lock()
self.waypoints_changed = True
self.waypoints: Optional[List[Waypoint]] = None
self.set_waypoints([])
def __init__(self, dt, target_speed, vehicle, param_dict):
super(RandomBehaviorPlanner, self).__init__(vehicle, dt, param_dict)
self.dt = dt
self.target_speed = target_speed
self.path_planner = PathPlanner(self.vehicle, {'dt': dt,
'target_speed': target_speed})
self.p_l = param_dict['p_l']
self.p_r = param_dict['p_r']
self.switcher_step = 0 # cycles through 0, 1, ..., (Tds - 1) each timestep
self.chg_hazard_l = False # there is a hazard on the left
self.hazard_c = False # there is a hazard ahead
self.chg_hazard_r = False # there is a hazard on the right
def __init__(self):
rospy.init_node('waypoint_updater')
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
# TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below
self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
# TODO: Add other member variables you need below
self.current_pose = None
self.base_waypoints = None
self.planner = PathPlanner(LOOKAHEAD_WPS)
def play_chords(note_pos):
planner = PathPlanner("left_arm")
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]
for i, pos in enumerate(new_note_pos):
print(i, pos)
# 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.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())
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def __init__(self):
rospy.init_node('waypoint_updater')
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
# TODO: Add a subscriber for /traffic_waypoint and /obstacle_waypoint below
rospy.Subscriber('/traffic_waypoint', Int32, self.traffic_cb)
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_lights_cb)
self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
self.speed_limit = rospy.get_param('/waypoint_loader/velocity') * 1000 / 3600.
# TODO: Add other member variables you need below
self.light_wp = None
self.lights = None
self.current_pose = None
self.base_waypoints = None
self.planner = PathPlanner(LOOKAHEAD_WPS)
self.planner.set_speed_limit(self.speed_limit)
def __init__(self, dt, target_speed, vehicle, param_dict, CAV_agents_dict):
super(CAVBehaviorPlanner, self).__init__(vehicle, dt, param_dict)
self.dt = dt
self.target_speed = target_speed
self.path_planner = PathPlanner(self.vehicle, {'dt': dt,
'target_speed': target_speed})
self.F = param_dict['F']
self.w = param_dict['w']
self.theta_CL = param_dict['theta_CL']
self.theta_CR = param_dict['theta_CR']
self.eps = param_dict['eps']
self.chg_hazard_l = False
self.hazard_c = False
self.chg_hazard_r = False
self.switcher_step = 0 # cycles through 0, 1, ..., (Tds - 1) each timestep
self.Qv_l = 0.9 * self.target_speed
self.Qv_c = 0.9 * self.target_speed
self.Qv_r = 0.9 * self.target_speed
self.change_buf = Queue(maxsize=self.F)
while not self.change_buf.full():
self.change_buf.put(False)
self.Qf = 0
self.rCL = 0
self.rCR = 0
self.neighbor_left = []
self.neighbor_current = []
self.neighbor_right = []
self.behavior = Behavior.KEEP_LANE
self.closeneighbor_left = []
self.closeneighbor_current = []
self.closeneighbor_right = []
self.CAV_agents_dict = CAV_agents_dict
def generate_paths(self, cost_map):
problem_valid = False
# Choose a goal position
num_alternatives = len(self.possible_goals)
goal_position = self.possible_goals[random.randint(0, num_alternatives-1)]
while not problem_valid:
# Trying to generate a new problem
start_position = (random.randint(0, self.height - 1), random.randint(0, self.width - 1))
# If the start happen to be within an obstacle, we discard them and
# try new samples
if cost_map.is_occupied(start_position[0], start_position[1]):
continue
if start_position == goal_position:
continue
try:
path_planner = PathPlanner(cost_map)
dijkstra_path, cost = path_planner.dijkstra(start_position, goal_position)
greedy_path, cost = path_planner.greedy(start_position, goal_position)
a_star_path, cost = path_planner.a_star(start_position, goal_position)
problem_valid = True
except AttributeError:
# In case there is no valid path
continue
# print(start_position, goal_position)
# plt.matshow(cost_map.grid)
# plt.plot(start_position[1], start_position[0], 'g*', markersize=8)
# plt.plot(goal_position[1], goal_position[0], 'rx', markersize=8)
# title = str(start_position) + ", " + str(goal_position)
# plt.title(title)
# plt.show()
return [dijkstra_path, greedy_path, a_star_path]
def __init__(self):
#self.listener = KinectSubscriber()
self.map_maker = MapMaker()
#self.map_maker.set_map()
#self.map_data = self.map_maker.get_lowres_map()
self.path_planner = PathPlanner()
def main():
# print("Here are the marker rotations: " + str(marker_rotations))
# print("Here are the marker translations: " + str(marker_translations))
#assert len(marker_translations) == 1
#R = marker_rotations[0]
#t = marker_translations[0]
global top_left_x
global top_left_y
global top_left_z
# get_top_left_AR_tag()
top_left_x, top_left_y, top_left_z = 0.969, 0.143, -0.214
# assert len(marker_translations) == 1
# top_left_x, top_left_y, top_left_z = marker_translations[0]
sawyer = Robot(PathPlanner("right_arm"), None, "right_gripper", 0.095)
# Adding environment obstacles for the robot to avoid during path planning
# add_block_obstacle(sawyer, position=[0.67452, -0.4455, 0.916557], name='right_wall', size=[5, 0, 5], orient=[1, 0, 0, 0])
# add_block_obstacle(sawyer, position=[0.460625, 0.5444, 0.916557], name='left_wall', size=[5, 0, 5], orient=[1, 0, 0, 0])
# add_block_obstacle(sawyer, position=[, , TABLE_HEIGHT - sawyer.gripper_length], name='table', size=[3.5, 3.5, 0], orient=[0, 1, 0, 0])
# add_block_obstacle(planner, [0,0,0], 'ceiling', [10, 5, 1])
'''
add_block_obstacle(sawyer,
position=[0.81, -0.26, -0.1 + (0.5 * BLOCK_TOTAL_HEIGHT)],
name='baseplate',
size=[5 * BLOCK_SIDE_LENGTH, 6 * BLOCK_SIDE_LENGTH , 0.5 * BLOCK_TOTAL_HEIGHT])
add_block_obstacle(sawyer,
position=[0.81, -0.26, -0.1 - (BLOCK_TOTAL_HEIGHT / 2)],
name='table',
size=[5, 5, BLOCK_TOTAL_HEIGHT / 2])
'''
add_block_obstacle(sawyer,
position=[top_left_x, top_left_y, top_left_z - (BLOCK_TOTAL_HEIGHT / 2)],
name='table',
size=[5, 5, BLOCK_TOTAL_HEIGHT / 2])
import random
# BASE_PLATE_HEIGHT = 0.5 * BLOCK_SQUARE_HEIGHT
# dest_x, dest_y = block_coord_to_sawyer((5, 4))
# start_z_offset = 0.08
# for layer in range(2):
# new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT + layer * BLOCK_SQUARE_HEIGHT
# add_block_obstacle(sawyer, [dest_x, dest_y, new_dest_z - (0.5 * BLOCK_SQUARE_HEIGHT)], 'ehllo' + str(random.randint(1, 1000)), [BLOCK_SIDE_LENGTH, BLOCK_SIDE_LENGTH, BLOCK_SQUARE_HEIGHT])
# #TODO: Finish adding as many environment constraints as possible without compromising on path planning.
# return
dest_x, dest_y = block_coord_to_sawyer((5, 4))
# add_block_obstacle(sawyer, position=[dest_x, dest_y, top_left_z], name='table', size=[5, 5, 0], orient=[1, 0, 0, 0])
# add_block_obstacle(sawyer, position=[dest_x, dest_y, top_left_z - 0.006 + 0.25 * BLOCK_SQUARE_HEIGHT], name='baseplate', size=[5 * BLOCK_SIDE_LENGTH , 6 * BLOCK_SIDE_LENGTH, .5 * BLOCK_SQUARE_HEIGHT], orient=[1, 0, 0, 0])
# print('Top left:', top_left_x, top_left_y)
#start_x3 = top_left_x - 0.074 - (BLOCK_SIDE_LENGTH / 2.0)
#start_y3 = top_left_y + 0.0425 + (BLOCK_SIDE_LENGTH / 2.0)
#print(start_x3, start_y3)
start_x3, start_y3 = start_num_to_sawyer(3)
print(start_x3, start_y3)
# print()
# for i in range(8):
# print(start_num_to_sawyer(i))
#while 1 < 2:
# pick_and_place_block(sawyer, start_x3, start_y3, top_left_z + BLOCK_SQUARE_HEIGHT, dest_x, dest_y, top_left_z + (0.5 * BLOCK_SQUARE_HEIGHT), 'block_test')
#pick_and_place_block(sawyer, start_x3, start_y3, top_left_z + BLOCK_SQUARE_HEIGHT, dest_x, dest_y, top_left_z + BLOCK_SQUARE_HEIGHT, 'block_test')
#pick_and_place_block(sawyer, top_left_x, top_left_y, top_left_z, dest_x, dest_y, top_left_z + BLOCK_SQUARE_HEIGHT, 'block_test')
start_z_offset = 0.08
BASE_PLATE_HEIGHT = 0.5 * BLOCK_SQUARE_HEIGHT
print(top_left_x, top_left_y, top_left_z)
# import random
# for layer in range(3):
# # dest_x, dest_y = block_coord_to_sawyer((5 + layer * 2, 4))
# # new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT
# new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT + layer * BLOCK_SQUARE_HEIGHT
# pick_and_place_block(sawyer,
# start_x3, start_y3, top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset,
# dest_x, dest_y, new_dest_z,
# 'block_' + str(random.randint(1, 1000)))
import numpy as np
workspace_src_path = os.path.abspath(os.path.join(planningPath, '..'))
creator_cv_path = os.path.join(workspace_src_path, 'creator_cv')
creator_cv_src_path = os.path.join(creator_cv_path, 'src')
creator_cv_blocks_path = os.path.join(creator_cv_src_path, 'blocks')
file_name = 'test.npy'
blocks = np.load(creator_cv_blocks_path + '/' + file_name)
for layer, layer_blocks = enumerate(blocks):
for layer_block in layer_blocks:
dest_x, dest_y = block_coord_to_sawyer(layer_block)
# new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT
new_dest_z = top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset + BASE_PLATE_HEIGHT + layer * BLOCK_SQUARE_HEIGHT
pick_and_place_block(sawyer,
start_x3, start_y3, top_left_z + BLOCK_SQUARE_HEIGHT + sawyer.gripper_length + start_z_offset,
dest_x, dest_y, new_dest_z,
'block_' + str(random.randint(1, 10000)))
def main():
"""
Main Script
"""
#############
offset = -0.3
#############
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("left_arm")
##
## Init Gripper
##
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
left = baxter_interface.Gripper('left', CHECK_VERSION)
# #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;
tf_listener = tf.TransformListener(rospy.Duration(1))
id_num = raw_input("Input Grasp ID: ")
tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num),
rospy.Time(0), rospy.Duration(2.0))
t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num),
rospy.Time())
matrix = quaternion_matrix(q)
new_matrix = matrix
# new_matrix = np.eye(4)
# new_matrix[:, 0] = matrix[:, 2]
# new_matrix[:, 1] = -matrix[:, 1]
# new_matrix[:, 2] = matrix[:, 0]
t_pre = t + new_matrix[0:3, 2] * offset
quaternion_from_matrix
q = quaternion_from_matrix(new_matrix)
print("try to move to object...........Move it!")
##########
id_num = 0
##########
while not rospy.is_shutdown():
i_pregrasp = 0
while not rospy.is_shutdown():
###########################################
# if i_pregrasp > 5:
# tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num), rospy.Time(0), rospy.Duration(2.0))
# t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num), rospy.Time())
# if id_num > rospy.get_param('good_grasps'):
# print("Fail!")
# return
############################################
i_pregrasp += 1
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = t_pre[0]
goal_1.pose.position.y = t_pre[1]
goal_1.pose.position.z = t_pre[2]
#Orientation as a quaternion
goal_1.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_1, list())
print(plan)
if not plan:
raise Exception("Execution failed")
## in Rviz
rospy.sleep(6)
raw_input("Press <Enter> to PRE grasp pose: ")
print("PRE grasp pose %d" % i_pregrasp)
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
i_grasp = 0
while not rospy.is_shutdown():
i_grasp += 1
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = t[0]
goal_2.pose.position.y = t[1]
goal_2.pose.position.z = t[2]
#Orientation as a quaternion
goal_2.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_2, list())
if not plan:
raise Exception("Execution failed")
## in Rviz
rospy.sleep(6)
raw_input("Press <Enter> to GRASP pose : ")
print("GRASP pose %d" % i_grasp)
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
##
## grasp
##
# open_gripper(left)
# close_gripper(left)
i_leave = 1
while not rospy.is_shutdown():
i_leave += 1
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
# [0.551, 0.690, -0.049]
goal_3.pose.position.x = 0.551
goal_3.pose.position.y = 0.690
goal_3.pose.position.z = -0.049
#Orientation as a quaternion
goal_3.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_3, list())
if not plan:
raise Exception("Execution failed")
rospy.sleep(6)
raw_input("Press <Enter> to LEAVE pose: ")
print("LEAVE pose %d" % i_leave)
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
##
## grasp
##
open_gripper(left)
raw_input("Press <Enter> to continue")
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
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():
"""
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)
# raw_input("Press <Enter> to move the right arm to goal pose: ")
rospy.sleep(1)
# Might have to edit this for part 5
if not planner.execute_plan(plan):
raise Exception("Execution failed")
else:
break
except Exception as e:
print e
traceback.print_exc()
# else:
# break
while not rospy.is_shutdown():
right_gripper = robot_gripper.Gripper('right')
right_gripper.set_moving_force(80.0)
right_gripper.set_holding_force(80.0)
# right_gripper.calibrate()
# Set your goal positions here
print("starting")
# move_to_goal(0.85, -0.3001, 0.1)
# rospy.sleep(1.)
move_to_goal(0.85, -0.2995, 0.1)
print("opening")
right_gripper.open()
rospy.sleep(1.)
print("executing")
move_to_goal(0.85, -0.2995, -0.030) #-0.41
print("closings")
# right_gripper.close()
print("MISSED: ", right_gripper.missed())
print("FORCEEE: ", right_gripper.force())
print("Done")
# move_to_goal(0.4225 + 0.1, -0.1265, 0.7725 - 0.92)
# right_gripper.close()
# move_to_goal(0.4225 + 0.1 + 0.05, -0.1265, 0.7725 - 0.92)
# right_gripper.open()
break
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
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()
if __name__ == '__main__':
rospy.init_node('moveit_node')
planner_left = PathPlanner("left_arm")
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]
