def vector_traj(oracle, vector, manip_name=None):
with oracle.robot:
if manip_name is not None:
oracle.robot.SetActiveManipulator(manip_name)
oracle.robot.SetActiveDOFs(get_arm_indices(oracle))
with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
end_config = inverse_kinematics(oracle, vector_trans(get_manip_trans(oracle), vector))
if end_config is None:
return None
return linear_arm_traj(oracle, end_config, manip_name=manip_name)
def preprocess_edge(oracle, edge):
if not hasattr(edge, 'preprocessed'):
edge.preprocessed = True
with oracle.robot_saver():
CSpace.robot_arm_and_base(oracle.robot.GetActiveManipulator()).set_active()
for q in edge.configs():
if not hasattr(q, 'saver'):
oracle.robot.SetActiveDOFValues(q.value)
q.saver = oracle.robot_saver()
q.manip_trans = get_manip_trans(oracle)
q.aabbs = [(aabb_min(aabb), aabb_max(aabb)) for aabb in [oracle.compute_part_aabb(part) for part in USE_ROBOT_PARTS]]
def collision_free_grasp(oracle, geom_hash, grasp_tform, approach_vector, manip_name=None):
body_name = oracle.get_body_name(geom_hash)
with oracle.state_saver():
if manip_name is not None: oracle.robot.SetActiveManipulator(manip_name)
oracle.set_active_state(active_obstacles=False, active_objects=set([body_name]))
open_gripper(oracle)
oracle.robot.SetDOFValues(oracle.default_left_arm_config, oracle.robot.GetActiveManipulator().GetArmIndices())
set_trans(oracle.bodies[body_name], object_trans_from_manip_trans(get_manip_trans(oracle), grasp_tform))
if not robot_collision(oracle, body_name):
gripper_config, gripper_traj = oracle.task_manip.CloseFingers(offset=None, movingdir=None, execute=False, outputtraj=False, outputfinal=True,
coarsestep=None, translationstepmult=None, finestep=None, outputtrajobj=True)
return Grasp(grasp_tform, gripper_config, TrajTrajectory(CSpace(oracle.robot,
indices=oracle.robot.GetActiveManipulator().GetGripperIndices()), gripper_traj), approach_vector)
return None
def collision_free_grasp(oracle, geom_hash, grasp_tform, approach_vector, manip_name=None):
body_name = oracle.get_body_name(geom_hash)
with oracle.state_saver():
if manip_name is not None: oracle.robot.SetActiveManipulator(manip_name)
oracle.set_active_state(active_obstacles=False, active_objects=set([body_name]))
open_gripper(oracle)
oracle.robot.SetDOFValues(oracle.default_left_arm_config, oracle.robot.GetActiveManipulator().GetArmIndices())
set_trans(oracle.bodies[body_name], object_trans_from_manip_trans(get_manip_trans(oracle), grasp_tform))
if not robot_collision(oracle, body_name):
gripper_config, gripper_traj = oracle.task_manip.CloseFingers(offset=None, movingdir=None, execute=False, outputtraj=False, outputfinal=True,
coarsestep=None, translationstepmult=None, finestep=None, outputtrajobj=True)
return Grasp(grasp_tform, gripper_config, TrajTrajectory(CSpace(oracle.robot,
indices=oracle.robot.GetActiveManipulator().GetGripperIndices()), gripper_traj), approach_vector)
return None
def preprocess_edge(oracle, edge):
if not hasattr(edge, 'preprocessed'):
edge.preprocessed = True
with oracle.robot_saver():
CSpace.robot_arm_and_base(
oracle.robot.GetActiveManipulator()).set_active()
for q in edge.configs():
if not hasattr(q, 'saver'):
oracle.robot.SetActiveDOFValues(q.value)
q.saver = oracle.robot_saver()
q.manip_trans = get_manip_trans(oracle)
q.aabbs = [(aabb_min(aabb), aabb_max(aabb)) for aabb in [
oracle.compute_part_aabb(part)
for part in USE_ROBOT_PARTS
]]
def policy(self, belief, goal):
# The positions of objects don't change as the robot moves (i.e they are in a global frame)
# The variances grow though as the robot moves
# The robot config has no uncertainty
# Thus, things are sort of relative to the robot
# Sample poses that are reachable given the uncertainty and can see the table
# Can ignore transition uncertainty for action effects
# Only need to keep track of uncertainty along hte first move action as a precondition
# Can either sample robot trajectory rollouts or object poses as the robot moves with increased uncertainty
# Just do large objects for base collisions
from manipulation.primitives.transforms import vector_trans
from manipulation.bodies.robot import approach_vector_from_object_trans
DISPLAY_GRASPS_HPN = False
if DISPLAY_GRASPS_HPN:
obj = 'objA'
conf = belief.pbs.getConf(
) # belief.conf # Default conf (i.e. base is wrong)
grasp_desc = belief.objects[obj].attrs['graspDesc']
shape = belief.objects[obj].attrs['shape']
print shape.parts()[0].vertices()
import util.windowManager3D as wm
win = 'W'
for i in range(len(grasp_desc)):
wm.getWindow(win).clear()
obj_trans = get_object_frame(conf, self.hand, grasp_desc[i])
#approach_trans = get_approach_frame(conf, self.hand)
#print np.round(approach_trans.matrix, 2)
approach_vector = or_from_hpn_approach_vector(
conf.robot, self.hand, grasp_desc[i])
transformed_vector = approach_vector_from_object_trans(
obj_trans.matrix, approach_vector)
obj_trans = hu.Transform(
vector_trans(obj_trans.matrix, transformed_vector))
#temp_matrix = obj_trans.matrix.copy() # NOTE - this works as well?
#temp_matrix[:3, 3] = approach_trans.matrix[:3, 3]
#obj_trans = hu.Transform(temp_matrix)
#print shape.origin() # Origin
#print np.round(obj_trans.matrix, 2)
shape.applyTrans(obj_trans).draw(win, color='blue')
conf.draw(win, color='red')
raw_input('Continue?')
return None
assert not check_belief_collisions(belief, .9)
if satisfies(belief, goal):
return []
if self.plan is None:
env = get_env()
manip_problem = or_manipulation_problem(env, belief, goal)
#manip_problem.set_viewer(env)
self.oracle = ManipulationOracle(
manip_problem, env, active_arms=[self.hand], reset_robot=False
) # TODO - need to avoid resetting the original configuration
# TODO - try just replacing the current problem
DISPLAY_GRASPS_OR = False
if DISPLAY_GRASPS_OR: # NOTE - OR Grasps are from Manip to Pose (not Gripper)
from manipulation.bodies.robot import get_manip_trans, approach_vector_from_object_trans
from manipulation.primitives.transforms import object_trans_from_manip_trans, set_trans, point_from_trans
from manipulation.grasps.grasps import get_grasps
from manipulation.primitives.display import draw_arrow
obj = 'objA'
#set_trans(self.oracle.bodies[obj], np.eye(4))
#print self.oracle.bodies[obj].ComputeAABB()
#for grasp in get_grasps(self.oracle, obj): # NOTE - this randomizes the grasps
for grasp in get_or_grasps(self.oracle, obj):
manip_trans = get_manip_trans(self.oracle)
#grasp_trans = or_from_hpn_grasp(belief.conf.robot, self.hand, grasp_desc[i]).matrix
obj_trans = object_trans_from_manip_trans(
manip_trans, grasp.grasp_trans)
set_trans(self.oracle.bodies[obj], obj_trans)
approach_vector = approach_vector_from_object_trans(
obj_trans, grasp.approach_vector)
approach_trans = vector_trans(obj_trans, approach_vector)
_ = draw_arrow(env, point_from_trans(approach_trans),
point_from_trans(obj_trans))
print
print grasp.approach_vector
print grasp.grasp_index
print np.round(obj_trans, 2)
raw_input('Continue?')
return None
# TODO - check that the plan is still good by converting the state
stream_problem = compile_problem(self.oracle)
self.oracle.draw_goals(
) # NOTE - this must be after compile_problem to ensure the goal_poses convert
if is_viewer_active(self.oracle.env):
raw_input('Start?')
search_fn = get_fast_downward(
'eager', verbose=False
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, universe = focused_planner(stream_problem,
search=search_fn,
greedy=False,
stream_cost=10,
verbose=False,
debug=False)
#plan, universe = incremental_planner(stream_problem, search=search_fn, verbose=True, debug=False)
if plan is None:
plan = []
#visualize_plan(Plan(convert_state(self.oracle, stream_problem.initial_atoms),
# executable_plan(self.oracle, plan)), self.oracle, display=True, save=None)
#for state in get_states(universe, plan):
# or_state = convert_state(self.oracle, state)
# set_state(or_state, self.oracle)
# raw_input('Continue?')
look_iterations = 3
self.plan = []
state_sequence = get_states(universe, plan)
for i, (action, args) in enumerate(convert_plan(plan)):
or_state = convert_state(self.oracle, state_sequence[i])
set_state(or_state, self.oracle)
if action.name in ['move']:
last_config = self.oracle.get_robot_config()
for _ in range(look_iterations):
for obj in belief.objects:
if or_state.get(obj, True) is not None:
result = look_at_ik(self.oracle, obj)
if result is not None:
look_config = self.oracle.get_robot_config(
)
self.plan += [
('move_no_base', (last_config,
look_config)),
('look', (obj, look_config)),
]
last_config = look_config # TODO - does this mean I have to change the next action config?
#print 'Looking at', obj
#raw_input('Pause')
self.plan.append((action.name, args))
elif action.name in ['pick', 'place']:
obj, pose, grasp, approach_config, pap = args
self.plan += [
('move_no_base', (approach_config, pap.vector_config)),
('move_no_base', (pap.vector_config,
pap.grasp_config)),
(action.name, args),
('move_no_base', (pap.grasp_config,
pap.vector_config)),
('move_no_base', (pap.vector_config, approach_config)),
]
if self.replan:
break
else:
self.plan.append((action.name, args))
#raw_input('Continue?')
print self.plan
if not self.plan:
return None
print SEPARATOR
action, args = self.plan.pop(0)
print action, args
if not self.plan and self.replan:
self.plan = None
return [self.convert_action(belief, action, args)
] # NOTE - need to return a list for now