diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py
new file mode 100644
index 00000000..25889efd
--- /dev/null
+++ b/abr_control/arms/isaacsim_config.py
@@ -0,0 +1,594 @@
+"""Isaac Sim Configuration Module
+
+This module provides robot configuration for Isaac Sim simulation environment.
+It handles robot-specific parameters, kinematics, and dynamics calculations.
+"""
+
+import numpy as np
+import omni
+
+
+class IsaacsimConfig:
+    """A wrapper for Isaac Sim simulator to generate kinematics and dynamics calculations.
+    
+    This class provides robot-specific configurations and interfaces with Isaac Sim
+    to compute forward kinematics, Jacobians, mass matrices, and other dynamics
+    properties necessary for robot controllers.
+
+    Parameters
+    ----------
+    robot_type : str
+        The name of the robot model to load. Supported robots:
+        - "ur5": Universal Robots UR5 manipulator
+        - "jaco2": Kinova Jaco2 manipulator  
+        - "h1": Unitree H1 humanoid robot
+        - "h1_hands": Unitree H1 humanoid with hands
+
+    Attributes
+    ----------
+    robot_type : str
+        Name of the robot type
+    env_idx : int
+        Environment index for multi-environment simulations
+    ee_link_name : str
+        Name of the end-effector link
+    robot_path : str
+        USD file path for the robot model
+    has_EE : bool
+        Whether robot has a physical end-effector
+    EE_parent_link : str
+        Name of the parent link for end-effector attachment
+    ee_offset : list
+        Offset from parent link to end-effector [x, y, z]
+    target_min : numpy.ndarray
+        Minimum bounds for target workspace [x, y, z]
+    target_max : numpy.ndarray
+        Maximum bounds for target workspace [x, y, z]  
+    controlled_dof : list
+        List of controlled joint names
+    START_ANGLES : numpy.ndarray
+        Default starting joint angles in radians
+    """
+
+    def __init__(self, robot_type):
+        """Initialize robot configuration for specified robot type.
+
+        Parameters
+        ----------
+        robot_type : str
+            The name of the robot model to load
+            
+        Raises
+        ------
+        ValueError
+            If robot_type is not supported
+        """
+        self.robot_type = robot_type
+        self.env_idx = 0  # Assuming single robot environment
+        self.ee_link_name = "EE"  # Default name for virtual end-effector
+        
+        # Initialize robot-specific configurations
+        self._configure_robot()
+        
+        # Convert START_ANGLES string to numpy array
+        if hasattr(self, '_start_angles_str'):
+            self.START_ANGLES = np.array(self._start_angles_str.split(), dtype=float)
+
+    def _configure_robot(self):
+        """Configure robot-specific parameters based on robot type."""
+        if self.robot_type == "ur5":
+            self._configure_ur5()
+        elif self.robot_type == "jaco2":
+            self._configure_jaco2()
+        elif self.robot_type == "h1":
+            self._configure_h1()
+        elif self.robot_type == "h1_hands":
+            self._configure_h1_hands()
+        else:
+            raise ValueError(f"Unsupported robot type: {self.robot_type}")
+
+    def _configure_ur5(self):
+        """Configure parameters for UR5 robot."""
+        self.robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd"
+        self.has_EE = False  # UR5 has no end-effector
+        self.EE_parent_link = "wrist_3_link"
+        self.ee_offset = [0.0, 0.0, 0.0]
+        self._start_angles_str = "0 -.67 -.67 0 0 0"
+        self.target_min = np.array([-0.4, -0.4, 0.3])
+        self.target_max = np.array([0.4, 0.4, 0.6])
+        self.controlled_dof = [
+            'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
+            'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
+        ]
+        print(f"Virtual end effector '{self.ee_link_name}' will be attached "
+              f"(robot has no physical EE).")
+
+    def _configure_jaco2(self):
+        """Configure parameters for Jaco2 robot."""
+        self.robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd"
+        self.has_EE = True  # Jaco2 has an end-effector
+        self.EE_parent_link = "j2n6s300_link_6"
+        self.ee_link_name = "j2n6s300_end_effector"
+        self._start_angles_str = "2.0 3.14 1.57 4.71 0.0 3.04"
+        self.target_min = np.array([-0.4, -0.4, 0.3])
+        self.target_max = np.array([0.4, 0.4, 0.6])
+        self.controlled_dof = [
+            'j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3',
+            'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'
+        ]
+        print(f"End effector '{self.ee_link_name}' found in USD, using existing EE.")
+
+    def _configure_h1(self):
+        """Configure parameters for H1 humanoid robot."""
+        self.robot_path = "/Isaac/Robots/Unitree/H1/h1.usd"
+        self.has_EE = False  # H1 has no end-effector
+        self.EE_parent_link = "right_elbow_link"
+        self.ee_offset = [0.26455, 0.00118, -0.0209]  # H1 specific offset
+        self._start_angles_str = "0. 0. 0. 0."
+        self.target_min = np.array([0.12, -0.4, 1.4])
+        self.target_max = np.array([0.4, 0.05, 2.2])
+        self.controlled_dof = [
+            'right_shoulder_pitch_joint', 'right_shoulder_roll_joint',
+            'right_shoulder_yaw_joint', 'right_elbow_joint'
+        ]
+        self.lock_prim_standing = '/World/robot/pelvis'
+        print(f"Virtual end effector '{self.ee_link_name}' will be attached "
+              f"(robot has no physical EE).")
+
+    def _configure_h1_hands(self):
+        """Configure parameters for H1 humanoid robot with hands."""
+        self.robot_path = "/Isaac/Robots/Unitree/H1/h1_with_hand.usd"
+        self.has_EE = True  # H1 with hands has an end-effector
+        self.EE_parent_link = "right_elbow_link"
+        self.ee_link_name = "right_hand_link"
+        self._start_angles_str = "0. 0. 0. 0. 0."
+        self.target_min = np.array([0.12, -0.4, 1.4])
+        self.target_max = np.array([0.49, 0.05, 2.2])
+        self.controlled_dof = [
+            'right_shoulder_pitch_joint', 'right_shoulder_roll_joint',
+            'right_shoulder_yaw_joint', 'right_elbow_joint', 'right_hand_joint'
+        ]
+        self.lock_prim_standing = '/World/robot/pelvis'
+        print(f"End effector '{self.ee_link_name}' found in USD, using existing EE.")
+
+    def _connect(self, world, stage, articulation_view, dof_indices, 
+                 joint_indices, prim_path):
+        """Connect the configuration to Isaac Sim simulation.
+        
+        Called by the interface once the Isaac Sim simulation is created,
+        this connects the config to the simulator to access kinematics
+        and dynamics information.
+        
+        Parameters
+        ----------
+        world : omni.isaac.core.World
+            The Isaac Sim World object managing the simulation environment
+        stage : pxr.Usd.Stage
+            The USD stage containing the scene hierarchy and prims
+        articulation_view : omni.isaac.core.articulations.ArticulationView
+            ArticulationView providing access to robot's kinematic and
+            dynamic properties in Isaac Sim
+        dof_indices : numpy.ndarray
+            Indices of controlled robot joints in the articulation's DOF array
+        joint_indices : numpy.ndarray
+            Indices of controlled robot joints in the articulation's joint array
+        prim_path : str
+            USD prim path to the robot articulation in scene hierarchy
+        """
+        # Store Isaac Sim interface objects
+        self.world = world
+        self.stage = stage
+        self.articulation_view = articulation_view
+        self.dof_indices = np.copy(dof_indices)
+        self.joint_indices = np.copy(joint_indices)
+        self.prim_path = prim_path
+        
+        # Robot configuration parameters
+        self.N_JOINTS = len(self.dof_indices)
+        self.N_ALL_DOF = self.articulation_view.num_dof
+        self.N_ALL_JOINTS = self.articulation_view.num_joints
+        self._is_fixed_base = self._check_fixed_base()
+        
+        # Offset for non-fixed base robots (mobile robots)
+        # Necessary for correct indexing of Jacobian matrices
+        self.base_offset = 0 if self._is_fixed_base else 6
+        
+        # Pre-compute indices for efficient matrix operations
+        self._setup_matrix_indices()
+        
+        # Initialize data storage arrays
+        self._initialize_data_arrays()
+
+    def _check_fixed_base(self):
+        """Check if robot has a fixed base or is mobile.
+        
+        Returns
+        -------
+        bool
+            True if robot has fixed base, False if mobile
+        """
+        num_dof = self.articulation_view.num_dof
+        jacobian_shape = self.articulation_view.get_jacobian_shape()[2]
+        return num_dof == jacobian_shape
+
+    def _setup_matrix_indices(self):
+        """Pre-compute indices for efficient matrix operations."""
+        # Jacobian indices for position and rotation (6DOF)
+        self.jac_indices = np.hstack([
+            self.dof_indices + (ii * self.N_ALL_DOF) for ii in range(6)
+        ])
+        
+        # Mass matrix indices
+        self.M_indices = [
+            ii * self.N_ALL_DOF + jj
+            for jj in self.dof_indices
+            for ii in self.dof_indices
+        ]
+
+    def _initialize_data_arrays(self):
+        """Initialize arrays for storing dynamics data."""
+        self._g = np.zeros(self.N_JOINTS)
+        self._J6N = np.zeros((6, self.N_JOINTS))
+        self._MNN = np.zeros((self.N_ALL_DOF, self.N_ALL_DOF))
+        self._R = np.zeros((3, 3))
+
+    def g(self, q=None):
+        """Get gravitational forces for controlled joints.
+
+        Parameters
+        ----------
+        q : numpy.ndarray, optional
+            Joint angles (not used in current implementation)
+            
+        Returns
+        -------
+        numpy.ndarray
+            Gravitational forces for controlled joints
+        """
+        gravity_forces = self.articulation_view.get_generalized_gravity_forces()
+        self._g = gravity_forces[self.env_idx]
+        
+        if self._is_fixed_base:
+            return -self._g[self.dof_indices]
+        else:
+            # For mobile robots, gravity compensation is typically handled differently
+            return np.zeros(len(self.dof_indices))
+
+    def dJ(self, name, q=None, dq=None, x=None):
+        """Get derivative of Jacobian with respect to time.
+
+        Parameters
+        ----------
+        name : str
+            Name of the prim to compute derivative for
+        q : numpy.ndarray, optional
+            Joint angles
+        dq : numpy.ndarray, optional
+            Joint velocities
+        x : numpy.ndarray, optional
+            Additional state variables
+            
+        Raises
+        ------
+        NotImplementedError
+            This method is not currently implemented
+        """
+        raise NotImplementedError("Jacobian derivative not implemented yet")
+
+    def J(self, name, q=None, x=None):
+        """Get Jacobian matrix for the specified end-effector.
+        
+        For mobile robots, the floating base is accounted for with an offset.
+
+        Parameters
+        ----------
+        name : str
+            Name of the prim to compute Jacobian for
+        q : numpy.ndarray, optional
+            Joint angles (not used in current implementation)
+        x : numpy.ndarray, optional
+            Additional state variables
+            
+        Returns
+        -------
+        numpy.ndarray
+            6×N Jacobian matrix where N is number of controlled joints
+            First 3 rows: linear velocity Jacobian
+            Last 3 rows: angular velocity Jacobian
+            
+        Raises
+        ------
+        RuntimeError
+            If ArticulationView contains no environments or Jacobian is all zeros
+        """
+        jacobians = self.articulation_view.get_jacobians(clone=True)
+        
+        if jacobians.shape[0] == 0:
+            raise RuntimeError("ArticulationView contains no environments.")
+
+        # Account for fixed vs mobile base indexing
+        offset = -1 if self._is_fixed_base else 0
+        body_index = self.articulation_view.get_body_index(self.EE_parent_link) + offset
+
+        J_full = jacobians[self.env_idx, body_index, :, :]
+        
+        if len(jacobians.shape) == 4:
+            # Apply offset for mobile robots and extract controlled DOF columns
+            indices = self.dof_indices + self.base_offset
+            J = J_full[:, indices]
+        else:
+            raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}")
+
+        # Store linear and angular velocity Jacobians
+        self._J6N[:3, :] = J[:3, :]  # Linear velocity
+        self._J6N[3:, :] = J[3:, :]  # Angular velocity
+
+        if np.allclose(J, 0):
+            raise RuntimeError(
+                "Jacobian is all zeros - check robot configuration and joint addresses"
+            )
+            
+        return np.copy(self._J6N)
+
+    def M(self, q=None):
+        """Get mass/inertia matrix for controlled joints.
+        
+        For mobile robots, the floating base is accounted for with an offset.
+
+        Parameters
+        ----------
+        q : numpy.ndarray, optional
+            Joint angles (not used in current implementation)
+            
+        Returns
+        -------
+        numpy.ndarray
+            N×N mass matrix where N is number of controlled joints
+        """
+        # Get full mass matrix (num_envs, joint_count, joint_count)
+        mass_matrices = self.articulation_view.get_mass_matrices()
+        self._MNN = mass_matrices[self.env_idx]
+        
+        # Apply offset for mobile robots and extract controlled joints
+        indices = self.dof_indices + self.base_offset
+        M = self._MNN[np.ix_(indices, indices)]
+        
+        return M
+
+    def R(self, name, q=None):
+        """Get rotation matrix for the specified object.
+        
+        Parameters
+        ----------
+        name : str
+            Name of the object (body, geom, or site) in USD stage
+        q : numpy.ndarray, optional
+            Joint positions (not used unless simulating different state)
+            
+        Returns
+        -------
+        numpy.ndarray
+            3×3 rotation matrix
+            
+        Raises
+        ------
+        RuntimeError
+            If prim is not found or invalid
+        """
+        prim = self._get_prim(name)
+        if not prim.IsValid():
+            raise RuntimeError(f"Prim '{name}' not found or invalid")
+
+        # Get 4×4 world transform matrix
+        matrix = omni.usd.get_world_transform_matrix(prim)
+
+        # Extract 3×3 rotation matrix
+        self._R = np.array([
+            [matrix[0][0], matrix[0][1], matrix[0][2]],
+            [matrix[1][0], matrix[1][1], matrix[1][2]],
+            [matrix[2][0], matrix[2][1], matrix[2][2]]
+        ])
+
+        return self._R
+
+    def quaternion(self, name, q=None):
+        """Get quaternion orientation for the specified prim.
+        
+        Parameters
+        ----------
+        name : str
+            Name of the prim to get quaternion for
+        q : numpy.ndarray, optional
+            Joint angles (not used in current implementation)
+            
+        Returns
+        -------
+        numpy.ndarray
+            Quaternion as [w, x, y, z]
+        """
+        if name == "EE":
+            name = self.ee_link_name
+            
+        prim = self._get_prim(name)
+
+        # Get 4×4 world transform matrix and extract quaternion
+        matrix = omni.usd.get_world_transform_matrix(prim)
+        quat = matrix.ExtractRotationQuat()
+        quat_np = np.array([quat.GetReal(), *quat.GetImaginary()])
+
+        return quat_np
+
+    def C(self, q=None, dq=None):
+        """Get Coriolis and centrifugal forces.
+        
+        Note: Currently returns forces for controlled joints only.
+        
+        Parameters
+        ----------
+        q : numpy.ndarray, optional
+            Joint angles
+        dq : numpy.ndarray, optional
+            Joint velocities
+            
+        Returns
+        -------
+        numpy.ndarray
+            Coriolis and centrifugal forces for controlled joints
+        """
+        coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(
+            joint_indices=self.dof_indices
+        )
+        return coriolis[self.env_idx]
+
+    def T(self, name, q=None, x=None):
+        """Get full transform matrix for the specified prim.
+
+        Parameters
+        ----------
+        name : str
+            Name of the prim to get transform for
+        q : numpy.ndarray, optional
+            Joint angles
+        x : numpy.ndarray, optional
+            Additional state variables
+            
+        Raises
+        ------
+        NotImplementedError
+            This method is not currently implemented
+        """
+        raise NotImplementedError("Full transform matrix not implemented yet")
+
+    def Tx(self, name, q=None, x=None):
+        """Get position of the specified prim.
+
+        Parameters
+        ----------
+        name : str
+            Name of the prim to get position for
+        q : numpy.ndarray, optional
+            Joint angles (not used in current implementation)
+        x : numpy.ndarray, optional
+            Additional state variables
+            
+        Returns
+        -------
+        numpy.ndarray
+            Position as [x, y, z] in meters
+            
+        Raises
+        ------
+        RuntimeError
+            If prim is not found or invalid
+        """
+        if name == "EE":
+            name = self.ee_link_name
+            
+        prim = self._get_prim(name)
+        if not prim.IsValid():
+            raise RuntimeError(f"Invalid prim: {name}")
+
+        matrix = omni.usd.utils.get_world_transform_matrix(prim)
+        position = matrix.ExtractTranslation()
+        
+        return np.array([position[0], position[1], position[2]], dtype=np.float64)
+
+    def T_inv(self, name, q=None, x=None):
+        """Get inverse transform matrix for the specified prim.
+
+        Parameters
+        ----------
+        name : str
+            Name of the prim to get inverse transform for
+        q : numpy.ndarray, optional
+            Joint angles
+        x : numpy.ndarray, optional
+            Additional state variables
+            
+        Raises
+        ------
+        NotImplementedError
+            This method is not currently implemented
+        """
+        raise NotImplementedError("Inverse transform matrix not implemented yet")
+
+    def _get_prim_path(self, name):
+        """Find the full prim path for a given prim name.
+        
+        Parameters
+        ----------
+        name : str
+            Name of the prim to find
+            
+        Returns
+        -------
+        str or None
+            Full prim path if found, None otherwise
+        """
+        for prim in self.stage.Traverse():
+            if str(prim.GetPath()).endswith(name):
+                return prim.GetPath()
+        return None
+
+    def _get_prim(self, name):
+        """Get USD prim object for the given name.
+        
+        Parameters
+        ----------
+        name : str
+            Name of the prim to retrieve
+            
+        Returns
+        -------
+        pxr.Usd.Prim
+            USD prim object
+        """
+        prim_path = self._get_prim_path(name)
+        prim = self.stage.GetPrimAtPath(prim_path)
+        return prim
+
+    def _get_joint_positions(self):
+        """Get current positions of controlled joints.
+        
+        Returns
+        -------
+        numpy.ndarray
+            Joint positions for controlled joints
+        """
+        full_positions = self.articulation_view.get_joint_positions()[self.env_idx]
+        return full_positions[self.dof_indices]
+
+    def _get_joint_velocities(self):
+        """Get current velocities of controlled joints.
+        
+        Returns
+        -------
+        numpy.ndarray
+            Joint velocities for controlled joints
+        """
+        full_velocities = self.articulation_view.get_joint_velocities()[self.env_idx]
+        return full_velocities[self.dof_indices]
+
+    def _set_joint_positions(self, q):
+        """Set positions for controlled joints.
+        
+        Parameters
+        ----------
+        q : numpy.ndarray
+            Target joint positions
+        """
+        full_positions = self.articulation_view.get_joint_positions()[self.env_idx]
+        full_positions[self.dof_indices] = q
+        self.articulation_view.set_joint_positions(full_positions)
+
+    def _set_joint_velocities(self, dq):
+        """Set velocities for controlled joints.
+        
+        Parameters
+        ----------
+        dq : numpy.ndarray
+            Target joint velocities
+        """
+        full_velocities = self.articulation_view.get_joint_velocities()[self.env_idx]
+        full_velocities[self.dof_indices] = dq
+        self.articulation_view.set_joint_velocities(full_velocities)
\ No newline at end of file
diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py
new file mode 100644
index 00000000..76574bf2
--- /dev/null
+++ b/abr_control/interfaces/nv_isaacsim.py
@@ -0,0 +1,433 @@
+"""Isaac Sim Interface Module
+
+This module provides an interface for controlling robots in Isaac Sim simulation environment.
+It extends the base Interface class to provide Isaac Sim specific functionality.
+"""
+
+import numpy as np
+from isaacsim import SimulationApp
+from .interface import Interface
+
+# Initialize simulation app before importing other Isaac Sim modules
+simulation_app = SimulationApp({"headless": False})
+
+import omni
+import omni.kit.commands  # type: ignore
+import isaacsim.core.utils.stage as stage_utils  # type: ignore
+from omni.isaac.core import World  # type: ignore
+from omni.isaac.core.articulations import ArticulationView  # type: ignore
+from isaacsim.core.api.robots import Robot  # type: ignore
+from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics  # type: ignore
+from isaacsim.core.utils.nucleus import get_assets_root_path  # type: ignore
+import isaacsim.core.utils.numpy.rotations as rot_utils  # type: ignore
+
+
+class IsaacSim(Interface):
+    """An interface for Isaac Sim simulation environment.
+
+    This class provides methods to connect to Isaac Sim, control robots,
+    and interact with the simulation environment.
+
+    Parameters
+    ----------
+    robot_config : object
+        Configuration object containing all relevant information about the robot
+        such as: number of joints, number of links, mass information, etc.
+    dt : float, optional
+        Simulation time step in seconds. Default is 0.001.
+
+    Attributes
+    ----------
+    robot_config : object
+        The robot configuration object
+    dt : float
+        Simulation time step
+    prim_path : str
+        USD prim path for the robot in the scene
+    world : World
+        Isaac Sim world object
+    articulation_view : ArticulationView
+        View of the robot articulation
+    dof_indices : list
+        Indices of degrees of freedom being controlled
+    joint_indices : list
+        Indices of joints being controlled
+    """
+
+    def __init__(self, robot_config, dt=0.001):
+        """Initialize the Isaac Sim interface.
+        
+        Parameters
+        ----------
+        robot_config : object
+            Robot configuration object
+        dt : float, optional
+            Simulation time step in seconds. Default is 0.001.
+        """
+        super().__init__(robot_config)
+        self.robot_config = robot_config
+        self.dt = dt
+        self.prim_path = "/World/robot"
+
+        # Initialize attributes that will be set during connection
+        self.world = None
+        self.articulation_view = None
+        self.context = None
+        self.stage = None
+        self.dof_indices = []
+        self.joint_indices = []
+        self.all_dof_names = []
+        self.all_joint_names = []
+        self.all_body_names = []
+
+    def connect(self, joint_names=None):
+        """Connect to the Isaac Sim environment and set up the robot.
+
+        Parameters
+        ----------
+        joint_names : list, optional
+            List of joint names to send control signals to and get feedback from.
+            If None, the joints in the kinematic tree connecting the end-effector
+            to the world are used. Default is None.
+        
+        Raises
+        ------
+        Exception
+            If a specified joint name does not exist in the robot model.
+        """
+        # Initialize the simulation world
+        self.world = World(
+            stage_units_in_meters=1.0,
+            physics_dt=self.dt,
+            rendering_dt=self.dt
+        )
+        self.world.scene.add_default_ground_plane()
+        self.context = omni.usd.get_context()
+        self.stage = self.context.get_stage()
+
+        # Load the robot from USD file
+        assets_root_path = get_assets_root_path()
+        robot_usd_path = f"{assets_root_path}{self.robot_config.robot_path}"
+        print(f"Robot '{self.robot_config.robot_type}' is loaded from USD path: {robot_usd_path}")
+
+        # Add robot to the stage
+        stage_utils.add_reference_to_stage(
+            usd_path=robot_usd_path,
+            prim_path=self.prim_path,
+        )
+        robot = self.world.scene.add(
+            Robot(prim_path=self.prim_path, name=self.robot_config.robot_type)
+        )
+
+        # Reset world to ensure proper initialization
+        self.world.reset()
+
+        # Create and initialize articulation view
+        self.articulation_view = ArticulationView(
+            prim_paths_expr=self.prim_path,
+            name=self.robot_config.robot_type + "_view"
+        )
+        self.world.scene.add(self.articulation_view)
+        self.articulation_view.initialize()
+
+        # Add virtual end-effector if robot doesn't have one
+        if not self.robot_config.has_EE:
+            print("Robot has no EE, virtual one is attached.")
+            self.add_virtual_ee_link(
+                self.robot_config.EE_parent_link,
+                self.robot_config.ee_link_name,
+                offset=self.robot_config.ee_offset
+            )
+
+        # Reset the world to initialize physics
+        self.world.reset()
+
+        # Set up joint information
+        self._setup_joint_indices(joint_names)
+
+        # Connect robot config with simulation data
+        print("Connecting to robot config...")
+        self.robot_config._connect(
+            self.world,
+            self.stage,
+            self.articulation_view,
+            self.dof_indices,
+            self.joint_indices,
+            self.prim_path,
+        )
+
+        # Additional setup for mobile robots
+        if not self.robot_config._is_fixed_base:
+            self.world.add_physics_callback("keep_standing", self.keep_standing)
+            # Adapt max force for better reactivity
+            for joint_name in self.robot_config.controlled_dof:
+                self.set_max_force(name=joint_name, value=0.7)
+        else:
+            self.set_gains()
+
+    def _setup_joint_indices(self, joint_names):
+        """Set up joint and DOF indices for control.
+        
+        Parameters
+        ----------
+        joint_names : list or None
+            List of joint names to control
+        """
+        self.dof_indices = []
+        self.joint_indices = []
+
+        if joint_names is None:
+            print("No joint names provided, using all controllable joints.")
+            joint_names = self.robot_config.controlled_dof
+
+        # Get all available names
+        self.all_dof_names = self.articulation_view.dof_names
+        self.all_joint_names = self.articulation_view.joint_names
+        self.all_body_names = self.articulation_view.body_names
+
+        # Map joint names to indices
+        for name in joint_names:
+            if name not in self.all_dof_names or name not in self.all_joint_names:
+                raise Exception(f"Joint name {name} does not exist in robot model")
+            
+            joint_idx = self.articulation_view.get_joint_index(name)
+            dof_idx = self.articulation_view.get_dof_index(name)
+            self.dof_indices.append(dof_idx)
+            self.joint_indices.append(joint_idx)
+
+    def disconnect(self):
+        """Disconnect from Isaac Sim and clean up resources.
+        
+        Properly closes the simulation application and cleans up any
+        remaining connections or callbacks.
+        """
+        if hasattr(self, 'simulation_app') and self.simulation_app:
+            self.simulation_app.close()
+        print("IsaacSim connection closed...")
+
+    def send_forces(self, u):
+        """Apply torques to the controlled degrees of freedom.
+
+        Parameters
+        ----------
+        u : numpy.ndarray
+            Array of forces/torques to apply to the controlled DOFs.
+            Length must match the number of controlled joints.
+        """
+        # Create full torque vector for all DOFs
+        full_torques = np.zeros(self.robot_config.N_ALL_DOF)
+        # Apply control torques to the controlled joints
+        full_torques[self.dof_indices] = u
+        # Apply the control signal
+        self.articulation_view.set_joint_efforts(full_torques)
+        # Move simulation ahead one time step
+        self.world.step(render=True)
+
+    def send_target_angles(self, q):
+        """Move the robot to specified joint angles.
+
+        Parameters
+        ----------
+        q : numpy.ndarray
+            Array of target joint angles in radians.
+            Length must match the number of controlled joints.
+        """
+        self.robot_config._set_joint_positions(q)
+        self.world.step(render=True)
+
+    def get_feedback(self):
+        """Get current robot state information.
+
+        Returns
+        -------
+        dict
+            Dictionary containing:
+            - 'q': Joint angles in radians
+            - 'dq': Joint velocities in rad/sec
+        """
+        self.q = self.robot_config._get_joint_positions()
+        self.dq = self.robot_config._get_joint_velocities()
+        return {"q": self.q, "dq": self.dq}
+
+    def set_xyz(self, name, xyz):
+        """Set the position of an object in the environment.
+
+        Parameters
+        ----------
+        name : str
+            The prim path of the object to move
+        xyz : numpy.ndarray
+            The [x, y, z] location of the target in meters
+        """
+        prim = self.robot_config._get_prim(name)
+        xformable = UsdGeom.Xformable(prim)
+        transform_matrix = Gf.Matrix4d().SetTranslate(
+            Gf.Vec3d(xyz[0], xyz[1], xyz[2])
+        )
+        xformable.MakeMatrixXform().Set(transform_matrix)
+
+    def set_orientation(self, name, quat_wxyz):
+        """Set the orientation of an object in the environment.
+
+        Parameters
+        ----------
+        name : str
+            The prim path of the object to rotate
+        quat_wxyz : numpy.ndarray
+            The [w, x, y, z] quaternion representation of the target orientation
+        """
+        prim = self.robot_config._get_prim(name)
+        xformable = UsdGeom.Xformable(prim)
+        quat = Gf.Quatd(
+            quat_wxyz[0],
+            Gf.Vec3d(quat_wxyz[1], quat_wxyz[2], quat_wxyz[3])
+        )
+        transform_matrix = Gf.Matrix4d().SetRotate(quat)
+        xformable.MakeMatrixXform().Set(transform_matrix)
+
+    def keep_standing(self, dt):
+        """Physics callback to keep humanoid robots upright.
+        
+        This method is used as a physics callback to prevent humanoid
+        robots from falling over during simulation.
+
+        Parameters
+        ----------
+        dt : float
+            Time step (automatically passed by physics callback system)
+        """
+        prim = self.stage.GetPrimAtPath(self.robot_config.lock_prim_standing)
+        prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0))
+        prim.GetAttribute("xformOp:translate").Set(Gf.Vec3f(0.0, 0.0, 1.4))
+
+    def create_target_prim(self, prim_path="/World/target", size=0.1, 
+                          color=np.array([0, 0, 1.0])):
+        """Create a visual-only target cube in the scene.
+
+        Parameters
+        ----------
+        prim_path : str, optional
+            USD prim path for the target object. Default is "/World/target".
+        size : float, optional
+            Size of the cube in meters. Default is 0.1.
+        color : numpy.ndarray, optional
+            RGB color values [0-1]. Default is blue [0, 0, 1.0].
+
+        Returns
+        -------
+        UsdGeom.Cube
+            The created cube primitive
+        """
+        # Create cube geometry
+        cube_prim = UsdGeom.Cube.Define(self.stage, prim_path)
+        cube_prim.CreateSizeAttr(size)
+
+        # Create and apply material for color
+        material_path = prim_path + "/Material"
+        material = UsdShade.Material.Define(self.stage, material_path)
+
+        # Create shader
+        shader = UsdShade.Shader.Define(self.stage, material_path + "/Shader")
+        shader.CreateIdAttr("UsdPreviewSurface")
+        shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(
+            Gf.Vec3f(color[0], color[1], color[2])
+        )
+        shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.4)
+        shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0)
+
+        # Connect shader to material and bind material to cube
+        material.CreateSurfaceOutput().ConnectToSource(
+            shader.ConnectableAPI(), "surface"
+        )
+        UsdShade.MaterialBindingAPI(cube_prim).Bind(material)
+
+        # Disable collision to ensure it's purely visual
+        cube_prim.GetPrim().CreateAttribute(
+            "physics:collisionEnabled", Sdf.ValueTypeNames.Bool
+        ).Set(False)
+        
+        return cube_prim
+
+    def create_random_pos(self):
+        """Generate a random position within the robot's target bounds.
+
+        Returns
+        -------
+        numpy.ndarray
+            Random [x, y, z] position within the configured target bounds
+        """
+        pos_target = np.array([
+            np.random.uniform(
+                low=self.robot_config.target_min[0],
+                high=self.robot_config.target_max[0]
+            ),
+            np.random.uniform(
+                low=self.robot_config.target_min[1],
+                high=self.robot_config.target_max[1]
+            ),
+            np.random.uniform(
+                low=self.robot_config.target_min[2],
+                high=self.robot_config.target_max[2]
+            ),
+        ])
+        return pos_target
+
+    def set_max_force(self, name, value):
+        """Set maximum force for a joint using PhysX DriveAPI.
+
+        Parameters
+        ----------
+        name : str
+            Name of the joint to configure
+        value : float
+            Maximum force value to set
+        """
+        prim = self.robot_config._get_prim(name)
+        # Apply the DriveAPI if not already present
+        # Use "linear" for prismatic joints, "angular" for revolute joints
+        driveAPI = UsdPhysics.DriveAPI.Apply(prim, "angular")
+        # Set the max force
+        driveAPI.CreateMaxForceAttr(value)
+
+    def set_gains(self):
+        """Set PD controller gains for robot joints.
+        
+        Sets appropriate stiffness and damping values for controlled
+        and non-controlled joints. Controlled joints get low stiffness
+        for force control, while other joints maintain position.
+        """
+        # Initialize with default high stiffness for position holding
+        stiffness = np.ones(self.robot_config.N_ALL_DOF) * 100.0
+        damping = np.ones(self.robot_config.N_ALL_DOF) * 10.0
+
+        # Set low gains for controlled joints (force control)
+        for idx in self.dof_indices:
+            stiffness[idx] = 0.0
+            damping[idx] = 0.1
+
+        self.articulation_view.set_gains(stiffness, damping)
+
+    def add_virtual_ee_link(self, EE_parent_link, ee_name, offset):
+        """Add a virtual end-effector link as an Xform under the parent link.
+
+        This method creates a virtual end-effector when the robot model
+        doesn't include one explicitly.
+
+        Parameters
+        ----------
+        EE_parent_link : str
+            Name of the parent link to attach the virtual EE to
+        ee_name : str
+            Name for the virtual end-effector link
+        offset : numpy.ndarray or list
+            [x, y, z] offset from parent link in meters
+        """
+        parent_path = f"{self.prim_path}/{EE_parent_link}"
+        # Full path to the new EE transform, nested under parent
+        ee_prim_path = f"{parent_path}/{ee_name}"
+        
+        # Create the Xform prim
+        ee_prim = UsdGeom.Xform.Define(self.stage, ee_prim_path)
+        
+        # Set transform relative to parent
+        ee_prim.AddTranslateOp().Set(Gf.Vec3d(*offset))
\ No newline at end of file
diff --git a/examples/IsaacSim/force_floating_control.py b/examples/IsaacSim/force_floating_control.py
new file mode 100644
index 00000000..2bf38cc8
--- /dev/null
+++ b/examples/IsaacSim/force_floating_control.py
@@ -0,0 +1,82 @@
+"""
+A basic script for connecting to the arm and putting it in floating
+mode, which only compensates for gravity. The end-effector position
+is recorded and plotted when the script is exited (with ctrl-c).
+
+In this example, the floating controller is applied in the joint space
+"""
+import sys
+import traceback
+import numpy as np
+from abr_control.arms.isaacsim_config import IsaacsimConfig as arm
+from abr_control.controllers import Floating
+from abr_control.interfaces.nv_isaacsim import IsaacSim
+if len(sys.argv) > 1:
+    arm_model = sys.argv[1]
+else:
+    arm_model = "ur5"
+robot_config = arm(arm_model)
+dt = 0.005
+interface = IsaacSim(robot_config, dt)
+interface.connect(joint_names=robot_config.controlled_dof)
+
+interface.send_target_angles(robot_config.START_ANGLES)
+
+# instantiate the controller
+ctrlr = Floating(robot_config, task_space=False, dynamic=True)
+
+# set up arrays for tracking end-effector and target position
+ee_track = []
+q_track = []
+
+try:
+    # get the end-effector's initial position
+    feedback = interface.get_feedback()
+    start = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"])
+    print("\nSimulation starting...\n")
+    while 1:
+        # get joint angle and velocity feedback
+        feedback = interface.get_feedback()
+
+        # calculate the control signal
+        u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"])
+
+        # send forces into Isaacsim
+        interface.send_forces(u)
+
+        # calculate the position of the hand
+        hand_xyz = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"])
+        # track end effector position
+        ee_track.append(np.copy(hand_xyz))
+        q_track.append(np.copy(feedback["q"]))
+
+
+except:
+    print(traceback.format_exc())
+
+finally:
+    # close the connection to the arm
+    #interface.disconnect()
+
+    print("Simulation terminated...")
+
+    ee_track = np.array(ee_track)
+
+    if ee_track.shape[0] > 0:
+        # plot distance from target and 3D trajectory
+        import matplotlib.pyplot as plt
+        from mpl_toolkits.mplot3d import axes3d  # pylint: disable=W0611
+
+        fig = plt.figure(figsize=(8, 12))
+        ax1 = fig.add_subplot(211)
+        ax1.set_title("Joint Angles")
+        ax1.set_ylabel("Angle (rad)")
+        ax1.set_xlabel("Time (ms)")
+        ax1.plot(q_track)
+        ax1.legend()
+
+        ax2 = fig.add_subplot(212, projection="3d")
+        ax2.set_title("End-Effector Trajectory")
+        ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz")
+        ax2.legend()
+        plt.show()
diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py
new file mode 100644
index 00000000..b8e8351c
--- /dev/null
+++ b/examples/IsaacSim/force_osc_xyz.py
@@ -0,0 +1,136 @@
+"""
+Move the jaco2 IsaacSim arm to a target position.
+The simulation ends after 1500 time steps, and the
+trajectory of the end-effector is plotted in 3D.
+"""
+import sys
+import traceback
+import numpy as np
+from abr_control.arms.isaacsim_config import IsaacsimConfig as arm
+from abr_control.controllers import OSC, Damping
+from abr_control.interfaces.nv_isaacsim import IsaacSim
+from abr_control.utils import transformations
+
+
+if len(sys.argv) > 1:
+    arm_model = sys.argv[1]
+else:
+    arm_model = "ur5" # works with "h1_hands" / "h1" / jaco2 / ur5
+robot_config = arm(arm_model)
+
+dt = 0.005 # 200 Hz
+target_name="target"
+
+# create our IsaacSim interface
+interface = IsaacSim(robot_config, dt)
+interface.connect()
+interface.send_target_angles(robot_config.START_ANGLES)
+isaac_target = interface.create_target_prim()
+
+# damp the movements of the arm
+damping = Damping(robot_config, kv=30)  #kv=10)
+# instantiate controller
+ctrlr = OSC(
+    robot_config,
+    kp= 300, # 200
+    null_controllers=[damping],
+    vmax=[0.5, 0],  # [m/s, rad/s]
+    ctrlr_dof= [True, True, True, False, False, False]
+)
+
+# set up lists for tracking data
+ee_track = []
+target_track = []
+
+green = [0, 0.9, 0, 0.5]
+red = [0.9, 0, 0, 0.5]
+np.random.seed(0)
+
+try:
+    # get the end-effector's initial position
+    feedback = interface.get_feedback()
+    interface.set_xyz("target", interface.create_random_pos())
+
+    count = 0
+    print("\nSimulation starting...\n")
+    while 1:
+    
+        feedback = interface.get_feedback()
+        target = np.hstack(
+            [
+                robot_config.Tx(target_name),
+                transformations.euler_from_quaternion(
+                    robot_config.quaternion(target_name), "rxyz"
+                ),
+            ]
+        )
+        
+        # calculate the control signal
+        u = ctrlr.generate(
+            q=feedback["q"],
+            dq=feedback["dq"],
+            target=target,
+        )
+        interface.send_forces(u)
+
+        # calculate end-effector position
+        ee_xyz = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"])
+        # track data
+        ee_track.append(np.copy(ee_xyz))
+        target_track.append(np.copy(target[:3]))
+
+        error = np.linalg.norm(ee_xyz - target[:3])
+        if error < 0.1: #0.02:
+            count += 1
+        else:
+            count = 0
+        if count >= 50:
+            print("Generating a new target")
+            interface.set_xyz("target", interface.create_random_pos())
+            count = 0
+
+except:
+    print(traceback.format_exc())
+
+finally:
+    # stop and reset the IsaacSim simulation
+    #interface.disconnect()
+
+    print("Simulation terminated...")
+
+    ee_track = np.array(ee_track)
+    target_track = np.array(target_track)
+
+    if ee_track.shape[0] > 0:
+        # plot distance from target and 3D trajectory
+        import matplotlib.pyplot as plt
+        from mpl_toolkits.mplot3d import axes3d  # pylint: disable=W0611
+
+        fig = plt.figure(figsize=(8, 12))
+        ax1 = fig.add_subplot(211)
+        ax1.set_ylabel("Distance (m)")
+        ax1.set_xlabel("Time (ms)")
+        ax1.set_title("Distance to target")
+        ax1.plot(
+            np.sqrt(np.sum((np.array(target_track) - np.array(ee_track)) ** 2, axis=1))
+        )
+
+        ax2 = fig.add_subplot(212, projection="3d")
+        ax2.set_title("End-Effector Trajectory")
+        ax2.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz")
+        ax2.scatter(
+            target_track[1, 0],
+            target_track[1, 1],
+            target_track[1, 2],
+            label="target",
+            c="r",
+        )
+        ax2.scatter(
+            ee_track[0, 0],
+            ee_track[0, 1],
+            ee_track[0, 2],
+            label="start",
+            c="g",
+        )
+        ax2.legend()
+        plt.show()
\ No newline at end of file
diff --git a/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity.py b/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity.py
new file mode 100644
index 00000000..645b7f37
--- /dev/null
+++ b/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity.py
@@ -0,0 +1,195 @@
+"""
+Running operational space control using Mujoco. The controller will
+move the end-effector to the target object's X position and orientation.
+
+The cartesian direction being controlled is set in the first three booleans
+of the ctrlr_dof parameter
+"""
+import sys
+import numpy as np
+
+from abr_control.arms.isaacsim_config import IsaacsimConfig as arm
+from abr_control.controllers import OSC, Damping
+from abr_control.controllers.path_planners import PathPlanner
+from abr_control.controllers.path_planners.position_profiles import Linear
+from abr_control.controllers.path_planners.velocity_profiles import Gaussian
+from abr_control.interfaces.nv_isaacsim import IsaacSim
+
+max_a = 2
+n_targets = 100
+
+if len(sys.argv) > 1:
+    arm_model = sys.argv[1]
+else:
+    arm_model = "jaco2" # works with jaco2 / ur5 
+robot_config = arm(arm_model)
+
+ctrlr_dof = [True, True, True, False, False, False]
+dof_labels = ["x", "y", "z", "a", "b", "g"]
+dof_print = f"* DOF Controlled: {np.array(dof_labels)[ctrlr_dof]} *"
+stars = "*" * len(dof_print)
+print(stars)
+print(dof_print)
+print(stars)
+dt = 0.005 # 200 Hz
+target_prim_path="/World/target"
+
+# create our interface
+interface = IsaacSim(robot_config, dt)
+interface.connect()
+interface.send_target_angles(robot_config.START_ANGLES)
+isaac_target = interface.create_target_prim(prim_path=target_prim_path)
+
+# damp the movements of the arm
+damping = Damping(robot_config, kv=10)
+# create opreational space controller
+ctrlr = OSC(
+    robot_config,
+    kp=30,
+    kv=20,
+    ko=180,
+    null_controllers=[damping],
+    vmax=[10, 10],  # [m/s, rad/s]
+    # control (x, alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma]
+    ctrlr_dof=ctrlr_dof,
+)
+
+path_planner = PathPlanner(
+    pos_profile=Linear(), vel_profile=Gaussian(dt=dt, acceleration=max_a)
+)
+
+# set up lists for tracking data
+ee_track = []
+target_track = []
+
+
+print("\nSimulation starting...\n")
+for ii in range(0, n_targets):
+        feedback = interface.get_feedback()
+        hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
+        pos_target = interface.create_random_pos()
+        path_planner.generate_path(
+            start_position=hand_xyz, target_position=pos_target, max_velocity=2
+        )
+
+        interface.set_xyz("target", pos_target)
+        #interface.set_xyz(target_prim_path, pos_target)
+        at_target = 0
+        count = 0
+
+        while at_target < 500:
+            if count > 5000:
+                break
+            filtered_target = path_planner.next()
+            interface.set_xyz(target_prim_path, filtered_target[:3])
+
+            feedback = interface.get_feedback()
+            hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
+
+            u = ctrlr.generate(
+                q=feedback["q"],
+                dq=feedback["dq"],
+                target=filtered_target,
+            )
+
+            # apply the control signal, step the sim forward
+            interface.send_forces(u)
+
+            # track data
+            ee_track.append(np.copy(hand_xyz))
+            target_track.append(np.copy(pos_target[:3]))
+
+            if np.linalg.norm(hand_xyz - pos_target) < 0.02:
+                at_target += 1
+            else:
+                at_target = 0
+            count += 1
+
+
+try:
+    print("\nSimulation starting...\n")
+    for ii in range(0, n_targets):
+        feedback = interface.get_feedback()
+        hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
+
+        pos_target = np.array(
+            [
+                np.random.uniform(low=-0.4, high=0.4),
+                np.random.uniform(low=-0.4, high=0.4),
+                np.random.uniform(low=0.3, high=0.6),
+            ]
+        )
+
+        path_planner.generate_path(
+            start_position=hand_xyz, target_position=pos_target, max_velocity=2
+        )
+
+        #interface.set_xyz("target", pos_target)
+        interface.set_xyz(target_prim_path, pos_target)
+        at_target = 0
+        count = 0
+
+        while at_target < 500:
+            if count > 5000:
+                break
+            filtered_target = path_planner.next()
+            interface.set_xyz(target_prim_path, filtered_target[:3])
+            #interface.set_xyz("target_orientation", filtered_target[:3])
+
+            feedback = interface.get_feedback()
+            hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
+
+            u = ctrlr.generate(
+                q=feedback["q"],
+                dq=feedback["dq"],
+                target=filtered_target,
+            )
+
+            # apply the control signal, step the sim forward
+            interface.send_forces(u)
+
+            # track data
+            ee_track.append(np.copy(hand_xyz))
+            target_track.append(np.copy(pos_target[:3]))
+
+            if np.linalg.norm(hand_xyz - pos_target) < 0.02:
+                at_target += 1
+            else:
+                at_target = 0
+            count += 1
+
+finally:
+    # stop and reset the simulation
+    interface.disconnect()
+
+    print("Simulation terminated...")
+
+    ee_track = np.array(ee_track)
+    target_track = np.array(target_track)
+
+    if ee_track.shape[0] > 0:
+        # plot distance from target and 3D trajectory
+        import matplotlib.pyplot as plt
+        from mpl_toolkits.mplot3d import axes3d  # pylint: disable=W0611
+
+        fig = plt.figure(figsize=(8, 12))
+        ax1 = fig.add_subplot(211)
+        ax1.set_ylabel("3D position (m)")
+        for ii, controlled_dof in enumerate(ctrlr_dof[:3]):
+            if controlled_dof:
+                ax1.plot(ee_track[:, ii], label=dof_labels[ii])
+                ax1.plot(target_track[:, ii], "--")
+        ax1.legend()
+
+        ax3 = fig.add_subplot(212, projection="3d")
+        ax3.set_title("End-Effector Trajectory")
+        ax3.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2], label="ee_xyz")
+        ax3.scatter(
+            target_track[0, 0],
+            target_track[0, 1],
+            target_track[0, 2],
+            label="target",
+            c="g",
+        )
+        ax3.legend()
+        plt.show()