diff --git a/eval/cup_stacking_eval.py b/eval/cup_stacking_eval.py index 0eb1bfc..1ff7cc6 100644 --- a/eval/cup_stacking_eval.py +++ b/eval/cup_stacking_eval.py @@ -28,7 +28,7 @@ TAG_TO_OBJECT: dict[int, str] = {1: "blue_cup", 2: "pink_cup"} ANCHOR_TAG_ID: int = 0 ANCHOR_WORLD_POSE: tuple[float, float, float] = (0.0, 0.0, 0.0) -OBJECT_Z: float = 0.12 +OBJECT_Z: float = 1.0 OBJECT_ROLL: float = 0.0 OBJECT_PITCH: float = 0.0 @@ -44,7 +44,7 @@ class CupStackingEvalSceneCfg(SingleArmFrankaTaskSceneCfg): usd_path=str(KITCHEN_OBJECTS_ROOT / "BlueCup" / "BlueCup.usd"), mass_props=MassPropertiesCfg(mass=0.1), ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.36, -0.4, 0.12), rot=(1.0, 0.0, 0.0, 0.0)), + init_state=RigidObjectCfg.InitialStateCfg(pos=(9.0, 6.45, 1.0), rot=(1.0, 0.0, 0.0, 0.0)), ) pink_cup: RigidObjectCfg = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/Scene/pink_cup", @@ -52,7 +52,7 @@ class CupStackingEvalSceneCfg(SingleArmFrankaTaskSceneCfg): usd_path=str(KITCHEN_OBJECTS_ROOT / "PinkCup" / "PinkCup.usd"), mass_props=MassPropertiesCfg(mass=0.1), ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.46, -0.4, 0.12), rot=(1.0, 0.0, 0.0, 0.0)), + init_state=RigidObjectCfg.InitialStateCfg(pos=(9.2, 6.5, 1.0), rot=(1.0, 0.0, 0.0, 0.0)), ) @@ -105,7 +105,7 @@ def __post_init__(self) -> None: self.viewer.lookat = (0.4, -1.3, -0.2) self.dynamic_reset_gripper_effort_limit = False - self.scene.robot.init_state.pos = (0.35, -0.74, 0.01) + self.scene.robot.init_state.pos = (9.0, 6.0, 0.9) self.scene.robot.init_state.rot = (0.707, 0.0, 0.0, 0.707) self.scene.robot.init_state.joint_pos = { "panda_joint1": 0.0, diff --git a/packages/simulator/assets/scenes/kitchen/nycu_light_collider_wooden.usd b/packages/simulator/assets/scenes/kitchen/nycu_light_collider_wooden.usd new file mode 100644 index 0000000..248bf78 Binary files /dev/null and b/packages/simulator/assets/scenes/kitchen/nycu_light_collider_wooden.usd differ diff --git a/packages/simulator/assets/scenes/kitchen/objects/PinkCup/PinkCup.usd b/packages/simulator/assets/scenes/kitchen/objects/PinkCup/PinkCup.usd index 038b353..0cddd3d 100755 Binary files a/packages/simulator/assets/scenes/kitchen/objects/PinkCup/PinkCup.usd and b/packages/simulator/assets/scenes/kitchen/objects/PinkCup/PinkCup.usd differ diff --git a/packages/simulator/src/simulator/assets/scenes/kitchen.py b/packages/simulator/src/simulator/assets/scenes/kitchen.py index 91e94e7..99f12fd 100644 --- a/packages/simulator/src/simulator/assets/scenes/kitchen.py +++ b/packages/simulator/src/simulator/assets/scenes/kitchen.py @@ -7,7 +7,7 @@ """Configuration for the Kitchen Scene""" SCENES_ROOT = Path(ASSETS_ROOT) / "scenes" -KITCHEN_USD_PATH = str(SCENES_ROOT / "kitchen" / "scene.usd") +KITCHEN_USD_PATH = str(SCENES_ROOT / "kitchen" / "nycu_light_collider_wooden.usd") KITCHEN_CFG = AssetBaseCfg( spawn=sim_utils.UsdFileCfg( diff --git a/packages/simulator/src/simulator/datagen/state_machine/cup_stacking.py b/packages/simulator/src/simulator/datagen/state_machine/cup_stacking.py index b9bc7a8..1aa321c 100644 --- a/packages/simulator/src/simulator/datagen/state_machine/cup_stacking.py +++ b/packages/simulator/src/simulator/datagen/state_machine/cup_stacking.py @@ -41,7 +41,7 @@ _HOVER_Z_OFFSET = 0.15 _GRASP_Z_OFFSET = 0.08 -_LIFT_Z_OFFSET = 0.2 +_LIFT_Z_OFFSET = 0.25 _RELEASE_Z_OFFSET = 0.09 _GRIPPER_DOWN_ROLL_W = math.pi _GRIPPER_DOWN_PITCH_W = 0.0 @@ -133,9 +133,9 @@ def __init__(self) -> None: 80, # Phase 1: Approach down to the blue cup 20, # Phase 2: Close gripper to grasp 100, # Phase 3: Lift blue cup upward - 85, # Phase 4: Move blue cup above the pink cup - 35, # Phase 5: Lower/place and release - 30, # Phase 6: Move up and away + 120, # Phase 4: Move blue cup above the pink cup + 40, # Phase 5: Lower/place and release + 80, # Phase 6: Move up and away ] # ------------------------------------------------------------------ @@ -250,11 +250,13 @@ def _phase_lift_blue(self, blue_cup_pos_w, num_envs, device): def _phase_move_above_pink(self, pink_cup_pos_w, num_envs, device): target_pos_w = pink_cup_pos_w.clone() target_pos_w[:, 2] += _LIFT_Z_OFFSET + #target_pos_w[:, 1] += 0.01 # +y 1cm return target_pos_w, _constant_gripper(num_envs, device, _GRIPPER_CLOSE) def _phase_lower_to_release(self, pink_cup_pos_w, num_envs, device): target_pos_w = pink_cup_pos_w.clone() target_pos_w[:, 2] += _RELEASE_Z_OFFSET + #target_pos_w[:, 1] += 0.01 # +y 1cm return target_pos_w, _constant_gripper(num_envs, device, _GRIPPER_CLOSE) def _phase_lift_away(self, pink_cup_pos_w, num_envs, device): diff --git a/packages/simulator/src/simulator/tasks/cup_stacking/cup_stacking_env_cfg.py b/packages/simulator/src/simulator/tasks/cup_stacking/cup_stacking_env_cfg.py index 4b5a503..5b44985 100644 --- a/packages/simulator/src/simulator/tasks/cup_stacking/cup_stacking_env_cfg.py +++ b/packages/simulator/src/simulator/tasks/cup_stacking/cup_stacking_env_cfg.py @@ -26,7 +26,7 @@ TAG_TO_OBJECT: dict[int, str] = {1: "blue_cup", 2: "pink_cup"} ANCHOR_TAG_ID: int = 0 ANCHOR_WORLD_POSE: tuple[float, float, float] = (0.5, -0.2, 0.0) -OBJECT_Z: float = 0.12 +OBJECT_Z: float = 1.0 OBJECT_ROLL: float = 0.0 OBJECT_PITCH: float = 0.0 @@ -109,7 +109,7 @@ def __post_init__(self) -> None: self.viewer.lookat = (0.4, -1.3, -0.2) self.dynamic_reset_gripper_effort_limit = False - self.scene.robot.init_state.pos = (0.35, -0.74, 0.01) + self.scene.robot.init_state.pos = (9.0, 6.0, 0.9) self.scene.robot.init_state.rot = (0.707, 0.0, 0.0, 0.707) self.scene.robot.init_state.joint_pos = { "panda_joint1": 0.0, diff --git a/packages/simulator/src/simulator/tasks/template/single_arm_franka_cfg.py b/packages/simulator/src/simulator/tasks/template/single_arm_franka_cfg.py index 54e3078..0d6030b 100644 --- a/packages/simulator/src/simulator/tasks/template/single_arm_franka_cfg.py +++ b/packages/simulator/src/simulator/tasks/template/single_arm_franka_cfg.py @@ -70,7 +70,13 @@ class SingleArmFrankaTaskSceneCfg(InteractiveSceneCfg): wrist: TiledCameraCfg = TiledCameraCfg( prim_path="{ENV_REGEX_NS}/Robot/panda_hand/wrist", - offset=TiledCameraCfg.OffsetCfg(pos=(0.04, 0.0, 0.0), rot=(0.707, 0, 0, 0.707), convention="ros"), + offset=TiledCameraCfg.OffsetCfg(pos=(0.04, 0.0, 0.0), rot=(0.707, 0, 0, 0.707), convention="ros"), + + # rot=(0.707, 0, 0, 0.707) + # => wxyz=(0, 0.707, 0.707, 0) + # rot=(0, 0.707, 0, 0.707) + # => wxyz=(0.707, 0, 0.707, 0) + data_types=["rgb"], spawn=sim_utils.PinholeCameraCfg( focal_length=24, @@ -87,11 +93,13 @@ class SingleArmFrankaTaskSceneCfg(InteractiveSceneCfg): front: TiledCameraCfg = TiledCameraCfg( prim_path="/World/front_camera", offset=TiledCameraCfg.OffsetCfg( - pos=(0.35, 1.1, 0.6), rot=(0.0, -0.0, -0.60182, -0.79864), convention="opengl" + # pos=(8.19043, 6.69887, 1.06662), rot=(0.459121, 0.459121, -0.53778, -0.53778), convention="opengl" + pos=(7.80362, 6.78599, 1.1197), rot=(0.37025, 0.37025, -0.60242, -0.60242), convention="opengl" ), data_types=["rgb"], spawn=sim_utils.PinholeCameraCfg( - focal_length=55, + #focal_length=55, + focal_length=18.14756, focus_distance=400.0, horizontal_aperture=38.11, clipping_range=(0.01, 50.0),