Skip to content

Commit a33e500

Browse files
jmatejczjmatejcz
committed
partially fixed getting position bug
1 parent 3508158 commit a33e500

File tree

3 files changed

+57
-60
lines changed

3 files changed

+57
-60
lines changed

‎src/rai_bench/rai_bench/main.py

+52-52
Original file line numberDiff line numberDiff line change
@@ -94,60 +94,60 @@
9494
connector_path = configs_dir + "o3de_config.yaml"
9595
#### Create scenarios manually
9696
# load different scenes
97-
# one_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
98-
# base_config_path=Path(configs_dir + "scene1.yaml"),
99-
# connector_config_path=Path(connector_path),
100-
# )
101-
# multiple_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
102-
# base_config_path=Path(configs_dir + "scene2.yaml"),
103-
# connector_config_path=Path(connector_path),
104-
# )
105-
# red_cubes_simulation_config = O3DExROS2SimulationConfig.load_config(
106-
# base_config_path=Path(configs_dir + "scene3.yaml"),
107-
# connector_config_path=Path(connector_path),
108-
# )
109-
# multiple_cubes_simulation_config = O3DExROS2SimulationConfig.load_config(
110-
# base_config_path=Path(configs_dir + "scene4.yaml"),
111-
# connector_config_path=Path(connector_path),
112-
# )
113-
# # combine different scene configs with the tasks to create various scenarios
114-
# scenarios = [
115-
# Scenario(
116-
# task=GrabCarrotTask(logger=bench_logger),
117-
# simulation_config=one_carrot_simulation_config,
118-
# ),
119-
# Scenario(
120-
# task=GrabCarrotTask(logger=bench_logger),
121-
# simulation_config=multiple_carrot_simulation_config,
122-
# ),
123-
# Scenario(
124-
# task=PlaceCubesTask(logger=bench_logger),
125-
# simulation_config=red_cubes_simulation_config,
126-
# ),
127-
# Scenario(
128-
# task=PlaceCubesTask(logger=bench_logger),
129-
# simulation_config=multiple_cubes_simulation_config,
130-
# ),
131-
# ]
97+
one_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
98+
base_config_path=Path(configs_dir + "scene1.yaml"),
99+
connector_config_path=Path(connector_path),
100+
)
101+
multiple_carrot_simulation_config = O3DExROS2SimulationConfig.load_config(
102+
base_config_path=Path(configs_dir + "scene2.yaml"),
103+
connector_config_path=Path(connector_path),
104+
)
105+
red_cubes_simulation_config = O3DExROS2SimulationConfig.load_config(
106+
base_config_path=Path(configs_dir + "scene3.yaml"),
107+
connector_config_path=Path(connector_path),
108+
)
109+
multiple_cubes_simulation_config = O3DExROS2SimulationConfig.load_config(
110+
base_config_path=Path(configs_dir + "scene4.yaml"),
111+
connector_config_path=Path(connector_path),
112+
)
113+
# combine different scene configs with the tasks to create various scenarios
114+
scenarios = [
115+
Scenario(
116+
task=GrabCarrotTask(logger=bench_logger),
117+
simulation_config=one_carrot_simulation_config,
118+
),
119+
# Scenario(
120+
# task=GrabCarrotTask(logger=bench_logger),
121+
# simulation_config=multiple_carrot_simulation_config,
122+
# ),
123+
# Scenario(
124+
# task=PlaceCubesTask(logger=bench_logger),
125+
# simulation_config=red_cubes_simulation_config,
126+
# ),
127+
# Scenario(
128+
# task=PlaceCubesTask(logger=bench_logger),
129+
# simulation_config=multiple_cubes_simulation_config,
130+
# ),
131+
]
132132

133133
### Create scenarios automatically
134-
scene_paths = [
135-
configs_dir + "scene1.yaml",
136-
configs_dir + "scene2.yaml",
137-
configs_dir + "scene3.yaml",
138-
configs_dir + "scene4.yaml",
139-
]
140-
simulations_configs = [
141-
O3DExROS2SimulationConfig.load_config(Path(path), Path(connector_path))
142-
for path in scene_paths
143-
]
144-
tasks = [
145-
GrabCarrotTask(logger=bench_logger),
146-
PlaceCubesTask(logger=bench_logger),
147-
]
148-
scenarios = Benchmark.create_scenarios(
149-
tasks=tasks, simulation_configs=simulations_configs
150-
)
134+
# scene_paths = [
135+
# configs_dir + "scene1.yaml",
136+
# configs_dir + "scene2.yaml",
137+
# configs_dir + "scene3.yaml",
138+
# configs_dir + "scene4.yaml",
139+
# ]
140+
# simulations_configs = [
141+
# O3DExROS2SimulationConfig.load_config(Path(path), Path(connector_path))
142+
# for path in scene_paths
143+
# ]
144+
# tasks = [
145+
# GrabCarrotTask(logger=bench_logger),
146+
# PlaceCubesTask(logger=bench_logger),
147+
# ]
148+
# scenarios = Benchmark.create_scenarios(
149+
# tasks=tasks, simulation_configs=simulations_configs
150+
# )
151151

152152
# custom request to arm
153153
base_arm_pose = PoseModel(translation=Translation(x=0.3, y=0.0, z=0.5))

‎src/rai_bench/rai_bench/o3de_test_bench/tasks/grab_carrot_task.py

+3-6
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ def calculate_result(self, simulation_bridge: SimulationBridge) -> float:
4141
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
4242

4343
scene_state = simulation_bridge.get_scene_state()
44-
44+
self.logger.info(f"ENTITIES: {scene_state.entities}")
4545
initial_carrots = self.filter_entities_by_prefab_type(
4646
simulation_bridge.spawned_entities, prefab_types=["carrot"]
4747
)
@@ -50,15 +50,14 @@ def calculate_result(self, simulation_bridge: SimulationBridge) -> float:
5050
)
5151
num_initial_carrots = len(initial_carrots)
5252

53-
self.logger.info(initial_carrots)
54-
self.logger.info(final_carrots)
5553
if num_initial_carrots != len(final_carrots):
5654
raise EntitiesMismatchException(
5755
"Number of initially spawned entities does not match number of entities present at the end."
5856
)
5957

6058
else:
61-
59+
self.logger.info(initial_carrots)
60+
self.logger.info(final_carrots)
6261
for ini_carrot in initial_carrots:
6362
for final_carrot in final_carrots:
6463
if ini_carrot.name == final_carrot.name:
@@ -67,8 +66,6 @@ def calculate_result(self, simulation_bridge: SimulationBridge) -> float:
6766
# NOTE the specific coords that refer to for example
6867
# middle of the table can differ across simulations,
6968
# take that into consideration
70-
self.logger.info(initial_y)
71-
self.logger.info(final_y)
7269
if (
7370
initial_y <= 0.0
7471
): # Carrot started in the incorrect place (right side)

‎src/rai_sim/rai_sim/o3de/o3de_bridge.py

+2-2
Original file line numberDiff line numberDiff line change
@@ -175,14 +175,14 @@ def _despawn_entity(self, entity: SpawnedEntity):
175175
)
176176

177177
def get_object_pose(self, entity: SpawnedEntity) -> PoseModel:
178-
self.logger.info(f"GET OBJECT POSE: {entity}")
179178
object_frame = entity.name + "/"
180179
ros2_pose = do_transform_pose(
181180
Pose(), self.connector.get_transform(object_frame + "odom", object_frame)
182181
)
183182
ros2_pose = do_transform_pose(
184183
ros2_pose, self.connector.get_transform("world", "odom")
185184
)
185+
self.logger.info(f"ros2 pose: {ros2_pose}")
186186
return self.from_ros2_pose(ros2_pose)
187187

188188
def get_scene_state(self) -> SceneState:
@@ -194,7 +194,7 @@ def get_scene_state(self) -> SceneState:
194194
entities: list[SpawnedEntity] = []
195195
for entity in self.spawned_entities:
196196
current_pose = self.get_object_pose(entity)
197-
self.logger.info(f"AFTER GET OBJECT POSE: {current_pose}")
197+
self.logger.info(f"current pose: {current_pose}")
198198
entities.append(
199199
SpawnedEntity(
200200
id=entity.id,

0 commit comments

Comments
 (0)