def example(): task = generate_task(task_generator_id='pick_and_place') env = CausalWorld(task=task, enable_visualization=True) env.reset() intervention_space = env.get_variable_space_used() for _ in range(100): for i in range(200): obs, reward, done, info = env.step(env.action_space.low) intervention = { 'tool_block': { 'size': np.random.uniform(intervention_space['tool_block']['size'][0], intervention_space['tool_block']['size'][1]) } } env.do_intervention(intervention) env.close()
def privileged_information(): task = generate_task(task_generator_id='pushing') env = CausalWorld(task=task, enable_visualization=True) env.expose_potential_partial_solution() env.reset() for _ in range(10): goal_intervention_dict = env.sample_new_goal() success_signal, obs = env.do_intervention(goal_intervention_dict) print("Goal Intervention success signal", success_signal) for i in range(1000): obs, reward, done, info = env.step(env.action_space.low) print("now we solve it with privileged info") success_signal, obs = env.do_intervention( info['possible_solution_intervention'], check_bounds=False) print("Partial Solution Setting Intervention Succes Signal", success_signal) for i in range(500): obs, reward, done, info = env.step(env.action_space.low) env.close()
def goal_interventions(): task = generate_task(task_generator_id='stacked_blocks') env = CausalWorld(task=task, enable_visualization=True) env.reset() for _ in range(10): for i in range(200): obs, reward, done, info = env.step(env.action_space.sample()) goal_intervention_dict = env.sample_new_goal() print("new goal chosen: ", goal_intervention_dict) success_signal, obs = env.do_intervention(goal_intervention_dict) print("Goal Intervention success signal", success_signal) env.close()
def without_intervention_split(): task = generate_task(task_generator_id='pushing') env = CausalWorld(task=task, enable_visualization=True) env.reset() for _ in range(2): for i in range(200): obs, reward, done, info = env.step(env.action_space.sample()) success_signal, obs = env.do_intervention( {'stage_color': np.random.uniform(0, 1, [ 3, ])}) print("Intervention success signal", success_signal) env.close()
def example(): task = generate_task(task_generator_id='pushing') env = CausalWorld(task=task, enable_visualization=True) env.reset() counter = 0 for _ in range(1): for i in range(210): obs, reward, done, info = env.step(env.action_space.low) if i % 50 == 0 and i > 0: print(i) intervention = {'goal_block': {'cartesian_position': [0, -0.08+(0.04*counter), 0.0325], 'color':[0, 0, 1]}} env.do_intervention(intervention, check_bounds=False) counter += 1 print("intervention") if i == 201: intervention = {'goal_block': { 'cartesian_position': [0, 0.08, 0.0325], 'color': [0, 1, 0]}} env.do_intervention(intervention, check_bounds=False) env.close()
def end_effector_pos(): task = generate_task(task_generator_id='reaching') env = CausalWorld(task=task, enable_visualization=True, action_mode="joint_positions", normalize_actions=False, normalize_observations=False) obs = env.reset() for _ in range(100): goal_dict = env.sample_new_goal() success_signal, obs = env.do_intervention(goal_dict) obs, reward, done, info = env.step(control_policy(env, obs)) for _ in range(250): obs, reward, done, info = env.step(control_policy(env, obs)) env.close()
def with_intervention_split_2(): task = generate_task(task_generator_id='pushing', variables_space='space_b') env = CausalWorld(task=task, enable_visualization=False) interventions_space = task.get_intervention_space_a() env.reset() for _ in range(2): for i in range(200): obs, reward, done, info = env.step(env.action_space.sample()) success_signal, obs = env.do_intervention({ 'stage_color': np.random.uniform(interventions_space['stage_color'][0], interventions_space['stage_color'][1]) }) print("Intervention success signal", success_signal) env.close()
class TestPicking(unittest.TestCase): def setUp(self): self.task = generate_task(task_generator_id="picking") self.env = CausalWorld(task=self.task, enable_visualization=False, skip_frame=1, action_mode="end_effector_positions", normalize_actions=False, normalize_observations=False) return def tearDown(self): self.env.close() return def test_determinism(self): self.env.set_action_mode('joint_positions') observations_1 = [] rewards_1 = [] horizon = 2000 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) obs = self.env.reset() observations_1.append(obs) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] obs = self.env.reset() observations_2.append(obs) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) if not np.array_equal(observations_1[i], observations_2[i]): print(observations_1[i] - observations_2[i]) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def lift_last_finger_first(self, current_obs): desired_action = current_obs[19:19 + 9] desired_action[6:] = [-0, -0.08, 0.4] for _ in range(250): obs, reward, done, info = self.env.step(desired_action) return desired_action def move_first_two_fingers(self, current_obs): desired_action = current_obs[19:19 + 9] desired_action[:6] = [ 0., 0.15313708, 0.05586292, 0.13262061, -0.07656854, 0.05586292 ] for _ in range(250): obs, reward, done, info = self.env.step(desired_action) return obs def grip_block(self): grip_locations = get_suggested_grip_locations( self.env._task._stage.get_object('tool_block').get_size(), self.env._task._stage.get_object( 'tool_block').world_to_cube_r_matrix()) desired_action = np.zeros(9) desired_action[6:] = [-0, -0.08, 0.4] desired_action[:3] = grip_locations[0] desired_action[3:6] = grip_locations[1] # grasp the block now for _ in range(250): obs, reward, done, info = self.env.step(desired_action) return desired_action def lift_block(self, desired_grip): desired_action = desired_grip for _ in range(40): desired_action[2] += 0.005 desired_action[5] += 0.005 for _ in range(10): obs, reward, done, info = self.env.step(desired_action) return obs def test_02_mass(self): self.env.set_action_mode('end_effector_positions') intervention = {'tool_block': {'mass': 0.02}} self.env.do_intervention(interventions_dict=intervention) for _ in range(1): obs = self.env.reset() obs = self.move_first_two_fingers(obs) self.lift_last_finger_first(obs) desired_grip = self.grip_block() self.assertEqual(self.env.get_robot().get_tip_contact_states(), [1, 1, 0], "contact states are not closed") final_obs = self.lift_block(desired_grip) self.assertGreater(final_obs[-22], 0.2, "the block didn't get lifted") def test_08_mass(self): self.env.set_action_mode('end_effector_positions') intervention = {'tool_block': {'mass': 0.08}} self.env.do_intervention(interventions_dict=intervention) for _ in range(1): obs = self.env.reset() obs = self.move_first_two_fingers(obs) self.lift_last_finger_first(obs) desired_grip = self.grip_block() self.assertEqual(self.env.get_robot().get_tip_contact_states(), [1, 1, 0], "contact states are not closed") final_obs = self.lift_block(desired_grip) self.assertGreater(final_obs[-22], 0.2, "the block didn't get lifted") def test_1_mass(self): self.env.set_action_mode('end_effector_positions') intervention = {'tool_block': {'mass': 0.1}} self.env.do_intervention(interventions_dict=intervention) for _ in range(1): obs = self.env.reset() obs = self.move_first_two_fingers(obs) self.lift_last_finger_first(obs) desired_grip = self.grip_block() self.assertEqual(self.env.get_robot().get_tip_contact_states(), [1, 1, 0], "contact states are not closed") final_obs = self.lift_block(desired_grip) self.assertGreater(final_obs[-22], 0.2, "the block didn't get lifted") def test_determinism_w_interventions(self): self.env.set_action_mode('joint_positions') observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) new_goal = self.env.sample_new_goal() self.env.set_starting_state(interventions_dict=new_goal) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_determinism_w_in_episode_interventions(self): self.env.set_action_mode('joint_positions') observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) #now we will restart again and perform an in epsiode intervention self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) if i == 50: success_signal = self.env.do_intervention({ 'tool_block': { 'cylindrical_position': [0.1, np.pi / 2, 0.0325] } }) observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) if not np.array_equal(observations_1[i], observations_2[i]): print(observations_1[i] - observations_2[i]) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_goal_intervention(self): task = generate_task(task_generator_id='picking') env = CausalWorld(task=task, enable_visualization=False, normalize_observations=False) for _ in range(10): invalid_interventions_before = env.get_tracker( ).invalid_intervention_steps new_goal = env.sample_new_goal() env.set_starting_state(interventions_dict=new_goal) invalid_interventions_after = env.get_tracker( ).invalid_intervention_steps for _ in range(2): for _ in range(100): obs, reward, done, info = env.step(env.action_space.low) #TODO: this shouldnt be the case when the benchmark is complete #Its a hack for now if invalid_interventions_before == invalid_interventions_after: assert np.array_equal( cyl2cart(new_goal['goal_block'] ['cylindrical_position']), obs[-7:-4]) env.reset() env.close()
class TestCreativeStackedBlocks(unittest.TestCase): def setUp(self): self.task = generate_task(task_generator_id="creative_stacked_blocks") self.env = CausalWorld(task=self.task, enable_visualization=False) return def tearDown(self): self.env.close() return def test_determinism(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) obs = self.env.reset() observations_1.append(obs) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] obs = self.env.reset() observations_2.append(obs) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) if not np.array_equal(observations_1[i], observations_2[i]): print( np.array(observations_1[i]) - np.array(observations_2[i])) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_determinism_w_interventions(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) new_goal = self.env.sample_new_goal() self.env.set_starting_state(interventions_dict=new_goal) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) if not np.array_equal(observations_1[i], observations_2[i]): print( np.array(observations_1[i]) - np.array(observations_2[i])) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_determinism_w_in_episode_interventions(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) #now we will restart again and perform an in epsiode intervention self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) if i == 50: success_signal = self.env.do_intervention( {'tool_level_0_num_1': { 'cylindrical_position': [0, 0, 0.2] }}) observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2
class TestReaching(unittest.TestCase): def setUp(self): self.task = generate_task(task_generator_id="reaching") self.env = CausalWorld(task=self.task, enable_visualization=False, action_mode='joint_positions', normalize_observations=False, normalize_actions=False) return def tearDown(self): self.env.close() return # def test_forward_kinemetics(self): # horizon = 100 # obs = self.env.reset() # desired_goal = obs[-9:] # for i in range(horizon): # obs, reward, done, info = self.env.step(desired_goal) # print(obs[19:28]) def test_determinism(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) obs = self.env.reset() observations_1.append(obs) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] obs = self.env.reset() observations_2.append(obs) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) if not np.array_equal(observations_1[i], observations_2[i]): print( np.array(observations_1[i]) - np.array(observations_2[i])) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_determinism_w_interventions(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) new_goal = self.env.sample_new_goal() self.env.set_starting_state(interventions_dict=new_goal) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_determinism_w_in_episode_interventions(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) #now we will restart again and perform an in epsiode intervention self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) if i == 50: new_goal = self.env.sample_new_goal() success_signal = self.env.do_intervention(new_goal) observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2
class TestPushing(unittest.TestCase): def setUp(self): self.task = generate_task(task_generator_id="pushing") self.env = CausalWorld(task=self.task, enable_visualization=False) return def tearDown(self): self.env.close() return def test_determinism(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) obs = self.env.reset() observations_1.append(obs) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] obs = self.env.reset() observations_2.append(obs) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) if not np.array_equal(observations_1[i], observations_2[i]): print( np.array(observations_1[i]) - np.array(observations_2[i])) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_determinism_w_interventions_1(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) new_goal = self.env.sample_new_goal() self.env.set_starting_state(interventions_dict=new_goal) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) if not np.array_equal(observations_1[i], observations_2[i]): print(i) print( np.array(observations_1[i]) - np.array(observations_2[i])) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_determinism_w_interventions_2(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) intervention = { 'tool_block': { 'cylindrical_position': [0, 0.3, 0.0325] } } self.env.set_starting_state(interventions_dict=intervention) for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) for _ in range(10): observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) if not np.array_equal(observations_1[i], observations_2[i]): print(i) print( np.array(observations_1[i]) - np.array(observations_2[i])) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_determinism_w_in_episode_interventions(self): observations_1 = [] rewards_1 = [] horizon = 100 actions = [self.env.action_space.sample() for _ in range(horizon)] actions = np.array(actions) self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_1.append(obs) rewards_1.append(reward) #now we will restart again and perform an in epsiode intervention self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) if i == 50: success_signal = self.env.do_intervention({ 'tool_block': { 'cylindrical_position': [0.1, np.pi / 2, 0.0325] } }) observations_2 = [] rewards_2 = [] self.env.reset() for i in range(horizon): obs, reward, done, info = self.env.step(actions[i]) observations_2.append(obs) rewards_2.append(reward) assert np.array_equal(observations_1[i], observations_2[i]) assert rewards_1 == rewards_2 def test_goal_intervention(self): task = generate_task(task_generator_id='pushing') env = CausalWorld(task=task, enable_visualization=False, normalize_observations=False) for _ in range(10): invalid_interventions_before = env.get_tracker( ).invalid_intervention_steps new_goal = env.sample_new_goal() env.set_starting_state(interventions_dict=new_goal) invalid_interventions_after = env.get_tracker( ).invalid_intervention_steps for _ in range(2): for _ in range(100): obs, reward, done, info = env.step(env.action_space.low) #TODO: this shouldnt be the case when the benchmark is complete #Its a hack for now if invalid_interventions_before == invalid_interventions_after: assert np.array_equal( cyl2cart(new_goal['goal_block'] ['cylindrical_position']), obs[-7:-4]) env.reset() env.close()