Skip to content

Commit

Permalink
partially fixed getting position bug
Browse files Browse the repository at this point in the history
  • Loading branch information
@jmatejcz
jmatejcz committed Feb 20, 2025
1 parent 3508158 commit a33e500
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 60 deletions.
104 changes: 52 additions & 52 deletions src/rai_bench/rai_bench/main.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,60 +94,60 @@
connector_path = configs_dir + "o3de_config.yaml"
#### Create scenarios manually
# load different scenes
# one_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
# base_config_path=Path(configs_dir + "scene1.yaml"),
# connector_config_path=Path(connector_path),
# )
# multiple_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
# base_config_path=Path(configs_dir + "scene2.yaml"),
# connector_config_path=Path(connector_path),
# )
# red_cubes_simulation_config = O3DExROS2SimulationConfig.load_config(
# base_config_path=Path(configs_dir + "scene3.yaml"),
# connector_config_path=Path(connector_path),
# )
# multiple_cubes_simulation_config = O3DExROS2SimulationConfig.load_config(
# base_config_path=Path(configs_dir + "scene4.yaml"),
# connector_config_path=Path(connector_path),
# )
# # combine different scene configs with the tasks to create various scenarios
# scenarios = [
# Scenario(
# task=GrabCarrotTask(logger=bench_logger),
# simulation_config=one_carrot_simulation_config,
# ),
# Scenario(
# task=GrabCarrotTask(logger=bench_logger),
# simulation_config=multiple_carrot_simulation_config,
# ),
# Scenario(
# task=PlaceCubesTask(logger=bench_logger),
# simulation_config=red_cubes_simulation_config,
# ),
# Scenario(
# task=PlaceCubesTask(logger=bench_logger),
# simulation_config=multiple_cubes_simulation_config,
# ),
# ]
one_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
base_config_path=Path(configs_dir + "scene1.yaml"),
connector_config_path=Path(connector_path),
)
multiple_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
base_config_path=Path(configs_dir + "scene2.yaml"),
connector_config_path=Path(connector_path),
)
red_cubes_simulation_config = O3DExROS2SimulationConfig.load_config(
base_config_path=Path(configs_dir + "scene3.yaml"),
connector_config_path=Path(connector_path),
)
multiple_cubes_simulation_config = O3DExROS2SimulationConfig.load_config(
base_config_path=Path(configs_dir + "scene4.yaml"),
connector_config_path=Path(connector_path),
)
# combine different scene configs with the tasks to create various scenarios
scenarios = [
Scenario(
task=GrabCarrotTask(logger=bench_logger),
simulation_config=one_carrot_simulation_config,
),
# Scenario(
# task=GrabCarrotTask(logger=bench_logger),
# simulation_config=multiple_carrot_simulation_config,
# ),
# Scenario(
# task=PlaceCubesTask(logger=bench_logger),
# simulation_config=red_cubes_simulation_config,
# ),
# Scenario(
# task=PlaceCubesTask(logger=bench_logger),
# simulation_config=multiple_cubes_simulation_config,
# ),
]

### Create scenarios automatically
scene_paths = [
configs_dir + "scene1.yaml",
configs_dir + "scene2.yaml",
configs_dir + "scene3.yaml",
configs_dir + "scene4.yaml",
]
simulations_configs = [
O3DExROS2SimulationConfig.load_config(Path(path), Path(connector_path))
for path in scene_paths
]
tasks = [
GrabCarrotTask(logger=bench_logger),
PlaceCubesTask(logger=bench_logger),
]
scenarios = Benchmark.create_scenarios(
tasks=tasks, simulation_configs=simulations_configs
)
# scene_paths = [
# configs_dir + "scene1.yaml",
# configs_dir + "scene2.yaml",
# configs_dir + "scene3.yaml",
# configs_dir + "scene4.yaml",
# ]
# simulations_configs = [
# O3DExROS2SimulationConfig.load_config(Path(path), Path(connector_path))
# for path in scene_paths
# ]
# tasks = [
# GrabCarrotTask(logger=bench_logger),
# PlaceCubesTask(logger=bench_logger),
# ]
# scenarios = Benchmark.create_scenarios(
# tasks=tasks, simulation_configs=simulations_configs
# )

# custom request to arm
base_arm_pose = PoseModel(translation=Translation(x=0.3, y=0.0, z=0.5))
Expand Down
9 changes: 3 additions & 6 deletions src/rai_bench/rai_bench/o3de_test_bench/tasks/grab_carrot_task.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ def calculate_result(self, simulation_bridge: SimulationBridge) -> float:
initially_correct_now_incorrect = 0 # when the object which was in the correct place at the start, is in a incorrect place at the end

scene_state = simulation_bridge.get_scene_state()

self.logger.info(f"ENTITIES: {scene_state.entities}")
initial_carrots = self.filter_entities_by_prefab_type(
simulation_bridge.spawned_entities, prefab_types=["carrot"]
)
Expand All @@ -50,15 +50,14 @@ def calculate_result(self, simulation_bridge: SimulationBridge) -> float:
)
num_initial_carrots = len(initial_carrots)

self.logger.info(initial_carrots)
self.logger.info(final_carrots)
if num_initial_carrots != len(final_carrots):
raise EntitiesMismatchException(
"Number of initially spawned entities does not match number of entities present at the end."
)

else:

self.logger.info(initial_carrots)
self.logger.info(final_carrots)
for ini_carrot in initial_carrots:
for final_carrot in final_carrots:
if ini_carrot.name == final_carrot.name:
Expand All @@ -67,8 +66,6 @@ def calculate_result(self, simulation_bridge: SimulationBridge) -> float:
# NOTE the specific coords that refer to for example
# middle of the table can differ across simulations,
# take that into consideration
self.logger.info(initial_y)
self.logger.info(final_y)
if (
initial_y <= 0.0
): # Carrot started in the incorrect place (right side)
Expand Down
4 changes: 2 additions & 2 deletions src/rai_sim/rai_sim/o3de/o3de_bridge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,14 +175,14 @@ def _despawn_entity(self, entity: SpawnedEntity):
)

def get_object_pose(self, entity: SpawnedEntity) -> PoseModel:
self.logger.info(f"GET OBJECT POSE: {entity}")
object_frame = entity.name + "/"
ros2_pose = do_transform_pose(
Pose(), self.connector.get_transform(object_frame + "odom", object_frame)
)
ros2_pose = do_transform_pose(
ros2_pose, self.connector.get_transform("world", "odom")
)
self.logger.info(f"ros2 pose: {ros2_pose}")
return self.from_ros2_pose(ros2_pose)

def get_scene_state(self) -> SceneState:
Expand All @@ -194,7 +194,7 @@ def get_scene_state(self) -> SceneState:
entities: list[SpawnedEntity] = []
for entity in self.spawned_entities:
current_pose = self.get_object_pose(entity)
self.logger.info(f"AFTER GET OBJECT POSE: {current_pose}")
self.logger.info(f"current pose: {current_pose}")
entities.append(
SpawnedEntity(
id=entity.id,
Expand Down

0 comments on commit a33e500

Please sign in to comment.