def test_grasps(robot, node_points, elements):
element = elements[-1]
[element_body] = create_elements(node_points, [element])
phi = 0
link = link_from_name(robot, TOOL_NAME)
link_pose = get_link_pose(robot, link)
draw_pose(link_pose) #, parent=robot, parent_link=link)
wait_for_interrupt()
n1, n2 = element
p1 = node_points[n1]
p2 = node_points[n2]
length = np.linalg.norm(p2 - p1)
# Bottom of cylinder is p1, top is p2
print(element, p1, p2)
for phi in np.linspace(0, np.pi, 10, endpoint=True):
theta = np.pi / 4
for t in np.linspace(-length / 2, length / 2, 10):
translation = Pose(
point=Point(z=-t)
) # Want to be moving backwards because +z is out end effector
direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi))
angle = Pose(euler=Euler(yaw=1.2))
grasp_pose = multiply(angle, direction, translation)
element_pose = multiply(link_pose, grasp_pose)
set_pose(element_body, element_pose)
wait_for_interrupt()
def debug_elements(robot, node_points, node_order, elements):
#test_grasps(robot, node_points, elements)
#test_print(robot, node_points, elements)
#return
for element in elements:
color = (0, 0, 1) if doubly_printable(element, node_points) else (1, 0,
0)
draw_element(node_points, element, color=color)
wait_for_interrupt('Continue?')
# TODO: topological sort
node = node_order[40]
node_neighbors = get_node_neighbors(elements)
for element in node_neighbors[node]:
color = (0, 1,
0) if element_supports(element, node, node_points) else (1, 0,
0)
draw_element(node_points, element, color)
element = elements[-1]
draw_element(node_points, element, (0, 1, 0))
incoming_edges, _ = neighbors_from_orders(
get_supported_orders(elements, node_points))
supporters = []
retrace_supporters(element, incoming_edges, supporters)
for e in supporters:
draw_element(node_points, e, (1, 0, 0))
wait_for_interrupt('Continue?')
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_interrupt()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_interrupt()
def step_trajectory(trajectory, attachments={}, time_step=np.inf):
for _ in trajectory.iterate():
for attachment in attachments.values():
attachment.assign()
if time_step == np.inf:
wait_for_interrupt(time_step)
else:
wait_for_duration(time_step)
def test_confs(robot, num_samples=10):
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(num_samples):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_interrupt()
def test_ik(robot, node_order, node_points):
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for n in node_order:
point = node_points[n]
set_joint_positions(robot, movable_joints, sample_fn())
conf = inverse_kinematics(robot, link, (point, None))
if conf is not None:
set_joint_positions(robot, movable_joints, conf)
continue
link_point, link_quat = get_link_pose(robot, link)
#print(point, link_point)
#user_input('Continue?')
wait_for_interrupt()
def main(time_step=0.01):
# TODO: closed world and open world
real_world = connect(use_gui=True)
add_data_path()
task, state = get_problem1(localized='rooms',
p_other=0.5) # surfaces | rooms
for body in task.get_bodies():
add_body_name(body)
robot = task.robot
#dump_body(robot)
assert (USE_DRAKE_PR2 == is_drake_pr2(robot))
attach_viewcone(robot) # Doesn't work for the normal pr2?
draw_base_limits(get_base_limits(robot), color=(0, 1, 0))
#wait_for_interrupt()
# TODO: partially observable values
# TODO: base movements preventing pick without look
# TODO: do everything in local coordinate frame
# TODO: automatically determine an action/command cannot be applied
# TODO: convert numpy arrays into what they are close to
# TODO: compute whether a plan will still achieve a goal and do that
# TODO: update the initial state directly and then just redraw it to ensure uniqueness
step = 0
while True:
step += 1
print('\n' + 50 * '-')
print(step, state)
#print({b: p.value for b, p in state.poses.items()})
with ClientSaver():
commands = plan_commands(state)
print()
if commands is None:
print('Failure!')
break
if not commands:
print('Success!')
break
apply_commands(state, commands, time_step=time_step)
print(state)
wait_for_interrupt()
disconnect()
def step_plan(plan, **kwargs):
wait_for_interrupt()
attachments = {}
for action, args in plan:
trajectory = args[-1]
if action == 'move':
step_trajectory(trajectory, attachments, **kwargs)
elif action == 'pick':
attachment = trajectory.attachments.pop()
step_trajectory(trajectory, attachments, **kwargs)
attachments[attachment.child] = attachment
step_trajectory(trajectory.reverse(), attachments, **kwargs)
elif action == 'place':
attachment = trajectory.attachments.pop()
step_trajectory(trajectory, attachments, **kwargs)
del attachments[attachment.child]
step_trajectory(trajectory.reverse(), attachments, **kwargs)
else:
raise NotImplementedError(action)
wait_for_interrupt()
def display_trajectories(ground_nodes, trajectories, time_step=0.05):
if trajectories is None:
return
connect(use_gui=True)
floor, robot = load_world()
wait_for_interrupt()
movable_joints = get_movable_joints(robot)
#element_bodies = dict(zip(elements, create_elements(node_points, elements)))
#for body in element_bodies.values():
# set_color(body, (1, 0, 0, 0))
connected = set(ground_nodes)
for trajectory in trajectories:
if isinstance(trajectory, PrintTrajectory):
print(trajectory, trajectory.n1 in connected, trajectory.n2
in connected, is_ground(trajectory.element, ground_nodes),
len(trajectory.path))
connected.add(trajectory.n2)
#wait_for_interrupt()
#set_color(element_bodies[element], (1, 0, 0, 1))
last_point = None
handles = []
for conf in trajectory.path:
set_joint_positions(robot, movable_joints, conf)
if isinstance(trajectory, PrintTrajectory):
current_point = point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_NAME)))
if last_point is not None:
color = (0, 0, 1) if is_ground(trajectory.element,
ground_nodes) else (1, 0, 0)
handles.append(
add_line(last_point, current_point, color=color))
last_point = current_point
wait_for_duration(time_step)
#wait_for_interrupt()
#user_input('Finish?')
wait_for_interrupt()
disconnect()
def simulate_plan(plan, time_step=0.0, real_time=False): #time_step=np.inf
wait_for_interrupt()
enable_gravity()
if real_time:
enable_real_time()
for action, args in plan:
trajectory = args[-1]
if action == 'move':
simulate_trajectory(trajectory, time_step)
elif action == 'pick':
attachment = trajectory.attachments.pop()
simulate_trajectory(trajectory, time_step)
add_fixed_constraint(attachment.child, attachment.parent,
attachment.parent_link)
simulate_trajectory(trajectory.reverse(), time_step)
elif action == 'place':
attachment = trajectory.attachments.pop()
simulate_trajectory(trajectory, time_step)
remove_fixed_constraint(attachment.child, attachment.parent,
attachment.parent_link)
simulate_trajectory(trajectory.reverse(), time_step)
else:
raise NotImplementedError(action)
wait_for_interrupt()
