update for robot position in hubble

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/__init__.py

@@ -13,3 +13,5 @@
 - Reach environments for end-effector pose tracking
 
 """
+
+from .reach import *  # noqa: F401, F403

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/rizon_4s/joint_pos_env_cfg.py

@@ -169,7 +169,7 @@ class EventCfg:
         func=gear_assembly_events.randomize_gear_type,
         mode="reset",
         params={"gear_types": ["gear_small", "gear_medium", "gear_large"]},
-        # params={"gear_types": ["gear_small", "gear_medium"]},
+        # params={"gear_types": ["gear_large"]},
     )
 
     reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset")
@@ -407,15 +407,13 @@ def __post_init__(self):
         self.events.set_robot_to_grasp_pose.params["gripper_joint_setter_func"] = self.gripper_joint_setter_func
 
         # Populate reward term parameters for EE-gear keypoint tracking
-        self.rewards.end_effector_base_keypoint_tracking.params["end_effector_body_name"] = self.end_effector_body_name
-        self.rewards.end_effector_base_keypoint_tracking.params["grasp_rot_offset"] = self.grasp_rot_offset
-        self.rewards.end_effector_base_keypoint_tracking.params["gear_offsets_grasp"] = self.gear_offsets_grasp
+        self.rewards.ee_gear_keypoint_tracking.params["end_effector_body_name"] = self.end_effector_body_name
+        self.rewards.ee_gear_keypoint_tracking.params["grasp_rot_offset"] = self.grasp_rot_offset
+        self.rewards.ee_gear_keypoint_tracking.params["gear_offsets_grasp"] = self.gear_offsets_grasp
 
-        self.rewards.end_effector_base_keypoint_tracking_exp.params["end_effector_body_name"] = (
-            self.end_effector_body_name
-        )
-        self.rewards.end_effector_base_keypoint_tracking_exp.params["grasp_rot_offset"] = self.grasp_rot_offset
-        self.rewards.end_effector_base_keypoint_tracking_exp.params["gear_offsets_grasp"] = self.gear_offsets_grasp
+        self.rewards.ee_gear_keypoint_tracking_exp.params["end_effector_body_name"] = self.end_effector_body_name
+        self.rewards.ee_gear_keypoint_tracking_exp.params["grasp_rot_offset"] = self.grasp_rot_offset
+        self.rewards.ee_gear_keypoint_tracking_exp.params["gear_offsets_grasp"] = self.gear_offsets_grasp
 
         # Populate termination term parameters
         self.terminations.gear_dropped.params["gear_offsets_grasp"] = self.gear_offsets_grasp

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/rizon_4s/ros_inference_env_cfg.py

@@ -13,13 +13,20 @@
 
 @configclass
 class Rizon4sGearAssemblyROSInferenceEnvCfg(Rizon4sGearAssemblyEnvCfg):
-    """Configuration for ROS inference with Flexiv Rizon 4s and Grav gripper.
+    """ROS / Isaac Manipulator inference fields plus deployment alignment for NVIDIA Hubble Lab.
 
     This configuration:
     - Exposes variables needed for ROS inference
     - Overrides robot and gear initial poses for fixed/deterministic setup
+    - Aligns robot mounting pose with the Flexiv Rizon 4s installation at NVIDIA Hubble Lab
     """
 
+    # Single source for base + all gear rigid bodies (Rizon: closer to robot, centered)
+    ros_inference_factory_gears_init_state: RigidObjectCfg.InitialStateCfg = RigidObjectCfg.InitialStateCfg(
+        pos=(0.927, 0.046, -0.109),
+        rot=(0.0, 0.0, 0.70711, -0.70711),
+    )
+
     def __post_init__(self):
         # post init of parent
         super().__post_init__()
@@ -46,35 +53,39 @@ def __post_init__(self):
         # Dynamically generate action_scale_joint_space based on action_space
         self.action_scale_joint_space = [self.joint_action_scale] * self.action_space
 
-        # Override robot initial pose for ROS inference (fixed pose, no randomization)
-        # Joint positions and pos are inherited from parent, only override rotation to be deterministic
-        self.scene.robot.init_state.rot = (0.0, 0.0, 0.0, 1.0)  # Identity quaternion (x, y, z, w)
-
-        # Override gear base initial pose (fixed pose for ROS inference)
-        # Position configured for Rizon 4s workspace
-        self.scene.factory_gear_base.init_state = RigidObjectCfg.InitialStateCfg(
-            pos=(0.481, -0.073, -0.005),
-            rot=(0.0, 0.0, 0.70711, -0.70711),
-        )
-
-        # Override gear initial poses (fixed poses for ROS inference)
-        # Small gear
-        self.scene.factory_gear_small.init_state = RigidObjectCfg.InitialStateCfg(
-            pos=(0.481, -0.073, -0.005),
-            rot=(0.0, 0.0, 0.70711, -0.70711),
-        )
-
-        # Medium gear
-        self.scene.factory_gear_medium.init_state = RigidObjectCfg.InitialStateCfg(
-            pos=(0.481, -0.073, -0.005),
-            rot=(0.0, 0.0, 0.70711, -0.70711),
-        )
-
-        # Large gear
-        self.scene.factory_gear_large.init_state = RigidObjectCfg.InitialStateCfg(
-            pos=(0.481, -0.073, -0.005),
-            rot=(0.0, 0.0, 0.70711, -0.70711),
-        )
+        # --- NVIDIA Hubble Lab: Flexiv Rizon 4s mount ---
+        # Remove vertical mount stand since Hubble deployment does not use the sim stand asset
+        self.scene.stand = None
+
+        # Lab home joint pose (radians); aligns sim defaults / reset with the physical stand
+        self.scene.robot.init_state.joint_pos = {
+            "joint1": math.radians(-90.0),
+            "joint2": math.radians(90.0),
+            "joint3": 0.0,
+            "joint4": math.radians(90.0),
+            "joint5": 0.0,
+            "joint6": 0.0,
+            "joint7": 0.0,
+        }
+
+        # Orientation of robot is based on the Flexiv Rizon 4s mount in the Hubble Lab
+        self.scene.robot.init_state.pos = (0.0, 0.0, 0.0)
+        self.scene.robot.init_state.rot = (0.5, 0.5, 0.5, 0.5)
+
+        # Override gear base + all gears from one template (fixed pose for ROS inference)
+        _g = self.ros_inference_factory_gears_init_state
+        for _name in (
+            "factory_gear_base",
+            "factory_gear_small",
+            "factory_gear_medium",
+            "factory_gear_large",
+        ):
+            getattr(self.scene, _name).init_state = RigidObjectCfg.InitialStateCfg(
+                pos=_g.pos,
+                rot=_g.rot,
+                lin_vel=_g.lin_vel,
+                ang_vel=_g.ang_vel,
+            )
 
         # Fixed asset parameters for ROS inference - derived from configuration
         # These parameters are used by the ROS inference node to validate the environment setup

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/joint_pos_env_cfg.py

@@ -385,6 +385,15 @@ def __post_init__(self):
         self.events.set_robot_to_grasp_pose.params["grasp_rot_offset"] = self.grasp_rot_offset
         self.events.set_robot_to_grasp_pose.params["gripper_joint_setter_func"] = self.gripper_joint_setter_func
 
+        # Populate reward term parameters for EE-gear keypoint tracking
+        self.rewards.ee_gear_keypoint_tracking.params["end_effector_body_name"] = self.end_effector_body_name
+        self.rewards.ee_gear_keypoint_tracking.params["grasp_rot_offset"] = self.grasp_rot_offset
+        self.rewards.ee_gear_keypoint_tracking.params["gear_offsets_grasp"] = self.gear_offsets_grasp
+
+        self.rewards.ee_gear_keypoint_tracking_exp.params["end_effector_body_name"] = self.end_effector_body_name
+        self.rewards.ee_gear_keypoint_tracking_exp.params["grasp_rot_offset"] = self.grasp_rot_offset
+        self.rewards.ee_gear_keypoint_tracking_exp.params["gear_offsets_grasp"] = self.gear_offsets_grasp
+
         # Populate termination term parameters
         self.terminations.gear_dropped.params["gear_offsets_grasp"] = self.gear_offsets_grasp
         self.terminations.gear_dropped.params["end_effector_body_name"] = self.end_effector_body_name
@@ -483,6 +492,15 @@ def __post_init__(self):
         self.events.set_robot_to_grasp_pose.params["grasp_rot_offset"] = self.grasp_rot_offset
         self.events.set_robot_to_grasp_pose.params["gripper_joint_setter_func"] = self.gripper_joint_setter_func
 
+        # Populate reward term parameters for EE-gear keypoint tracking
+        self.rewards.ee_gear_keypoint_tracking.params["end_effector_body_name"] = self.end_effector_body_name
+        self.rewards.ee_gear_keypoint_tracking.params["grasp_rot_offset"] = self.grasp_rot_offset
+        self.rewards.ee_gear_keypoint_tracking.params["gear_offsets_grasp"] = self.gear_offsets_grasp
+
+        self.rewards.ee_gear_keypoint_tracking_exp.params["end_effector_body_name"] = self.end_effector_body_name
+        self.rewards.ee_gear_keypoint_tracking_exp.params["grasp_rot_offset"] = self.grasp_rot_offset
+        self.rewards.ee_gear_keypoint_tracking_exp.params["gear_offsets_grasp"] = self.gear_offsets_grasp
+
         # Populate termination term parameters
         self.terminations.gear_dropped.params["gear_offsets_grasp"] = self.gear_offsets_grasp
         self.terminations.gear_dropped.params["end_effector_body_name"] = self.end_effector_body_name

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/gear_assembly_env_cfg.py

@@ -263,34 +263,32 @@ class RewardsCfg:
         },
     )
 
-    end_effector_base_keypoint_tracking = RewTerm(
+    ee_gear_keypoint_tracking = RewTerm(
         func=mdp.keypoint_ee_gear_error,
         weight=-0.5,
         params={
             "robot_asset_cfg": SceneEntityCfg("robot"),
             "keypoint_scale": 0.15,
-            "ee_gear_threshold": 0.00,
-            "weight_ramp_start": 0.0,  # Set to 0.0 to enable ramp-up
+            "weight_ramp_start": 0.0,
             "weight_ramp_steps": 250_000,
         },
     )
 
-    end_effector_base_keypoint_tracking_exp = RewTerm(
+    ee_gear_keypoint_tracking_exp = RewTerm(
         func=mdp.keypoint_ee_gear_error_exp,
         weight=0.5,
         params={
             "robot_asset_cfg": SceneEntityCfg("robot"),
             "kp_exp_coeffs": [(50, 0.0001), (300, 0.0001)],
             "kp_use_sum_of_exps": False,
             "keypoint_scale": 0.15,
-            "ee_gear_threshold": 0.00,
-            "weight_ramp_start": 0.0,  # Set to 0.0 to enable ramp-up
+            "weight_ramp_start": 0.0,
             "weight_ramp_steps": 250_000,
         },
     )
 
+
     action_rate = RewTerm(func=mdp.action_rate_l2, weight=-5.0e-06)
-    # action = RewTerm(func=mdp.action_l2, weight=-5.0e-06)
 
 
 @configclass
@@ -341,9 +339,9 @@ def __post_init__(self):
         self.episode_length_s = 6.66
         self.viewer.eye = (3.5, 3.5, 3.5)
         # simulation settings
-        self.decimation = 33
+        self.decimation = 4
         self.sim.render_interval = self.decimation
-        self.sim.dt = 1.0 / 1000.0
+        self.sim.dt = 1.0 / 120.0
 
         self.gear_offsets = {
             "gear_small": [0.076125, 0.0, 0.0],

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py

@@ -342,27 +342,26 @@ def __call__(
             joint_pos = joint_pos + delta_dof_pos
 
             # Wrap arm joint positions to fall within robot's actual joint limits
-            joint_pos_limits = wp.to_torch(self.robot_asset.data.joint_pos_limits)[env_ids, : self.num_arm_joints, :]
+            joint_pos_limits = wp.to_torch(self.robot_asset.data.joint_pos_limits)[env_ids, :self.num_arm_joints, :]
             joint_min = joint_pos_limits[:, :, 0]
             joint_max = joint_pos_limits[:, :, 1]
             joint_range = joint_max - joint_min
 
             # Wrap only the arm joint positions (not gripper joints)
-            arm_joint_pos = joint_pos[:, : self.num_arm_joints]
+            arm_joint_pos = joint_pos[:, :self.num_arm_joints]
             arm_joint_pos = torch.where(
                 joint_range > 0,
                 joint_min + torch.remainder(arm_joint_pos - joint_min, joint_range),
                 arm_joint_pos,
             )
-            joint_pos[:, : self.num_arm_joints] = arm_joint_pos
+            joint_pos[:, :self.num_arm_joints] = arm_joint_pos
 
             joint_vel = torch.zeros_like(joint_pos)
 
             # Write to sim
-            self.robot_asset.set_joint_position_target_index(target=joint_pos, env_ids=env_ids)
-            self.robot_asset.set_joint_velocity_target_index(target=joint_vel, env_ids=env_ids)
-            self.robot_asset.write_joint_position_to_sim_index(position=joint_pos, env_ids=env_ids)
-            self.robot_asset.write_joint_velocity_to_sim_index(velocity=joint_vel, env_ids=env_ids)
+            self.robot_asset.set_joint_position_target(joint_pos, env_ids=env_ids)
+            self.robot_asset.set_joint_velocity_target(joint_vel, env_ids=env_ids)
+            self.robot_asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids)
 
         # Reset joint velocities to zero after IK convergence
         joint_vel = torch.zeros_like(wp.to_torch(self.robot_asset.data.joint_vel)[env_ids])
@@ -377,17 +376,16 @@ def __call__(
             hand_grasp_width = self.hand_grasp_width[gear_key]
             self.gripper_joint_setter_func(joint_pos, [row_idx], self.finger_joints, hand_grasp_width)
 
-        self.robot_asset.set_joint_position_target_index(target=joint_pos, joint_ids=self.all_joints, env_ids=env_ids)
-        self.robot_asset.write_joint_position_to_sim_index(position=joint_pos, env_ids=env_ids)
-        self.robot_asset.write_joint_velocity_to_sim_index(velocity=joint_vel, env_ids=env_ids)
+        self.robot_asset.set_joint_position_target(joint_pos, joint_ids=self.all_joints, env_ids=env_ids)
+        self.robot_asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids)
 
         # Set gripper to closed position
         for row_idx, env_id in enumerate(env_ids.tolist()):
             gear_key = all_gear_types[env_id]
             hand_close_width = self.hand_close_width[gear_key]
             self.gripper_joint_setter_func(joint_pos, [row_idx], self.finger_joints, hand_close_width)
 
-        self.robot_asset.set_joint_position_target_index(target=joint_pos, joint_ids=self.all_joints, env_ids=env_ids)
+        self.robot_asset.set_joint_position_target(joint_pos, joint_ids=self.all_joints, env_ids=env_ids)
 
 
 class randomize_gears_and_base_pose(ManagerTermBase):
@@ -466,11 +464,10 @@ def __call__(
         asset_names_to_process = [self.base_asset_name] + self.gear_asset_names
         for asset_name in asset_names_to_process:
             asset: RigidObject | Articulation = env.scene[asset_name]
-            default_root_pose = wp.to_torch(asset.data.default_root_pose)[env_ids].clone()
-            default_root_vel = wp.to_torch(asset.data.default_root_vel)[env_ids].clone()
-            positions = default_root_pose[:, 0:3] + env.scene.env_origins[env_ids] + rand_pose_samples[:, 0:3]
-            orientations = math_utils.quat_mul(default_root_pose[:, 3:7], orientations_delta)
-            velocities = default_root_vel + rand_vel_samples
+            root_states = wp.to_torch(asset.data.default_root_state)[env_ids].clone()
+            positions = root_states[:, 0:3] + env.scene.env_origins[env_ids] + rand_pose_samples[:, 0:3]
+            orientations = math_utils.quat_mul(root_states[:, 3:7], orientations_delta)
+            velocities = root_states[:, 7:13] + rand_vel_samples
             positions_by_asset[asset_name] = positions
             orientations_by_asset[asset_name] = orientations
             velocities_by_asset[asset_name] = velocities
@@ -500,5 +497,5 @@ def __call__(
             positions = positions_by_asset[asset_name]
             orientations = orientations_by_asset[asset_name]
             velocities = velocities_by_asset[asset_name]
-            asset.write_root_pose_to_sim_index(root_pose=torch.cat([positions, orientations], dim=-1), env_ids=env_ids)
-            asset.write_root_velocity_to_sim_index(root_velocity=velocities, env_ids=env_ids)
+            asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids)
+            asset.write_root_velocity_to_sim(velocities, env_ids=env_ids)