def __init__(self, is_in_simulation, has_gripper):
self.robot_name = "UR5"
self._OPENRAVE_GRIPPER_MAX_VALUE = 0.715584844
self._ROBOT_GRIPPER_MAX_VALUE = 255
self.is_in_simulation = is_in_simulation
self._has_gripper = has_gripper
if not self.is_in_simulation:
self.multicontroller = RaveCreateMultiController(self.GetEnv(), "")
self.SetController(self.multicontroller)
self.manipulator = self.SetActiveManipulator(self.GetManipulators()[0])
if self._has_gripper:
# Needed for "find a grasp" function (not parsed using or_urdf hence
# needs manual setting)
self.manipulator.SetChuckingDirection([1.0])
self.manipulator.SetLocalToolDirection([1.0, 0, 0])
self.ikmodel = databases.inversekinematics.InverseKinematicsModel(
self, iktype=IkParameterization.Type.Transform6D)
if not self.ikmodel.load():
RaveLogInfo("The IKModel for UR5 robot is now being generated. "
"Please be patient, this will take a while "
"(sometimes up to 30 minutes)...")
self.ikmodel.autogenerate()
self.task_manipulation = interfaces.TaskManipulation(self)
self.base_manipulation = interfaces.BaseManipulation(self)
def __init__(self, robot):
self.robot = robot
self.ik6d = rave.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=rave.IkParameterization.Type.Transform6D)
if not self.ik6d.load():
self.ik6d.autogenerate()
self.manipinterface = interfaces.BaseManipulation(robot)
def initialize_openrave(env,
manipulator_name,
min_delta=MIN_DELTA,
collision_checker='ode'):
env.StopSimulation()
for sensor in env.GetSensors():
sensor.Configure(Sensor.ConfigureCommand.PowerOff)
sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
sensor.Configure(Sensor.ConfigureCommand.RenderGeometryOff)
env.Remove(sensor)
env.SetCollisionChecker(RaveCreateCollisionChecker(env, collision_checker))
env.GetCollisionChecker().SetCollisionOptions(0)
assert len(env.GetRobots()) == 1
robot = env.GetRobots()[0]
cd_model = databases.convexdecomposition.ConvexDecompositionModel(robot)
if not cd_model.load():
print 'Generating convex decomposition model'
cd_model.autogenerate()
l_model = databases.linkstatistics.LinkStatisticsModel(robot)
if not l_model.load():
print 'Generating link statistics model'
l_model.autogenerate()
l_model.setRobotWeights()
l_model.setRobotResolutions(xyzdelta=min_delta)
robot.SetActiveManipulator(manipulator_name)
manipulator = robot.GetManipulator(manipulator_name)
robot.SetActiveDOFs(manipulator.GetArmIndices())
ikmodel = databases.inversekinematics.InverseKinematicsModel(
robot=robot,
iktype=IkParameterization.Type.Transform6D,
forceikfast=True,
freeindices=None,
freejoints=None,
manip=None)
if not ikmodel.load():
print 'Generating inverse kinematics model'
ikmodel.autogenerate()
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
ir_model = databases.inversereachability.InverseReachabilityModel(robot)
if ir_model.load():
print 'Generating inverse reachability model'
ir_model.autogenerate()
return robot, manipulator, base_manip, ir_model
def feasible_pick_place(env, robot, obj, grasps, approach_config,
do_motion_planning):
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
pose = Pose(get_pose(obj))
for grasp in grasps:
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
set_config(robot, approach_config,
robot.GetActiveManipulator().GetArmIndices())
if env.CheckCollision(robot) or robot.CheckSelfCollision():
continue
grasp_config = inverse_kinematics_helper(
env, robot,
manip_trans) # NOTE - maybe need to find all IK solutions
#grasp_config = solve_inverse_kinematics_exhaustive(env, robot, manip_trans)
if grasp_config is None:
continue
set_config(robot, grasp_config, get_active_arm_indices(robot))
#approach_config = get_full_config(robot)
traj, arm_traj = None, None
if do_motion_planning:
#traj = vector_traj_helper(env, robot, approach_vector)
traj = workspace_traj_helper(base_manip, approach_vector)
if traj is None:
continue
set_config(robot, traj.end(), get_active_arm_indices(robot))
#vector_config = oracle.get_robot_config()
arm_traj = motion_plan(env,
CSpace.robot_arm(get_manipulator(robot)),
approach_config,
self_collisions=True)
if arm_traj is None:
continue
return grasp_config, traj, arm_traj
return None
def __init__(self,robot,sensorname=None,sensorrobot=None,target=None,maxvelmult=None,randomize=False):
"""Starts a calibration sequencer using a robot and a sensor.
The *minimum needed* to be **specified** is the `robot` and a ``sensorname``. Supports camera sensors that do not belong to the current robot, in this case the IK is done assuming the target is grabbed by the active manipulator of the robot
Can use the visibility information of the target.
:param sensorrobot: If specified, used to determine what robot the sensor lies on.
"""
self.env = robot.GetEnv()
self.robot = robot
self.basemanip = interfaces.BaseManipulation(self.robot,maxvelmult=maxvelmult)
if target is None:
target = self.env.GetKinBody('calibration')
if randomize and target is not None:
pose = poseFromMatrix(target.GetTransform())
target.SetTransform(pose)
self.vmodel = databases.visibilitymodel.VisibilityModel(robot=robot,sensorrobot=sensorrobot,target=target,sensorname=sensorname)
self.vmodel.load()
self.Tpatternrobot = None
if self.vmodel.robot != self.vmodel.sensorrobot and target is not None:
print 'Assuming target \'%s\' is attached to %s'%(target.GetName(),self.vmodel.manip)
self.Tpatternrobot = dot(linalg.inv(self.vmodel.target.GetTransform()),self.vmodel.manip.GetEndEffectorTransform())
def feasible_pick_place(env, robot, obj, grasps, approach_config, do_motion_planning):
t0 = time()
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
#base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
print time() - t0
#t0 = time()
#task_manip = interfaces.TaskManipulation(robot, plannername=None, maxvelmult=None, graspername=None)
#task_manip = interfaces.TaskManipulation(robot, plannername=None, maxvelmult=None, graspername=None)
#print time() - t0
#print
# Slight overhead but it appears you can just keep creating them
#0.000910043716431
#0.000546932220459
pose = Pose(get_pose(obj))
#env.Remove(obj) # NOTE - I'm just testing this now
for grasp in grasps:
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
set_config(robot, approach_config, robot.GetActiveManipulator().GetArmIndices())
if env.CheckCollision(robot) or robot.CheckSelfCollision():
print 'Collision failure'
#rospy.loginfo("Failed collision")
continue
#DrawAxes(env, manip_trans) # TODO - OpenRAVE bug
#print grasp.grasp_trans
#handles = draw_axes(env, get_trans(obj))
#handles = draw_axes(env, manip_trans)
#raw_input('Continue?')
#grasp_config = inverse_kinematics_helper(env, robot, manip_trans) # NOTE - maybe need to find all IK solutions
grasp_config = solve_inverse_kinematics_exhaustive(env, robot, manip_trans)
if grasp_config is None:
print 'IK failure'
#rospy.loginfo("Failed IK")
continue
set_config(robot, grasp_config, get_active_arm_indices(robot))
#approach_config = get_full_config(robot)
traj, arm_traj = None, None
if do_motion_planning:
#traj = vector_traj_helper(env, robot, approach_vector)
traj = workspace_traj_helper(base_manip, approach_vector)
if traj is None:
print 'Vec traj failure'
continue
set_config(robot, traj.end(), get_active_arm_indices(robot))
#vector_config = oracle.get_robot_config()
arm_traj = motion_plan(env, CSpace.robot_arm(get_manipulator(robot)), approach_config, self_collisions=True)
#arm_traj = manip_traj_helper(base_manip, approach_config, max_iterations=25, max_tries=2)
if arm_traj is None:
print 'Approach traj failure'
continue
# TODO - need to do reverse trajectory
return grasp_config, traj, arm_traj
return None
def solve_tamp(env):
viewer = env.GetViewer() is not None
#problem = dantam(env)
problem = dantam2(env)
#problem = move_several_4(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi/2))
initialize_openrave(env, 'leftarm')
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
#USE_GRASP_APPROACH = GRASP_APPROACHES.SIDE
USE_GRASP_APPROACH = GRASP_APPROACHES.TOP
#USE_GRASP_TYPE = GRASP_TYPES.TOUCH
USE_GRASP_TYPE = GRASP_TYPES.GRASP
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()}
assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
##################################################
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
def collision_free(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def grasp_env_cfree(mt, g):
enable_all(False) # TODO - base config?
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def grasp_pose_cfree(mt, g, p):
enable_all(False) # TODO - base config?
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
##################################################
"""
class CollisionStream(Stream): # TODO - could make an initial state version of this that doesn't need pose2
def get_values(self, **kwargs):
self.enumerated = True
pose1, pose2 = map(get_value, self.inputs)
if collision_free(pose1, pose2):
return [PoseCFree(pose1, pose2)]
return []
#return [Movable()] # NOTE - only make movable when it fails a collision check
CheckedInitial = Pred(POSE) # How should this be handled? The planner will need to revisit on the next state anyways
class EnvCollisionStream(Stream): # NOTE - I could also make an environment OBJECT which I mutate. I'm kind of doing that now
movable = []
def get_values(self, **kwargs):
self.enumerated = True
pose, = map(get_value, self.inputs)
results = [CheckedInitial(pose)]
for obj, pose2 in problem.initial_poses.iteritems():
if obj not in self.movable and collision_free(pose, pose2):
pass
#results.append(PoseCFree(pose, pose2))
else:
self.movable.append(obj)
results.append(PoseEq(obj, pose2))
#results.append(Movable(obj))
if results:
pass # NOTE - I could make this fail if there is a collision
# I could prevent the binding by directly adding CheckedInitial to the universe
# In general, I probably can just mutate the problem however I see fit here
return results
"""
##################################################
# NOTE - can do pose, approach manip, true approach traj, motion plan
# NOTE - can make something that produces approach trajectories
def get_manip_vector(pose, grasp):
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
#enable_all(False)
#if manipulator.CheckEndEffectorCollision(manip_trans):
# return None
return ManipVector(manip_trans, approach_vector)
def sample_ik(pose, grasp, base_conf): # TODO - make this return the grasp
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
#print manipulator.CheckEndEffectorCollision(manip_trans)
if grasp_config is not None:
yield [Config(grasp_config)]
#traj = workspace_traj_helper(base_manip, approach_vector)
def sample_grasp_traj(pose, grasp, base_conf):
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector)
if grasp_config is None: return
yield [(Config(grasp_traj.end()), grasp_traj)]
##################################################
# NOTE - can either include the held object in the traj or have a special condition that not colliding
def sample_arm_traj(mq1, mq2, bq): # TODO - need to add holding back in
yield None
#enable_all(False)
#with robot:
# set_base_values(robot, bq.value)
# pass
# TODO - does it make sense to make a new stream for the biasing or to continuously just pass things
# I suppose I could cache the state or full plan as context
class MotionStream(Stream): # TODO - maybe make this produce the correct values
num = 0
#def get_values(self, **kwargs):
# self.enumerated = True
# mq1, mq2, bq = map(get_value, self.inputs)
# #mt = None
# mt = MotionStream.num
# MotionStream.num += 1 # Ensures all are unique
# return [ManipMotion(mq1, mq2, bq, mt)]
def sample_motion_plan(self, mq1, mq2, bq):
set_manipulator_values(manipulator, mq1.value)
set_base_values(robot, bq.value)
return motion_plan(env, cspace, mq2.value, self_collisions=True)
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2, bq = map(get_value, self.inputs)
collision_atoms = filter(lambda atom: atom.predicate in [MTrajGraspCFree, MTrajPoseCFree], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is MTrajGraspCFree:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is MTrajPoseCFree:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies) # How would I handle many constraints on the same traj?
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies)-1
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans))
robot.Grab(body1)
mt = self.sample_motion_plan(mq1, mq2, bq)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, bq, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
mt = self.sample_motion_plan(mq1, mq2, bq)
if mt:
return [ManipMotion(mq1, mq2, bq, mt)]
return []
##################################################
cond_streams = [
#MultiEasyGenStream(inputs=[O], outputs=[P], conditions=[], effects=[LegalPose(O, P)], generator=sample_poses),
#EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
# effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=sample_arm_traj),
ClassStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
effects=[ManipMotion(MQ, MQ2, BQ, MT)], StreamClass=MotionStream, order=1, max_level=0),
#MultiEasyGenStream(inputs=[P, G, BQ], outputs=[MQ], conditions=[],
# effects=[Kin(P, G, BQ, MQ)], generator=sample_ik),
EasyListGenStream(inputs=[P, G, BQ], outputs=[MQ, GT], conditions=[],
effects=[GraspTraj(P, G, BQ, MQ, GT)], generator=sample_grasp_traj),
#EasyTestStream(inputs=[O, P, T], conditions=[], effects=[CFree(O, P, T)],
# test=collision_free, eager=True),
EasyTestStream(inputs=[P, P2], conditions=[], effects=[PoseCFree(P, P2)],
test=collision_free, eager=True),
#ClassStream(inputs=[P, P2], conditions=[], outputs=[],
# effects=[PoseCFree(P, P2)], StreamClass=CollisionStream, eager=True),
EasyTestStream(inputs=[MT, P], conditions=[], effects=[MTrajPoseCFree(MT, P)],
test=lambda mt, p: True, eager=True),
EasyTestStream(inputs=[MT, G], conditions=[], effects=[MTrajGraspCFree(MT, G)],
test=lambda mt, g: True, eager=True),
EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[MTrajGraspPoseCFree(MT, G, P)],
test=lambda mt, g, p: True, eager=True),
#ClassStream(inputs=[P], conditions=[], outputs=[],
# effects=[CheckedInitial(P)], StreamClass=EnvCollisionStream),
]
##################################################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_full = Config(get_full_config(robot))
initial_base = get_base_conf(initial_full)
initial_manip = get_arm_conf(manipulator, initial_full)
initial_atoms = [
BaseEq(initial_base),
ManipEq(initial_manip),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, operators, cond_streams, constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager')
#plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF
#plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF
from misc.profiling import run_profile, str_profile
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=True)
#with env:
env.Lock()
(plan, universe), prof = run_profile(solve)
env.Unlock()
print SEPARATOR
universe.print_domain_statistics()
universe.print_statistics()
print SEPARATOR
print str_profile(prof)
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i+1, action, args
else:
print 'Failure'
##################################################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
open_gripper2(manipulator)
set_base_values(robot, initial_base.value)
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move_arm':
mq1, mq2, bq, mt = args
#set_manipulator_values(manipulator, mq2.value)
step_path(mt)
elif action.name == 'pick':
#o, p, q, mq, bq = args
o, p, g, mq, bq, gt = args
step_path(gt.reverse())
#grasp_gripper2(manipulator, g) # NOTE - g currently isn't a real grasp
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
#o, p, q, mq, bq = args
o, p, g, mq, bq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
#open_gripper2(manipulator)
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_conf(robot, (-.75, .2, -math.pi / 2))
initialize_openrave(env, ARM, min_delta=.01)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
all_bodies = bodies.values()
assert len({body.GetKinematicsGeometryHash()
for body in all_bodies
}) == 1 # NOTE - assuming all objects has the same geometry
body1 = all_bodies[-1] # Generic object 1
body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH,
USE_GRASP_TYPE)[:MAX_GRASPS]
poses = problem.known_poses if problem.known_poses else []
open_gripper(manipulator)
initial_conf = Conf(
robot.GetConfigurationValues()[manipulator.GetArmIndices()])
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G],
outputs=[Q, T],
conditions=[],
effects=[GraspMotion(P, G, Q, T)],
generator=sample_grasp_traj_fn(
env, robot, manipulator, body1, all_bodies)),
# Move trajectory
EasyListGenStream(inputs=[Q, Q2],
outputs=[T],
conditions=[],
effects=[FreeMotion(Q, Q2, T)],
generator=sample_free_motion_fn(
manipulator, base_manip, cspace, all_bodies),
order=1,
max_level=0),
EasyListGenStream(inputs=[Q, Q2, G],
outputs=[T],
conditions=[],
effects=[HoldingMotion(Q, Q2, G, T)],
generator=sample_holding_motion_fn(
robot, manipulator, base_manip, cspace, body1,
all_bodies),
order=1,
max_level=0),
# Collisions
EasyTestStream(inputs=[P, P2],
conditions=[],
effects=[CFreePose(P, P2)],
test=cfree_pose_fn(env, body1, body2),
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=cfree_traj_fn(env, robot, manipulator, body1,
body2, all_bodies)),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ConfEq(initial_conf),
HandEmpty(),
] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()]
goal_formula = And(
ConfEq(initial_conf),
*(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
solve = lambda: incremental_planner(
stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
#solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=15)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if viewer and plan is not None:
print SEPARATOR
visualize_solution(env, problem, initial_conf, robot, manipulator,
bodies, plan)
raw_input('Finish?')
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi/2))
initialize_openrave(env, ARM)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()}
assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
open_gripper2(manipulator)
initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot)))
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def cfree_gtraj_pose(gt, p):
return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(gt, gt.grasp, p)
####################
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
print 'cfree_mtraj_grasp'
return False
return True
def cfree_mtraj_pose(mt, p):
enable_all(False)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
if env.CheckCollision(robot, body2):
print 'cfree_mtraj_pose'
return False
return True
def cfree_mtraj_grasp_pose(mt, g, p):
enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
print 'cfree_mtraj_grasp_pose'
return False
return True
####################
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_manip_motion(mq1, mq2):
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if not mt: return
yield [(mt,)]
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G], outputs=[MQ, GT], conditions=[],
effects=[GraspMotion(P, G, MQ, GT)], generator=sample_grasp_traj),
# Move trajectory
EasyListGenStream(inputs=[MQ, MQ2], outputs=[MT], conditions=[],
effects=[ManipMotion(MQ, MQ2, MT)], generator=sample_manip_motion, order=1, max_level=0),
# Pick/place collisions
EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePosePose(P, P2)],
test=cfree_pose_pose, eager=True),
EasyTestStream(inputs=[GT, P], conditions=[], effects=[CFreeGTrajPose(GT, P)],
test=cfree_gtraj_pose),
# Move collisions
EasyTestStream(inputs=[MT, P], conditions=[], effects=[CFreeMTrajPose(MT, P)],
test=cfree_mtraj_pose),
EasyTestStream(inputs=[MT, G], conditions=[], effects=[CFreeMTrajGrasp(MT, G)],
test=cfree_mtraj_grasp),
EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[CFreeMTrajGraspPose(MT, G, P)],
test=cfree_mtraj_grasp_pose),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ManipEq(initial_manip),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10)
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i+1, action, args
else:
print 'Failure'
####################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
mq1, mq2, mt = args
step_path(mt)
elif action.name == 'pick':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi / 2))
initialize_openrave(env, ARM)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {
body.GetKinematicsGeometryHash()
for body in bodies.values()
}
assert len(
geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH,
USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
open_gripper2(manipulator)
initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot)))
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def cfree_gtraj_pose(gt, p):
return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(
gt, gt.grasp, p)
####################
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def cfree_mtraj_pose(mt, p):
enable_all(False)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
if env.CheckCollision(body2):
return False
return True
def cfree_mtraj_grasp_pose(mt, g, p):
enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
####################
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
class MotionStream(Stream):
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2 = map(get_value, self.inputs)
collision_atoms = filter(
lambda atom: atom.predicate in
[CFreeMTrajGrasp, CFreeMTrajPose], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is CFreeMTrajGrasp:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is CFreeMTrajPose:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
#print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies)
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies) - 1
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
grasp.grasp_trans))
robot.Grab(body1)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [
atom.instantiate({collision_params[atom]: MTRAJ(mt)})
for atom in collision_atoms
]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if mt:
return [ManipMotion(mq1, mq2, mt)]
return []
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G],
outputs=[MQ, GT],
conditions=[],
effects=[GraspMotion(P, G, MQ, GT)],
generator=sample_grasp_traj),
# Move trajectory
ClassStream(inputs=[MQ, MQ2],
outputs=[MT],
conditions=[],
effects=[ManipMotion(MQ, MQ2, MT)],
StreamClass=MotionStream,
order=1,
max_level=0),
# Pick/place collisions
EasyTestStream(inputs=[P, P2],
conditions=[],
effects=[CFreePosePose(P, P2)],
test=cfree_pose_pose,
eager=True),
EasyTestStream(inputs=[GT, P],
conditions=[],
effects=[CFreeGTrajPose(GT, P)],
test=cfree_gtraj_pose),
# Move collisions
EasyTestStream(inputs=[MT, P],
conditions=[],
effects=[CFreeMTrajPose(MT, P)],
test=cfree_mtraj_pose),
EasyTestStream(inputs=[MT, G],
conditions=[],
effects=[CFreeMTrajGrasp(MT, G)],
test=cfree_mtraj_grasp),
EasyTestStream(inputs=[MT, G, P],
conditions=[],
effects=[CFreeMTrajGraspPose(MT, G, P)],
test=cfree_mtraj_grasp_pose),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ManipEq(initial_manip),
HandEmpty(),
] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()]
goal_formula = And(
ManipEq(initial_manip),
*(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager')
#plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF
#plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF
from misc.profiling import run_profile, str_profile
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False)
#with env:
env.Lock()
(plan, universe), prof = run_profile(solve)
env.Unlock()
print SEPARATOR
universe.print_domain_statistics()
universe.print_statistics()
print SEPARATOR
print str_profile(prof)
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?' % (j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?' % (i, len(plan)))
if action.name == 'move':
mq1, mq2, mt = args
step_path(mt)
elif action.name == 'pick':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
