def optimize_feature(learner, max_time=INF, **kwargs):
#features = read_json(get_feature_path(learner.func.skill))
feature_fn, attributes, pairs = generate_candidates(
learner.func.skill, **kwargs)
start_time = time.time()
world = ROSWorld(use_robot=False, sim_only=True)
saver = ClientSaver(world.client)
print('Optimizing over {} feature pairs'.format(len(pairs)))
best_pair, best_score = None, -INF # maximize
for pair in randomize(pairs): # islice
if max_time <= elapsed_time(start_time):
break
world.reset(keep_robot=False)
for name in pair:
world.perception.add_item(name)
feature = feature_fn(world, *pair)
parameter = next(
learner.parameter_generator(world,
feature,
min_score=best_score,
valid=True,
verbose=False), None)
if parameter is None:
continue
context = learner.func.context_from_feature(feature)
sample = learner.func.sample_from_parameter(parameter)
x = x_from_context_sample(context, sample)
#x = learner.func.x_from_feature_parameter(feature, parameter)
score_fn = learner.get_score_f(context, noise=False,
negate=False) # maximize
score = float(score_fn(x)[0, 0])
if best_score < score:
best_pair, best_score = pair, score
world.stop()
saver.restore()
print(
'Best pair: {} | Best score: {:.3f} | Pairs: {} | Time: {:.3f}'.format(
best_pair, best_score, len(pairs), elapsed_time(start_time)))
assert best_score is not None
# TODO: ensure the same parameter is used
return dict(zip(attributes, best_pair))
class ROSWorld(object):
def __init__(self,
sim_only=False,
visualize=False,
use_robot=True,
**kwargs):
self.simulation = sim_only
if not self.simulation:
import rospy
rospy.init_node(self.__class__.__name__, anonymous=False)
self.client_saver = None
with HideOutput():
self.client = connect(
visualize
) # TODO: causes assert (len(visual_data) == 1) error?
#self.client = p.connect(p.GUI if visualize else p.DIRECT)
p.setPhysicsEngineParameter(enableFileCaching=0,
physicsClientId=self.client)
p.setRealTimeSimulation(0, physicsClientId=self.client)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,
False,
physicsClientId=self.client)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS,
True,
physicsClientId=self.client)
self.pr2 = None
if use_robot:
self._create_robot()
self.initial_beads = {}
if sim_only:
self.perception = PBPerception(self, **kwargs)
self.controller = PBController(self)
self.alternate_controller = None
else:
from perception_tools.retired.ros_perception import ROSPerception
from control_tools.retired.ros_controller import ROSController
self.perception = ROSPerception(self)
self.controller = ROSController(self)
self.alternate_controller = PBController(self)
self.sync_controllers()
@property
def robot(self):
return self.pr2
def _create_robot(self):
with ClientSaver(self.client):
with HideOutput():
pr2_path = os.path.join(get_models_path(), PR2_URDF)
self.pr2 = load_pybullet(pr2_path, fixed_base=True)
# Base link is the origin
base_pose = get_link_pose(self.robot,
link_from_name(self.robot, BASE_FRAME))
set_pose(self.robot, invert(base_pose))
return self.pr2
def get_body(self, name):
return self.perception.get_body(name)
def get_pose(self, name):
return self.perception.get_pose(name)
def sync_controllers(self):
if self.alternate_controller is None:
return
joints1 = self.controller.get_joint_names()
joints2 = set(self.alternate_controller.get_joint_names())
common_joints = [j for j in joints1 if j in joints2]
positions = self.controller.get_joint_positions(common_joints)
self.alternate_controller.set_joint_positions(common_joints, positions)
def reset(self, keep_robot=False):
# Avoid using remove_body(body)
# https://github.com/bulletphysics/bullet3/issues/2086
self.controller.reset()
self.initial_beads = {}
with ClientSaver(self.client):
for name in list(self.perception.sim_bodies):
self.perception.remove(name)
for constraint in get_constraints():
remove_constraint(constraint)
remove_all_debug()
reset_simulation()
if keep_robot:
self._create_robot()
def stop(self):
self.reset(keep_robot=False)
with ClientSaver(self.client):
disconnect()
def __enter__(self):
# TODO: be careful about the nesting of these
self.client_saver = ClientSaver(self.client)
def __exit__(self, exc_type, exc_val, exc_tb):
assert self.client_saver is not None
self.client_saver.restore()
self.client_saver = None
class PlanningWorld(object):
def __init__(self, task, use_robot=True, visualize=False, **kwargs):
self.task = task
self.real_world = None
self.visualize = visualize
self.client_saver = None
self.client = connect(use_gui=visualize, **kwargs)
print('Planner connected to client {}.'.format(self.client))
self.robot = None
with ClientSaver(self.client):
with HideOutput():
if use_robot:
self.robot = load_pybullet(os.path.join(
get_models_path(), PR2_URDF),
fixed_base=True)
#dump_body(self.robot)
#compute_joint_weights(self.robot)
self.world_name = 'world'
self.world_pose = Pose(unit_pose())
self.bodies = {}
self.fixed = []
self.surfaces = []
self.items = []
#self.holding_liquid = []
self.initial_config = None
self.initial_poses = {}
self.body_mapping = {}
@property
def arms(self):
return self.task.arms
@property
def initial_grasps(self):
return self.task.init_holding
def get_body(self, name):
return self.bodies[name]
def get_table(self):
for name in self.bodies:
if is_obj_type(name, TABLE):
return name
return None
def get_obstacles(self):
return [name for name in self.bodies if 'obstacle' in name]
def get_fixed(self): # TODO: make any non-movable object fixed
movable = self.get_movable()
return sorted(name for name in self.bodies if name not in movable)
def get_movable(self):
return sorted(
filter(lambda item: in_type_group(item, self.task.movable),
self.bodies))
def get_held(self):
return {grasp.obj_name for grasp in self.initial_grasps.values()}
def set_initial(self):
with ClientSaver(self.client):
set_configuration(self.robot, self.initial_config)
for name, pose in self.initial_poses.items():
set_pose(self.get_body(name), pose)
# TODO: set the initial grasps?
def _load_robot(self, real_world):
with ClientSaver(self.client):
# TODO: set the x,y,theta joints using the base pose
pose = get_pose(self.robot) # base_link is origin
base_pose = get_link_pose(self.robot,
link_from_name(self.robot, BASE_FRAME))
set_pose(self.robot, multiply(invert(base_pose), pose))
# base_pose = real_world.controller.return_cartesian_pose(arm='l')
# Because the robot might have an xyz
movable_names = real_world.controller.get_joint_names()
movable_joints = [
joint_from_name(self.robot, name) for name in movable_names
]
movable_positions = real_world.controller.get_joint_positions(
movable_names)
set_joint_positions(self.robot, movable_joints, movable_positions)
self.initial_config = get_configuration(self.robot)
self.body_mapping[self.robot] = real_world.robot
# TODO(caelan): can also directly access available joints
def add_body(self, name, fixed=True):
self.bodies[name] = load_pybullet(get_body_urdf(name),
fixed_base=fixed)
return self.bodies[name]
def _load_bodies(self, real_world):
with ClientSaver(self.client):
assert not self.bodies
#for real_body in self.bodies.values():
# remove_body(real_body)
self.bodies = {}
self.initial_poses = {}
with HideOutput():
for name, real_body in real_world.perception.sim_bodies.items(
):
if name in real_world.perception.get_surfaces():
self.add_body(name, fixed=True)
elif name in real_world.perception.get_items():
with real_world:
model_info = get_model_info(real_body)
# self.bodies[item] = clone_body(real_world.perception.get_body(item), client=self.client)
if 'wall' in name:
with real_world:
self.bodies[name] = clone_body(
real_body, client=self.client)
elif 'cylinder' in name:
self.bodies[name] = create_cylinder(
*model_info.path)
elif 'box' in name:
self.bodies[name] = create_box(*model_info.path)
elif model_info is None:
self.add_body(name, fixed=False)
else:
self.bodies[name] = load_model_info(model_info)
else:
raise ValueError(name)
# TODO: the floor might not be added which causes the indices to misalign
self.body_mapping[self.bodies[name]] = real_body
if name not in self.get_held():
self.initial_poses[name] = Pose(
real_world.perception.get_pose(name))
self.set_initial()
def load(self, real_world): # Avoids reloading the robot
self.fixed = tuple(real_world.perception.sim_surfaces.keys())
self.surfaces = tuple(filter(is_surface, self.fixed))
self.items = tuple(real_world.perception.sim_items.keys())
self.body_mapping = {}
#self.holding_liquid = tuple(real_world.perception.holding_liquid)
#config = get_configuration(real_world.pr2)
self._load_robot(real_world)
self._load_bodies(real_world)
self.real_world = real_world
def stop(self):
with ClientSaver(self.client):
reset_simulation()
disconnect()
def __enter__(self):
self.client_saver = ClientSaver(self.client)
def __exit__(self, exc_type, exc_val, exc_tb):
assert self.client_saver is not None
self.client_saver.restore()
self.client_saver = None
def __repr__(self):
return '{}(fixed={},movable={})'.format(self.__class__.__name__,
self.get_fixed(),
self.get_movable())