def __init__(self):
super().__init__('victor')
self.service_provider = GazeboServices()
# register a new callback to stop when the rope is overstretched
self.set_rope_end_points_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "set"), Position3DAction)
def main():
colorama.init(autoreset=True)
plt.style.use("slides")
np.set_printoptions(precision=3, suppress=True, linewidth=200)
parser = argparse.ArgumentParser(formatter_class=my_formatter)
parser.add_argument("fwd_model_dir",
help="load this saved forward model file",
type=pathlib.Path,
nargs='+')
parser.add_argument("test_config",
help="json file describing the test",
type=pathlib.Path)
parser.add_argument("labeling_params",
help='labeling params',
type=pathlib.Path)
args = parser.parse_args()
rospy.init_node('test_model_from_gazebo')
test_config = json.load(args.test_config.open("r"))
labeling_params = json.load(args.labeling_params.open("r"))
labeling_state_key = labeling_params['state_key']
# read actions from config
actions = [numpify(a) for a in test_config['actions']]
n_actions = len(actions)
time_steps = np.arange(n_actions + 1)
fwd_model, _ = dynamics_utils.load_generic_model(args.fwd_model_dir)
service_provider = GazeboServices()
service_provider.setup_env(
verbose=0,
real_time_rate=0,
max_step_size=fwd_model.data_collection_params['max_step_size'])
environment = fwd_model.scenario.get_environment(
params=fwd_model.data_collection_params)
start_state = fwd_model.scenario.get_state()
start_state = make_dict_tf_float32(start_state)
start_states = [start_state]
expanded_actions = [[actions]]
predicted_states = predict(fwd_model, environment, start_states,
expanded_actions, n_actions, 1, 1)
scenario = fwd_model.scenario
actual_states_lists = execute(service_provider, scenario, start_states,
expanded_actions)
visualize(scenario, environment, actual_states_lists, actions,
predicted_states, labeling_state_key, time_steps)
def __init__(self):
super().__init__()
self.service_provider = GazeboServices()
self.joint_state_viz_pub = rospy.Publisher("joint_states_viz", JointState, queue_size=10)
self.goto_home_srv = rospy.ServiceProxy("goto_home", Empty)
self.robot = get_moveit_robot()
def test_occupancy(self):
rospy.init_node('test_occupancy')
service_provider = GazeboServices()
movable_objects_services = {
"moving_box1": make_movable_object_services("moving_box1")
}
xs = [-1, 1, 1, -1, -1]
ys = [-1, -1, 1, 1, -1]
res = 0.1
extent = [-1, 1, -1, 1, 0, 1]
for x_i, y_i in zip(xs, ys):
object_positions = {"moving_box1": [x_i, y_i]}
ExperimentScenario.move_objects_to_positions(
movable_objects_services, object_positions, timeout=150)
environment = get_environment_for_extents_3d(
extent=extent,
res=res,
service_provider=service_provider,
robot_name="test")
# scale down to avoid out of bounds on the edges
row_i, col_i, channel_i = point_to_idx_3d_in_env(
x=0.99 * x_i, y=0.99 * y_i, z=0.01, environment=environment)
occupied = environment['env'][row_i, col_i, channel_i] > 0
self.assertTrue(occupied)
row_i, col_i, channel_i = point_to_idx_3d_in_env(
x=0 * x_i, y=0 * y_i, z=0.01, environment=environment)
occupied = environment['env'][row_i, col_i, channel_i] > 0
self.assertFalse(occupied)
def get_service_provider(service_provider_name):
if service_provider_name == 'victor':
from victor.victor_services import VictorServices
return VictorServices()
elif service_provider_name == 'gazebo':
from link_bot_gazebo_python.gazebo_services import GazeboServices
return GazeboServices()
else:
raise NotImplementedError()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('dir', type=pathlib.Path)
args = parser.parse_args()
rospy.init_node('filter_scenes')
link_states_listener = Listener("/gazebo/link_states", LinkStates)
bagfile_names = list(args.dir.iterdir())
bagfile_names = sorted(bagfile_names)
for bagfile_name in bagfile_names:
if bagfile_name.suffix == '.bag':
index = int(bagfile_name.stem[-4:])
srv_name = "gazebo/set_link_state"
rospy.loginfo(f"waiting for service {srv_name}")
set_srv = rospy.ServiceProxy(srv_name, SetLinkState)
set_srv.wait_for_service()
gazebo_service_provider = GazeboServices()
gazebo_service_provider.restore_from_bag(bagfile_name)
gazebo_service_provider.play()
r = input(f"{index:4d}: fix? [N/y]")
if r == 'Y' or r == 'y':
# let the use fix it
input("press enter when you're done fixing")
links_states = link_states_listener.get()
bagfile_name = args.dir / f'scene_{index:04d}.bag'
rospy.loginfo(f"Saving scene to {bagfile_name}")
with rosbag.Bag(bagfile_name, 'w') as bag:
bag.write('links_states', links_states)
def main():
colorama.init(autoreset=True)
parser = argparse.ArgumentParser()
parser.add_argument("recovery_dataset_dir",
type=pathlib.Path,
help='the hparams.json file for the recovery dataset')
args = parser.parse_args()
scenario = FloatingRopeScenario()
service_provider = GazeboServices()
with args.recovery_dataset_dir.open("r") as hparams_file:
hparams = json.load(hparams_file)
data_collection_params = hparams['data_collection_params']
data = []
while True:
state = scenario.get_state()
environment = get_environment_for_extents_3d(
extent=data_collection_params['extent'],
res=data_collection_params['res'],
service_provider=service_provider,
robot_name=scenario.robot_name())
example = {'state': state, 'environment': environment}
data.append(example)
k = input("state/environment saved. collect another? [y/N]")
if k != 'y':
break
now = int(time.time())
outfilename = f'state_and_environment_{now}.json.gz'
print(f"writing to {outfilename}")
dummy_proof_write(data, outfilename)
def main():
colorama.init(autoreset=True)
parser = argparse.ArgumentParser()
parser.add_argument("bagfile")
args = parser.parse_args()
srv_name = "gazebo/set_link_state"
rospy.loginfo(f"waiting for service {srv_name}")
set_srv = rospy.ServiceProxy(srv_name, SetLinkState)
set_srv.wait_for_service()
rospy.loginfo("resetting gazebo from bag file")
gazebo_service_provider = GazeboServices()
gazebo_service_provider.restore_from_bag(args.bagfile)
gazebo_service_provider.play()
rospy.loginfo("done")
class SimDualArmRopeScenario(BaseDualArmRopeScenario):
def __init__(self):
super().__init__('victor')
self.service_provider = GazeboServices()
# register a new callback to stop when the rope is overstretched
self.set_rope_end_points_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "set"), Position3DAction)
def execute_action(self, action: Dict):
return dual_arm_rope_execute_action(self.robot, action)
def on_before_data_collection(self, params: Dict):
super().on_before_data_collection(params)
# register kinematic controllers for fake-grasping
self.register_fake_grasping()
# Mark the rope as a not-obstacle
exclude = ExcludeModelsRequest()
exclude.model_names.append("rope_3d")
self.exclude_from_planning_scene_srv(exclude)
# move to init positions
self.robot.plan_to_joint_config("both_arms",
params['reset_joint_config'])
# Grasp the rope again
self.grasp_rope_endpoints()
def on_before_get_state_or_execute_action(self):
self.robot.connect()
def randomize_environment(self, env_rng: np.random.RandomState,
params: Dict):
# teleport movable objects out of the way
self.move_objects_out_of_scene(params)
# release the rope
self.robot.open_left_gripper()
self.detach_rope_from_gripper('left_gripper')
# plan to reset joint config, we assume this will always work
self.robot.plan_to_joint_config("both_arms",
params['reset_joint_config'])
# Grasp the rope again
self.grasp_rope_endpoints()
# randomize the object configurations
random_object_poses = self.random_new_object_poses(env_rng, params)
self.set_object_poses(random_object_poses)
def grasp_rope_endpoints(self):
self.robot.open_left_gripper()
self.robot.open_right_gripper()
self.service_provider.pause()
self.make_rope_endpoints_follow_gripper()
self.service_provider.play()
rospy.sleep(5)
self.robot.close_left_gripper()
self.robot.close_right_gripper()
self.reset_cdcpd()
def move_rope_out_of_the_scene(self):
set_req = Position3DActionRequest()
set_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
"left_gripper")
set_req.position.x = 1.3
set_req.position.y = 0.3
set_req.position.z = 1.3
self.pos3d.set(set_req)
set_req = Position3DActionRequest()
set_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
"right_gripper")
set_req.position.x = 1.3
set_req.position.y = -0.3
set_req.position.z = 1.3
self.pos3d.set(set_req)
def detach_rope_from_gripper(self, rope_link_name: str):
enable_req = Position3DEnableRequest()
enable_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
rope_link_name)
enable_req.enable = False
self.pos3d.enable(enable_req)
def detach_rope_from_grippers(self):
self.detach_rope_from_gripper('left_gripper')
self.detach_rope_from_gripper('right_gripper')
def move_objects_out_of_scene(self, params: Dict):
position = [0, 2, 0]
orientation = [0, 0, 0, 1]
out_of_scene_object_poses = {
k: (position, orientation)
for k in params['objects']
}
self.set_object_poses(out_of_scene_object_poses)
def before_restore(self):
unload = rospy.ServiceProxy(
"/victor/controller_manager/switch_controller", SwitchController)
unload(
SwitchControllerRequest(
stop_controllers=["both_arms_trajectory_controller"]))
def after_restore(self):
pass
def test_classifier(classifier_model_dir: pathlib.Path,
fwd_model_dir: List[pathlib.Path],
n_actions: int,
saved_state: Optional[pathlib.Path],
generate_actions: Callable):
fwd_model, _ = dynamics_utils.load_generic_model([pathlib.Path(p) for p in fwd_model_dir])
classifier: NNClassifierWrapper = classifier_utils.load_generic_model([classifier_model_dir])
service_provider = GazeboServices()
service_provider.setup_env(verbose=0,
real_time_rate=0,
max_step_size=fwd_model.data_collection_params['max_step_size'],
play=False)
if saved_state:
service_provider.restore_from_bag(saved_state)
scenario = fwd_model.scenario
scenario.on_before_get_state_or_execute_action()
# NOTE: perhaps it would make sense to have a "fwd_model" have API for get_env, get_state, sample_action, etc
# because the fwd_model knows it's scenario, and importantly it also knows it's data_collection_params
# which is what we're using here to pass to the scenario methods
params = fwd_model.data_collection_params
environment = numpify(scenario.get_environment(params))
start_state = numpify(scenario.get_state())
start_state_tiled = repeat(start_state, n_actions, axis=0, new_axis=True)
start_states_tiled = add_time_dim(start_state_tiled)
actions = generate_actions(environment, start_state_tiled, scenario, params, n_actions)
environment_tiled = repeat(environment, n_actions, axis=0, new_axis=True)
actions_dict = sequence_of_dicts_to_dict_of_tensors(actions)
actions_dict = add_time_dim(actions_dict)
predictions, _ = fwd_model.propagate_differentiable_batched(environment=environment_tiled,
state=start_states_tiled,
actions=actions_dict)
# Run classifier
state_sequence_length = 2
accept_probabilities, _ = classifier.check_constraint_batched_tf(environment=environment_tiled,
predictions=predictions,
actions=actions_dict,
state_sequence_length=state_sequence_length,
batch_size=n_actions)
# animate over the sampled actions
anim = RvizAnimation(scenario=scenario,
n_time_steps=n_actions,
init_funcs=[init_viz_env],
t_funcs=[
lambda s, e, t: init_viz_env(s, e),
viz_transition_for_model_t({}, fwd_model),
ExperimentScenario.plot_accept_probability_t,
],
)
example = {
'accept_probability': tf.squeeze(accept_probabilities, axis=1),
}
example.update(environment)
example.update(predictions)
example.update(actions_dict)
anim.play(example)