def main(node,
subscriber,
org_surface_pose=[0.65, 0.4, 0.15, np.pi, 0.0, 0.0],
new_surface_pose=[0.25, 0.85, 0.15, np.pi, 0.0, 0.0]):
"""Main loop."""
# Create node
rospy.init_node(node)
# Initialize robot and arm.
eb.enable_robot()
limb = eb.Arm("left")
# Initialize physics engine and environment.
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(3, 0, -89.99, [0, 0, 0])
# Load surface to physics engine
s_name = "surface"
s_path = URDF_DIR + "{}.urdf".format(s_name)
s_id = p.loadURDF(s_path, useFixedBase=True, globalScaling=10)
p.changeVisualShape(s_id, -1, rgbaColor=[1, 0.992, 0.816, 1])
rate = rospy.Rate(10)
loaded_body_ids = []
while not rospy.is_shutdown():
# Get readings from camera
arrangement = obtain_arrangement(subscriber)
# Get body ids from physics engine and the dictionary of the original objs
for i in loaded_body_ids:
p.removeBody(i)
loaded_body_ids = sim(arrangement)
# Get input from user
char = eb.get_char(5)
if char in ['\x1b', '\x03']:
p.disconnect()
rospy.signal_shutdown("Shutting down.")
# Go to main surface
if char == 'o':
limb.move_to_pose(org_surface_pose)
# Go to secondary surface
if char == 'n':
limb.move_to_pose(new_surface_pose)
rate.sleep()
PARSER.add_argument(
"--theta_offset",
"-t",
help=
"Theta offset from camera-detection frame to end-effector frame in m.",
default=np.pi / 2,
type=float)
ARGS = PARSER.parse_args()
# Create node
rospy.init_node(ARGS.node)
# Initialize robot and move selected arm to correct pose.
eb.enable_robot()
arm = eb.Arm(ARGS.arm)
# First from primary surface to secondary surface
surface_to_surface(ARGS.subscriber,
surface1_pose=[0.65, 0.4, 0.15, np.pi, 0.0, 0.0],
surface2_pose=[0.25, 0.85, 0.15, np.pi, 0.0, 0.0],
arm=arm,
gripper_height=ARGS.gripper_height,
x_offset=ARGS.x_offset,
y_offset=ARGS.y_offset,
theta_offset=ARGS.theta_offset)
# Then from secondary surface to primary surface
surface_to_surface(ARGS.subscriber,
surface1_pose=[0.25, 0.85, 0.15, np.pi, 0.0, 0.0],
surface2_pose=[0.65, 0.4, 0.15, np.pi, 0.0, 0.0],
num = -total_length - num
res = num * 1.0 # Make all numbers float, to be consistent
return res
# Create node
rospy.init_node('test_pnp')
# Define camera poses
org_surface_pose = [0.65, 0.4, 0.15, np.pi, 0.0, 0.0]
new_surface_pose = [0.25, 0.85, 0.15, np.pi, 0.0, 0.0]
# Initialize robot and move left arm to initial pose.
eb.enable_robot()
limb = eb.Arm("left")
plan = pd.read_csv("my_plan.csv")
plan.poseFrom = plan.poseFrom.apply(ast.literal_eval)
plan.poseTo = plan.poseTo.apply(ast.literal_eval)
scene = PlanningSceneInterface()
box_pose = PoseStamped()
box_pose.header.frame_id = "world"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 10
box_pose.pose.position.z = 0
box_pose.pose.orientation.x = 0
box_pose.pose.orientation.y = 0
box_pose.pose.orientation.z = 0
box_pose.pose.orientation.w = 1
def main(node, arm, initial_pose, prismatic_increment, rotational_increment):
"""Executes the ROS node"""
print 'Initializing; please wait.'
rospy.init_node(node)
eb.enable_robot()
limb = eb.Arm(arm)
if not initial_pose:
pass
else:
limb.move_to_pose(initial_pose)
print "You are now in control; press h for help and Esc to quit."
while not rospy.is_shutdown():
char = eb.get_char()
if char:
# Esc or ctrl-c to exit
if char in ['\x1b', '\x03']:
rospy.signal_shutdown("Example finished.")
# Prismatic motion
elif char in ['w', 'W']:
limb.move_by_increment('x', prismatic_increment)
elif char in ['s', 'S']:
limb.move_by_increment('x', -prismatic_increment)
elif char in ['a', 'A']:
limb.move_by_increment('y', prismatic_increment)
elif char in ['d', 'D']:
limb.move_by_increment('y', -prismatic_increment)
elif char in ['z', 'Z']:
limb.move_by_increment('z', prismatic_increment)
elif char in ['x', 'X']:
limb.move_by_increment('z', -prismatic_increment)
# Rotational motion
elif char in ['i', 'I']:
limb.move_by_increment('roll', rotational_increment)
elif char in ['k', 'K']:
limb.move_by_increment('roll', -rotational_increment)
elif char in ['j', 'J']:
limb.move_by_increment('pitch', rotational_increment)
elif char in ['l', 'L']:
limb.move_by_increment('pitch', -rotational_increment)
elif char in ['n', 'N']:
limb.move_by_increment('yaw', rotational_increment)
elif char in ['m', 'M']:
limb.move_by_increment('yaw', -rotational_increment)
# Gripper actions
elif char in ['o', 'O']:
limb.grip.open()
elif char in ['c', 'C']:
limb.grip.close()
# Info
elif char in ['p', 'P']:
print limb.get_pose()
# Help
elif char in ['h', 'H']:
print_help()
def get_query(node, subscriber, interactive=True):
"""Creates a simulation of the arrangement detected by the robot and uses it
to find an optimal arrangement of more bodies."""
# Create node
rospy.init_node(node)
# Display project info
pkg_path = rospkg.RosPack().get_path('experiment')
eb.display_image(pkg_path + '/share/images/main.png')
# Define camera poses
org_surface_pose = [0.65, 0.4, 0.15, np.pi, 0.0, 0.0]
new_surface_pose = [0.25, 0.85, 0.15, np.pi, 0.0, 0.0]
# Initialize robot and move left arm to initial pose.
eb.enable_robot()
limb = eb.Arm("left")
limb.move_to_pose(org_surface_pose)
eb.display_image(pkg_path + '/share/images/step1.png')
# Begin constructing query dic
s_name = "surface"
s_path = URDF_DIR + "{}.urdf".format(s_name)
s_info = {
"path": s_path,
"pose": [0.0, 0.0, 0.0],
"z offset": 0.05,
"area": 0.0855
}
overall_config = {s_name: s_info, "obstacles": {}}
if interactive:
# Initialize physics engine and environment.
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(3, 0, -89.99, [0, 0, 0])
# Load surface to physics engine
s_id = p.loadURDF(s_path, useFixedBase=True, globalScaling=10)
p.changeVisualShape(s_id, -1, rgbaColor=[1, 0.992, 0.816, 1])
# Get readings from camera
arrangement = obtain_arrangement(subscriber)
# Get body ids from physics engine and the dictionary of the original objs
loaded_body_ids, config_dic = extract_query(arrangement, interactive)
rate = rospy.Rate(10)
org_flag = False
new_flag = False
moved = False
while not rospy.is_shutdown():
char = eb.get_char(5)
if char in ['\x1b', '\x03']:
p.disconnect()
rospy.signal_shutdown("Shutting down.")
# Signal satisfaction with detected org config
if char == 'o':
org_flag = True
# Signal satisfaction with detected new config
if char == 'n':
new_flag = True
# Get readings of org config from camera again
if not org_flag:
arrangement = obtain_arrangement(subscriber)
for i in loaded_body_ids:
p.removeBody(i)
loaded_body_ids, config_dic = extract_query(arrangement,
interactive=True)
# Get readings of new config from camera (again)
elif org_flag and not new_flag:
if not moved:
limb.move_to_pose(new_surface_pose)
moved = True
overall_config.update({"originals": config_dic})
arrangement = obtain_arrangement(subscriber)
for i in loaded_body_ids:
p.removeBody(i)
loaded_body_ids, config_dic = extract_query(arrangement,
interactive=True,
new=True)
elif org_flag and new_flag:
# dump the json
overall_config.update({"news": config_dic})
# shutdown and exit
p.disconnect()
my_query = json.dumps(overall_config)
placement = generate_placement(my_query,
algo='middle',
cam_dist=3,
verbose=False)
ans_json = placement.dump_json()
with open("my_ans.json", "wb") as f:
json.dump(ans_json, f)
rospy.signal_shutdown("Shutting down.")
rate.sleep()
else:
# Get configuration of the main surface
rospy.sleep(5)
arrangement = obtain_arrangement(subscriber)
_, overall_config = extract_query(arrangement,
overall_config,
interactive=False)
# Get configuration of the surface with new objects
limb.move_to_pose(new_surface_pose)
rospy.sleep(5)
arrangement = obtain_arrangement(subscriber)
_, overall_config = extract_query(arrangement,
overall_config,
interactive=False,
new=True)
# Create the query from the configurations detected and save it
query_json = json.dumps(overall_config, sort_keys=True, indent=4)
with open("my_query.json", "wb") as f:
f.write(query_json)
# Find a solution for the placement problem and save it
eb.display_image(pkg_path + '/share/images/step2.png')
placement = generate_placement(json.loads(query_json))
ans_json = placement.dump_json()
with open("my_placement.json", "wb") as f:
f.write(ans_json)
# Find a solution for the planning problem and save it
eb.display_image(pkg_path + '/share/images/step3.png')
plan = generate_plan(placement, new=False)
plan.to_csv("my_plan.csv")
# Execute the rearrangement process
eb.display_image(pkg_path + '/share/images/step4.png')
gripper_height = 0.172
x_offset = 0.055
y_offset = -0.005
theta_offset = np.pi / 2
for index, step in plan.iterrows():
pick_x = (step["poseFrom"][1] /
10.0) + new_surface_pose[0] + x_offset
pick_y = (step["poseFrom"][0] /
10.0) + new_surface_pose[1] + y_offset
pick_theta = normalize(step["poseFrom"][2] + theta_offset)
pick_pose = [
pick_x, pick_y, new_surface_pose[2] - gripper_height, np.pi,
0.0, pick_theta
]
place_x = (step["poseTo"][1] /
10.0) + org_surface_pose[0] + x_offset
place_y = (step["poseTo"][0] /
10.0) + org_surface_pose[1] + y_offset
place_theta = normalize(step["poseTo"][2] + theta_offset)
place_pose = [
place_x, place_y, org_surface_pose[2] - gripper_height + 0.01,
np.pi, 0.0, place_theta
]
limb.pick_and_place(pick_pose, place_pose)
# Go back to initial image and exit
eb.display_image(pkg_path + '/share/images/main.png')
rospy.signal_shutdown("Shutting down.")