def generator(self, start_vertex, goal_connector, rg):
oracle = self.oracle
if USE_CONFIGURATION == CONFIGURATIONS.DYNAMIC:
IMMEDIATE = True
elif USE_CONFIGURATION == CONFIGURATIONS.FIXED_BRANCHING:
IMMEDIATE = False
start_config = start_vertex.substate['robot']
object_name = goal_connector.condition.object_name
start_pose = start_vertex.substate[object_name]
goal_point = goal_connector.condition.point
if not IMMEDIATE: yield
if PRINT: print '%s-%s: %d'%(self.__class__.__name__, goal_connector.condition, 0)
for trajectory in get_push_trajs(oracle, object_name, start_pose, goal_point, max_failures=INF):
for push in trajectory:
base_trajs = oracle.plan_base_roadmap(start_config, push.approach_config)
if base_trajs is None: continue
vertex1 = rg.vertex(Substate({object_name: push.start_pose}))
vertex2 = rg.vertex(Substate({object_name: push.end_pose}))
edge = rg.edge(MovePush(object_name, push, start_config, base_trajs, oracle))
edge.intermediates.add(edge.connectors[0])
if start_vertex.substate in edge.connectors[0].condition: start_vertex.connect(edge.connectors[0])
vertex1.connect(edge.connectors[0]) # TODO - icky, give conditions names
edge.connect(vertex2)
else:
vertex2.connect(goal_connector)
def push_script(oracle):
body_name = 'blue_cylinder'
body = oracle.get_body(body_name)
start_pose = Pose(get_pose(body))
goal_point = Point(get_point(body) + -.1*unit_x() + .8*unit_y())
# trans1 = get_manip_trans(oracle) # Tip of gripper with strange orientation
# trans2 = np.dot(get_manip_trans(oracle), np.linalg.inv(get_tool_trans(oracle))) # Gripper joint with good orientation
# trans3 = np.dot(get_manip_trans(oracle), get_tool_trans(oracle)) # Nonsense
# oracle.env.Remove(oracle.robot)
# handle1 = draw_point(oracle.env, trans_transform_point(trans1, unit_point()), color=(1, 0, 0, .5))
# handle2 = draw_point(oracle.env, trans_transform_point(trans2, unit_point()), color=(0, 1, 0, .5))
# handle3 = draw_point(oracle.env, trans_transform_point(trans3, unit_point()), color=(0, 0, 1, .5))
# raw_input()
# direction = np.array((-1, 0, 0))
# for contact in get_contacts(oracle, body_name, direction):
# print contact.contact_trans
# body_trans = object_trans_from_manip_trans(get_manip_trans(oracle), contact.contact_trans)
# set_trans(body, body_trans)
# handle = draw_point(oracle.env, trans_transform_point(body_trans, np.array([.1, 0, -.1])))
# print oracle.env.CheckCollision(oracle.robot, body)
# update_viewer()
# raw_input()
# for push in linear_push_trajectory(oracle, body_name, start_pose, goal_point):
# print push
# configs = [
# push.approach_config,
# push.start_vector_config,
# push.start_contact_config,
# push.end_contact_config,
# push.end_vector_config,
# push.approach_config,
# ]
# oracle.set_pose(body_name, push.start_pose)
# for config in configs:
# oracle.set_robot_config(config)
# raw_input('Next?')
# oracle.set_pose(body_name, push.end_pose)
# print
# for push in linear_push_trajectory(oracle, body_name, start_pose, goal_point):
# oracle.set_robot_config(push.start_contact_config)
# raw_input('Execute?')
# grab(oracle, body_name)
# execute_trajectories([push.push_traj])
# release(oracle, body_name)
# raw_input('Next?')
for push in get_push_trajs(oracle, body_name, start_pose, goal_point):
oracle.set_robot_config(push.approach_config)
raw_input('Execute?')
execute_trajectories(push.start_trajs)
grab(oracle, body_name)
execute_trajectories([push.push_traj])
release(oracle, body_name)
execute_trajectories(push.end_trajs)
raw_input('Next?')
