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)