From 3672d6ee96ed8a92dd6e6e52d283d5100f322edc Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 22 Apr 2025 12:03:14 -0400 Subject: [PATCH 01/42] added basic interface for isaacsim --- abr_control/interfaces/isaacsim.py | 78 ++++++++++++++++++++++++++++++ examples/isaacsim/cube.py | 56 +++++++++++++++++++++ 2 files changed, 134 insertions(+) create mode 100644 abr_control/interfaces/isaacsim.py create mode 100644 examples/isaacsim/cube.py diff --git a/abr_control/interfaces/isaacsim.py b/abr_control/interfaces/isaacsim.py new file mode 100644 index 00000000..9ff4e066 --- /dev/null +++ b/abr_control/interfaces/isaacsim.py @@ -0,0 +1,78 @@ +import numpy as np +from abr_control.utils import download_meshes, transformations +from isaacsim import SimulationApp +from .interface import Interface + + + + +class IsaacSim(Interface): + """An interface for IsaacSim. + robot_config : class instance + contains all relevant information about the arm + such as: number of joints, number of links, mass information etc. + dt : float, optional (Default: 0.001) + simulation time step in seconds + + + Parameters + ---------- + + """ + def __init__(self, robot_config, dt=0.001): + + super().__init__(robot_config) + + self.dt = dt # time step + self.count = 0 # keep track of how many times send forces is called + + ''' + self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles + self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities + + # joint target velocities, as part of the torque limiting control + # these need to be super high so that the joints are always moving + # at the maximum allowed torque + self.joint_target_velocities = np.ones(robot_config.N_JOINTS) * 10000.0 + ''' + + + + + def connect(self): + """All initial setup.""" + self.simulation_app = SimulationApp({"headless": False}) + print("Started Isaacsim as stand alone app...") + + def disconnect(self): + """Any socket closing etc that must be done to properly shut down""" + self.simulation_app.close() # close Isaac Sim + print("IsaacSim connection closed...") + + def send_forces(self, u): + """Applies the set of torques u to the arm. If interfacing to + a simulation, also moves dynamics forward one time step. + + u : np.array + An array of joint torques [Nm] + """ + + raise NotImplementedError + + def send_target_angles(self, q): + """Moves the arm to the specified joint angles + + q : numpy.array + the target joint angles [radians] + """ + + raise NotImplementedError + + def get_feedback(self): + """Returns a dictionary of the relevant feedback + + Returns a dictionary of relevant feedback to the + controller. At very least this contains q, dq. + """ + + raise NotImplementedError diff --git a/examples/isaacsim/cube.py b/examples/isaacsim/cube.py new file mode 100644 index 00000000..6278cd52 --- /dev/null +++ b/examples/isaacsim/cube.py @@ -0,0 +1,56 @@ +import numpy as np +from abr_control.arms import ur5 as arm + +from abr_control.interfaces.isaacsim import IsaacSim +from abr_control.utils import transformations + +# Sim step size +dt = 0.005 + +# Initialize our robot config +robot_config = arm.Config() + + +# Create our interface +# interface = CoppeliaSim(robot_config, dt=dt) +interface = IsaacSim(robot_config, dt=dt) +interface.connect() +# Imports must be done after instantiating the SimulationApp, which is done by the setup in the isaacsim interface +from isaacsim.core.api import World # type: ignore +from isaacsim.core.api.objects import DynamicCuboid # type: ignore + + + +#TODO add this in interface setup +#world = World(physics_dt=dt,rendering_dt=dt) + +world = World() +world.scene.add_default_ground_plane() +fancy_cube = world.scene.add( + + DynamicCuboid( + prim_path="/World/random_cube", + name="fancy_cube", + position=np.array([0, 0, 1.0]), + scale=np.array([0.5015, 0.5015, 0.5015]), + color=np.array([0, 0, 1.0]), + )) + + +world.reset() +for i in range(500): + + position, orientation = fancy_cube.get_world_pose() + linear_velocity = fancy_cube.get_linear_velocity() + + # will be shown on terminal + print("Cube position is : " + str(position)) + print("Cube's orientation is : " + str(orientation)) + print("Cube's linear velocity is : " + str(linear_velocity)) + + # we have control over stepping physics and rendering in this workflow + # things run in sync + world.step(render=True) # execute one physics step and one rendering step + + +interface.disconnect() \ No newline at end of file From 0a7b3d67ddb2172d3075b0de35b449cdcf841566 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 24 Apr 2025 11:49:48 -0400 Subject: [PATCH 02/42] create world with dt --- abr_control/interfaces/isaacsim.py | 9 +++------ examples/isaacsim/cube.py | 11 ++--------- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/abr_control/interfaces/isaacsim.py b/abr_control/interfaces/isaacsim.py index 9ff4e066..0a1ffc69 100644 --- a/abr_control/interfaces/isaacsim.py +++ b/abr_control/interfaces/isaacsim.py @@ -4,20 +4,17 @@ from .interface import Interface - - class IsaacSim(Interface): """An interface for IsaacSim. + + Parameters + ---------- robot_config : class instance contains all relevant information about the arm such as: number of joints, number of links, mass information etc. dt : float, optional (Default: 0.001) simulation time step in seconds - - Parameters - ---------- - """ def __init__(self, robot_config, dt=0.001): diff --git a/examples/isaacsim/cube.py b/examples/isaacsim/cube.py index 6278cd52..208fa3bb 100644 --- a/examples/isaacsim/cube.py +++ b/examples/isaacsim/cube.py @@ -1,6 +1,5 @@ import numpy as np from abr_control.arms import ur5 as arm - from abr_control.interfaces.isaacsim import IsaacSim from abr_control.utils import transformations @@ -10,21 +9,15 @@ # Initialize our robot config robot_config = arm.Config() - # Create our interface -# interface = CoppeliaSim(robot_config, dt=dt) interface = IsaacSim(robot_config, dt=dt) interface.connect() # Imports must be done after instantiating the SimulationApp, which is done by the setup in the isaacsim interface from isaacsim.core.api import World # type: ignore from isaacsim.core.api.objects import DynamicCuboid # type: ignore - - -#TODO add this in interface setup -#world = World(physics_dt=dt,rendering_dt=dt) - -world = World() +# Create a world +world = World(physics_dt=dt,rendering_dt=dt) world.scene.add_default_ground_plane() fancy_cube = world.scene.add( From 73d07bbc31b2d11141ec12e6e311585809f67370 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Fri, 25 Apr 2025 15:56:33 -0400 Subject: [PATCH 03/42] implemented method connect in interface for isaacsim --- abr_control/interfaces/isaacsim.py | 55 ++++++++++++++++++++++++++++-- 1 file changed, 52 insertions(+), 3 deletions(-) diff --git a/abr_control/interfaces/isaacsim.py b/abr_control/interfaces/isaacsim.py index 0a1ffc69..fa4c4b00 100644 --- a/abr_control/interfaces/isaacsim.py +++ b/abr_control/interfaces/isaacsim.py @@ -23,6 +23,11 @@ def __init__(self, robot_config, dt=0.001): self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called + self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles + self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities + + #self.misc_handles = {} # for tracking miscellaneous object handles + ''' self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities @@ -34,11 +39,55 @@ def __init__(self, robot_config, dt=0.001): ''' - - - def connect(self): + def connect(self, load_scene=True): """All initial setup.""" self.simulation_app = SimulationApp({"headless": False}) + from isaacsim.core.api import World # type: ignore + from isaacsim.core.utils.extensions import get_extension_path_from_name # type: ignore + from isaacsim.asset.importer.urdf import _urdf # type: ignore + import omni.kit.commands # type: ignore + import omni.usd # type: ignore + + self.robot_model = None + self.urdf_path = None + self.import_config = None + self.world = None + if load_scene: + # Create a world + self.world = World(physics_dt=self.dt,rendering_dt=self.dt) + self.world.scene.add_default_ground_plane() + + # Acquire the URDF extension interface for parsing and importing URDF files + urdf_interface = _urdf.acquire_urdf_interface() + + # Configure the settings for importing the URDF file + self.import_config = _urdf.ImportConfig() + self.import_config.convex_decomp = False # Disable convex decomposition for simplicity + self.import_config.fix_base = True # Fix the base of the robot to the ground + self.import_config.make_default_prim = True # Make the robot the default prim in the scene + self.import_config.self_collision = False # Disable self-collision for performance + self.import_config.distance_scale = 1 # Set distance scale for the robot + self.import_config.density = 0.0 # Set density to 0 (use default values) + + # Retrieve the path of the URDF file from the extension + extension_path = get_extension_path_from_name("isaacsim.asset.importer.urdf") + root_path = extension_path + "/data/urdf/robots/ur10/urdf" + #root_path = extension_path + "/data/urdf/robots/ur5/urdf" + file_name = "ur10.urdf" + #file_name = "ur5.urdf" + + # Parse the robot's URDF file to generate a robot model + result, self.robot_model = omni.kit.commands.execute( + "URDFParseFile", + urdf_path="{}/{}".format(root_path, file_name), + import_config=self.import_config + ) + + # Resetting the world needs to be called before querying anything related to an articulation specifically. + # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly + self.world.reset() + + print("Started Isaacsim as stand alone app...") def disconnect(self): From f34bf787a48589fbf54967e7184f932abc5629e6 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Fri, 25 Apr 2025 15:58:16 -0400 Subject: [PATCH 04/42] implemented method get_feedback to provide joint angles and joint angles vel as dictionary --- .../{isaacsim.py => nv_isaacsim.py} | 65 ++++++++++++++----- 1 file changed, 49 insertions(+), 16 deletions(-) rename abr_control/interfaces/{isaacsim.py => nv_isaacsim.py} (70%) diff --git a/abr_control/interfaces/isaacsim.py b/abr_control/interfaces/nv_isaacsim.py similarity index 70% rename from abr_control/interfaces/isaacsim.py rename to abr_control/interfaces/nv_isaacsim.py index fa4c4b00..4a5e86b0 100644 --- a/abr_control/interfaces/isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -29,9 +29,6 @@ def __init__(self, robot_config, dt=0.001): #self.misc_handles = {} # for tracking miscellaneous object handles ''' - self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles - self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities - # joint target velocities, as part of the torque limiting control # these need to be super high so that the joints are always moving # at the maximum allowed torque @@ -82,12 +79,29 @@ def connect(self, load_scene=True): urdf_path="{}/{}".format(root_path, file_name), import_config=self.import_config ) - - # Resetting the world needs to be called before querying anything related to an articulation specifically. - # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly - self.world.reset() - - + # Resetting the world needs to be called before querying anything related to an articulation specifically. + # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly + self.world.reset() + + # Get the articulation + from omni.isaac.core.articulations import Articulation, ArticulationSubset # type: ignore + from omni.isaac.core.utils.types import ArticulationAction # type: ignore + import omni.isaac.core.utils.stage as stage_utils # type: ignore + + # Import the robot onto the current stage and retrieve its prim path + result, prim_path = omni.kit.commands.execute( + "URDFImportRobot", + urdf_robot=self.robot_model, + import_config=self.import_config, + ) + print("result: ", result) + + self.world.initialize_physics() + + # Load robot + self.articulation = Articulation(prim_path=prim_path, name="ur10") + self.articulation.initialize() + print("Started Isaacsim as stand alone app...") def disconnect(self): @@ -95,6 +109,8 @@ def disconnect(self): self.simulation_app.close() # close Isaac Sim print("IsaacSim connection closed...") + + def send_forces(self, u): """Applies the set of torques u to the arm. If interfacing to a simulation, also moves dynamics forward one time step. @@ -103,7 +119,18 @@ def send_forces(self, u): An array of joint torques [Nm] """ - raise NotImplementedError + # Apply some torque + #TODO : replace with actual torque values + torque = [1.0] * len(q) + self.articulation.add_torque(torque) + + # Step the simulation + from omni.physx import _physx # type: ignore + _physx.step() + + + + def send_target_angles(self, q): """Moves the arm to the specified joint angles @@ -111,14 +138,20 @@ def send_target_angles(self, q): q : numpy.array the target joint angles [radians] """ + # Set the target angles for the joints + self.articulation.set_joint_positions(q) + return + + - raise NotImplementedError def get_feedback(self): - """Returns a dictionary of the relevant feedback + """Return a dictionary of information needed by the controller. - Returns a dictionary of relevant feedback to the - controller. At very least this contains q, dq. + Returns the joint angles and joint velocities in [rad] and [rad/sec], + respectively """ - - raise NotImplementedError + # Get the joint angles and velocities + self.q = self.articulation.get_joint_positions() + self.dq = self.articulation.get_joint_velocities() + return {"q": self.q, "dq": self.dq} From e2fe8ac8135f785e95ca76ce810dd737bb47af9b Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Mon, 28 Apr 2025 14:54:04 -0400 Subject: [PATCH 05/42] implemented sned_target_angles --- abr_control/interfaces/nv_isaacsim.py | 80 +++++++++++++++++---------- examples/isaacsim/cube.py | 3 +- examples/isaacsim/flying_cube.py | 50 +++++++++++++++++ examples/isaacsim/ur10_moving_cube.py | 27 +++++++++ 4 files changed, 130 insertions(+), 30 deletions(-) create mode 100644 examples/isaacsim/flying_cube.py create mode 100644 examples/isaacsim/ur10_moving_cube.py diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 4a5e86b0..346195eb 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -26,6 +26,12 @@ def __init__(self, robot_config, dt=0.001): self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities + self.robot_model = None + self.urdf_path = None + self.import_config = None + self.world = None + + #self.misc_handles = {} # for tracking miscellaneous object handles ''' @@ -37,23 +43,23 @@ def __init__(self, robot_config, dt=0.001): def connect(self, load_scene=True): - """All initial setup.""" - self.simulation_app = SimulationApp({"headless": False}) - from isaacsim.core.api import World # type: ignore - from isaacsim.core.utils.extensions import get_extension_path_from_name # type: ignore - from isaacsim.asset.importer.urdf import _urdf # type: ignore - import omni.kit.commands # type: ignore - import omni.usd # type: ignore - self.robot_model = None - self.urdf_path = None - self.import_config = None - self.world = None + if load_scene: + """All initial setup.""" + self.simulation_app = SimulationApp({"headless": False}) + from isaacsim.core.api import World # type: ignore + from isaacsim.core.api.objects import DynamicCuboid # type: ignore + from isaacsim.core.utils.extensions import get_extension_path_from_name # type: ignore + from isaacsim.asset.importer.urdf import _urdf # type: ignore + import omni.kit.commands # type: ignore + import omni.usd # type: ignore + import omni # Create a world self.world = World(physics_dt=self.dt,rendering_dt=self.dt) self.world.scene.add_default_ground_plane() + # Acquire the URDF extension interface for parsing and importing URDF files urdf_interface = _urdf.acquire_urdf_interface() @@ -66,6 +72,7 @@ def connect(self, load_scene=True): self.import_config.distance_scale = 1 # Set distance scale for the robot self.import_config.density = 0.0 # Set density to 0 (use default values) + # Retrieve the path of the URDF file from the extension extension_path = get_extension_path_from_name("isaacsim.asset.importer.urdf") root_path = extension_path + "/data/urdf/robots/ur10/urdf" @@ -82,28 +89,37 @@ def connect(self, load_scene=True): # Resetting the world needs to be called before querying anything related to an articulation specifically. # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly self.world.reset() + + + else: + self.world = SimulationApp.getWorld() + # Get the articulation from omni.isaac.core.articulations import Articulation, ArticulationSubset # type: ignore - from omni.isaac.core.utils.types import ArticulationAction # type: ignore import omni.isaac.core.utils.stage as stage_utils # type: ignore # Import the robot onto the current stage and retrieve its prim path - result, prim_path = omni.kit.commands.execute( + result, prim_path_ur = omni.kit.commands.execute( "URDFImportRobot", urdf_robot=self.robot_model, import_config=self.import_config, ) - print("result: ", result) - + + # necessary so self.q and self.dq are accessible self.world.initialize_physics() # Load robot - self.articulation = Articulation(prim_path=prim_path, name="ur10") + self.articulation = Articulation(prim_path=prim_path_ur, name="ur10") self.articulation.initialize() + print("Started Isaacsim as stand alone app...") + + + + def disconnect(self): """Any socket closing etc that must be done to properly shut down""" self.simulation_app.close() # close Isaac Sim @@ -118,17 +134,11 @@ def send_forces(self, u): u : np.array An array of joint torques [Nm] """ - # Apply some torque - #TODO : replace with actual torque values - torque = [1.0] * len(q) - self.articulation.add_torque(torque) - - # Step the simulation - from omni.physx import _physx # type: ignore - _physx.step() - + self.articulation.set_joint_efforts(u) + # move simulation ahead one time step + self.world.step(render=True) # execute one physics step and one rendering step @@ -139,10 +149,11 @@ def send_target_angles(self, q): the target joint angles [radians] """ # Set the target angles for the joints - self.articulation.set_joint_positions(q) - return - + from isaacsim.core.utils.types import ArticulationAction # type: ignore + self.articulation.get_articulation_controller().apply_action(ArticulationAction(q)) + # move simulation ahead one time step + self.world.step(render=True) # execute one physics step and one rendering step def get_feedback(self): @@ -155,3 +166,16 @@ def get_feedback(self): self.q = self.articulation.get_joint_positions() self.dq = self.articulation.get_joint_velocities() return {"q": self.q, "dq": self.dq} + + + def get_xyz(self, name): + """Returns the xyz position of the specified object + + name : string + name of the object you want the xyz position of + """ + #TODO check if we need misc handles for this + obj = self.world.scene.get_object(name) + object_position, object_orientation = obj.get_world_pose() + + return object_position \ No newline at end of file diff --git a/examples/isaacsim/cube.py b/examples/isaacsim/cube.py index 208fa3bb..0214fcbe 100644 --- a/examples/isaacsim/cube.py +++ b/examples/isaacsim/cube.py @@ -1,6 +1,6 @@ import numpy as np from abr_control.arms import ur5 as arm -from abr_control.interfaces.isaacsim import IsaacSim +from abr_control.interfaces.nv_isaacsim import IsaacSim from abr_control.utils import transformations # Sim step size @@ -45,5 +45,4 @@ # things run in sync world.step(render=True) # execute one physics step and one rendering step - interface.disconnect() \ No newline at end of file diff --git a/examples/isaacsim/flying_cube.py b/examples/isaacsim/flying_cube.py new file mode 100644 index 00000000..ead011eb --- /dev/null +++ b/examples/isaacsim/flying_cube.py @@ -0,0 +1,50 @@ +import numpy as np + +class FlyingCube(): + + def __init__(self, dt, world): + # IsaacSim imports must be done after instantiating the SimulationApp, which is done by the setup in the isaacsim interface + from omni.isaac.core.objects import DynamicCuboid # type: ignore + from omni.isaac.dynamic_control import _dynamic_control # type: ignore + + self.world = world + fancy_cube = self.world.scene.add( + DynamicCuboid( + prim_path="/World/random_cube", + name="fancy_cube", + position=np.array([0, 0, 1.0]), + scale=np.array([0.1015, 0.1015, 0.1015]), + color=np.array([0, 0, 1.0]), + ) + ) + # Set the cube to be dynamic using dynamic_control + self._dc = _dynamic_control.acquire_dynamic_control_interface() + self.cube_handle = self._dc.get_rigid_body("/World/random_cube") + self._cube = self.world.scene.get_object("fancy_cube") + + # Add velocity as an attribute of the world so that it is available at callback time + self.input_velocity = np.zeros(3) + self.world.add_physics_callback("send_actions", self.send_actions) + self.world.add_physics_callback("make_state", self.make_state) + + def send_actions(self, dt): + self._dc.set_rigid_body_linear_velocity(self.cube_handle, self.input_velocity) + + def make_state(self, dt): + position, orientation = self._cube.get_world_pose() + linear_velocity = self._cube.get_linear_velocity() + return { + 'position': np.array([[0., 0., 1.]]), + 'velocity': np.array([[0., 0., 0.]]), + 'orientation': np.array([[0., 0., 0., 0.]]) + } + + def step(self, velocity): + # Update the input velocity + self.input_velocity = velocity + self.world.step() + + # Get the cube's current state + position, orientation = self._cube.get_world_pose() + linear_velocity = self._cube.get_linear_velocity() + return np.concatenate([position, linear_velocity, orientation], axis=0) \ No newline at end of file diff --git a/examples/isaacsim/ur10_moving_cube.py b/examples/isaacsim/ur10_moving_cube.py new file mode 100644 index 00000000..16c617c8 --- /dev/null +++ b/examples/isaacsim/ur10_moving_cube.py @@ -0,0 +1,27 @@ +import numpy as np +from abr_control.arms import ur5 as arm +from abr_control.interfaces.nv_isaacsim import IsaacSim +from flying_cube import FlyingCube + +if __name__ == "__main__": + dt = 0.01 + + # Initialize our robot config + robot_config = arm.Config() + + # Create our interface + interface = IsaacSim(robot_config, dt=dt) + interface.connect() + + flying_cube = FlyingCube(dt, interface.world) + + interface.send_target_angles(np.array([1.0, 0.0,2.0,3.0,0.0,0.0])) + feedback = interface.get_feedback() + print ("q : " + str(feedback["q"])) + print ("dq : " + str(feedback["dq"])) + + # Example: Apply sinusoidal velocity to the cube + for t in np.arange(0, 10, dt): + velocity = np.array([np.sin(t), np.cos(t), 0.]) + state = flying_cube.step(velocity) + #print(f"Time: {t:.2f}, State: {state}") \ No newline at end of file From 657876005b9bb2fdaa9fe042f081e1dd4120ef9c Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 1 May 2025 11:48:06 -0400 Subject: [PATCH 06/42] add method to provide pose --- examples/isaacsim/flying_cube.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/isaacsim/flying_cube.py b/examples/isaacsim/flying_cube.py index ead011eb..6046ad7b 100644 --- a/examples/isaacsim/flying_cube.py +++ b/examples/isaacsim/flying_cube.py @@ -47,4 +47,7 @@ def step(self, velocity): # Get the cube's current state position, orientation = self._cube.get_world_pose() linear_velocity = self._cube.get_linear_velocity() - return np.concatenate([position, linear_velocity, orientation], axis=0) \ No newline at end of file + return np.concatenate([position, linear_velocity, orientation], axis=0) + + def _get_obj_pose(self): + return self._cube.get_world_pose() \ No newline at end of file From b953b7fa5dc0af6203fbedaf7fc44776fb6766d2 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 1 May 2025 13:52:35 -0400 Subject: [PATCH 07/42] example script to make arm follow cube --- abr_control/interfaces/nv_isaacsim.py | 66 ++++---------- examples/isaacsim/flying_cube.py | 5 +- ...sition_joint_control_inverse_kinematics.py | 89 +++++++++++++++++++ examples/isaacsim/ur5.py | 30 +++++++ 4 files changed, 140 insertions(+), 50 deletions(-) create mode 100644 examples/isaacsim/position_joint_control_inverse_kinematics.py create mode 100644 examples/isaacsim/ur5.py diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 346195eb..660fbc4a 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -1,9 +1,7 @@ import numpy as np -from abr_control.utils import download_meshes, transformations from isaacsim import SimulationApp from .interface import Interface - class IsaacSim(Interface): """An interface for IsaacSim. @@ -26,12 +24,11 @@ def __init__(self, robot_config, dt=0.001): self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities - self.robot_model = None - self.urdf_path = None self.import_config = None self.world = None - - + self.prim_path = None + self.robot = None + self.ai_link = None #self.misc_handles = {} # for tracking miscellaneous object handles ''' @@ -43,23 +40,18 @@ def __init__(self, robot_config, dt=0.001): def connect(self, load_scene=True): - - if load_scene: """All initial setup.""" self.simulation_app = SimulationApp({"headless": False}) from isaacsim.core.api import World # type: ignore - from isaacsim.core.api.objects import DynamicCuboid # type: ignore - from isaacsim.core.utils.extensions import get_extension_path_from_name # type: ignore from isaacsim.asset.importer.urdf import _urdf # type: ignore + from omni.isaac.core.robots.robot import Robot # type: ignore import omni.kit.commands # type: ignore - import omni.usd # type: ignore import omni # Create a world self.world = World(physics_dt=self.dt,rendering_dt=self.dt) self.world.scene.add_default_ground_plane() - # Acquire the URDF extension interface for parsing and importing URDF files urdf_interface = _urdf.acquire_urdf_interface() @@ -72,61 +64,40 @@ def connect(self, load_scene=True): self.import_config.distance_scale = 1 # Set distance scale for the robot self.import_config.density = 0.0 # Set density to 0 (use default values) - - # Retrieve the path of the URDF file from the extension - extension_path = get_extension_path_from_name("isaacsim.asset.importer.urdf") - root_path = extension_path + "/data/urdf/robots/ur10/urdf" - #root_path = extension_path + "/data/urdf/robots/ur5/urdf" - file_name = "ur10.urdf" - #file_name = "ur5.urdf" - - # Parse the robot's URDF file to generate a robot model - result, self.robot_model = omni.kit.commands.execute( - "URDFParseFile", - urdf_path="{}/{}".format(root_path, file_name), - import_config=self.import_config - ) - # Resetting the world needs to be called before querying anything related to an articulation specifically. - # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly - self.world.reset() - - + # Set the path to the URDF file + root_path = '/home/steffen/workspace_common/ur_description/urdf/' + file_name = 'ur5.urdf' + imported_robot = urdf_interface.parse_urdf(root_path, file_name, self.import_config) + self.prim_path = urdf_interface.import_robot(root_path, file_name, imported_robot, self.import_config, "ur5") + self.robot = Robot(self.prim_path) + self.ai_link = self.world.scene.add(self.robot) else: self.world = SimulationApp.getWorld() - # Get the articulation from omni.isaac.core.articulations import Articulation, ArticulationSubset # type: ignore import omni.isaac.core.utils.stage as stage_utils # type: ignore - - # Import the robot onto the current stage and retrieve its prim path - result, prim_path_ur = omni.kit.commands.execute( - "URDFImportRobot", - urdf_robot=self.robot_model, - import_config=self.import_config, - ) - + + # Resetting the world needs to be called before querying anything related to an articulation specifically. + # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly + self.world.reset() # necessary so self.q and self.dq are accessible self.world.initialize_physics() # Load robot - self.articulation = Articulation(prim_path=prim_path_ur, name="ur10") + self.articulation = Articulation(prim_path=self.prim_path, name="ur5") self.articulation.initialize() - print("Started Isaacsim as stand alone app...") - - def disconnect(self): """Any socket closing etc that must be done to properly shut down""" self.simulation_app.close() # close Isaac Sim print("IsaacSim connection closed...") - def send_forces(self, u): """Applies the set of torques u to the arm. If interfacing to a simulation, also moves dynamics forward one time step. @@ -148,14 +119,13 @@ def send_target_angles(self, q): q : numpy.array the target joint angles [radians] """ - # Set the target angles for the joints - from isaacsim.core.utils.types import ArticulationAction # type: ignore - self.articulation.get_articulation_controller().apply_action(ArticulationAction(q)) + self.robot.set_joint_positions(q) # move simulation ahead one time step self.world.step(render=True) # execute one physics step and one rendering step + def get_feedback(self): """Return a dictionary of information needed by the controller. diff --git a/examples/isaacsim/flying_cube.py b/examples/isaacsim/flying_cube.py index 6046ad7b..f2c6a703 100644 --- a/examples/isaacsim/flying_cube.py +++ b/examples/isaacsim/flying_cube.py @@ -49,5 +49,6 @@ def step(self, velocity): linear_velocity = self._cube.get_linear_velocity() return np.concatenate([position, linear_velocity, orientation], axis=0) - def _get_obj_pose(self): - return self._cube.get_world_pose() \ No newline at end of file + def _get_obj_pos(self): + position, orientation = self._cube.get_world_pose() + return position \ No newline at end of file diff --git a/examples/isaacsim/position_joint_control_inverse_kinematics.py b/examples/isaacsim/position_joint_control_inverse_kinematics.py new file mode 100644 index 00000000..8d98f28f --- /dev/null +++ b/examples/isaacsim/position_joint_control_inverse_kinematics.py @@ -0,0 +1,89 @@ +""" +Running the joint controller with an inverse kinematics path planner +in IsaacSim. The path planning system will generate +a trajectory in joint space that moves the end effector in a straight line +to the target, which can be moved by the user. +""" + +import numpy as np +from abr_control.arms import ur5 as arm +from abr_control.controllers import Joint, path_planners + +from abr_control.interfaces.nv_isaacsim import IsaacSim +from flying_cube import FlyingCube +from abr_control.utils import transformations + + +if __name__ == "__main__": + dt = 0.001 + + # change this flag to False to use position control + use_force_control = False + # Initialize our robot config + robot_config = arm.Config() + if use_force_control: + # create an operational space controller + ctrlr = Joint(robot_config, kp=300, kv=20) + + # create our path planner + n_timesteps = 2000 + path_planner = path_planners.InverseKinematics(robot_config) + + # Create our interface + interface = IsaacSim(robot_config, dt=dt) + interface.connect() + + flying_cube = FlyingCube(dt, interface.world) + feedback = interface.get_feedback() + print ("q : " + str(feedback["q"])) + print ("dq : " + str(feedback["dq"])) + + count = 0 + for t in np.arange(0, 100, dt): + # get arm feedback + #velocity = np.array([np.sin(t), np.cos(t), 0.]) + #state = flying_cube.step(velocity) + + feedback = interface.get_feedback() + hand_xyz = robot_config.Tx("EE", feedback["q"]) + + if count % n_timesteps == 0: + # get pos of flying cube + target_xyz = flying_cube._get_obj_pos() + # print('target_xyz ', target_xyz) + + R = robot_config.R("EE", q=feedback["q"]) + target_orientation = transformations.euler_from_matrix(R, "sxyz") + + # can use 3 different methods to calculate inverse kinematics + # see inverse_kinematics.py file for details + path_planner.generate_path( + position=feedback["q"], + target_position=np.hstack([target_xyz, target_orientation]), + method=3, + dt=0.005, + n_timesteps=n_timesteps, + plot=False, + ) + + # returns desired [position, velocity] + target, _ = path_planner.next() + if use_force_control: + # generate an operational space control signal + u = ctrlr.generate( + q=feedback["q"], + dq=feedback["dq"], + target=target, + ) + + # apply the control signal, step the sim forward + interface.send_forces(u) + + else: + # use position control + interface.send_target_angles(target[: robot_config.N_JOINTS]) + + count += 1 + # stop and reset the simulation + interface.disconnect() + print("Simulation terminated...") \ No newline at end of file diff --git a/examples/isaacsim/ur5.py b/examples/isaacsim/ur5.py new file mode 100644 index 00000000..4527942a --- /dev/null +++ b/examples/isaacsim/ur5.py @@ -0,0 +1,30 @@ +import numpy as np +from abr_control.arms import ur5 as arm +from abr_control.interfaces.nv_isaacsim import IsaacSim +from abr_control.utils import transformations + +# Sim step size +dt = 0.005 + +# Initialize our robot config +robot_config = arm.Config() + +# Create our interface +interface = IsaacSim(robot_config, dt=dt) +interface.connect() +feedback = interface.get_feedback() +print ("q : " + str(feedback["q"])) +print ("dq : " + str(feedback["dq"])) + +count = 0 + +for i in range(5000): + val = count % 6 + interface.send_target_angles(np.array([val, 5.6,1.0,0.0,0.0,0.0])) + + count += 1 + import time + time.sleep(5) + + +interface.disconnect() \ No newline at end of file From a2ca8382709330f2e78d6a07be3a7ff9c3d9f76f Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 6 May 2025 08:52:14 -0400 Subject: [PATCH 08/42] added methods getOreintation() and set_xyz to isaac interface --- abr_control/interfaces/nv_isaacsim.py | 41 ++++- examples/isaacsim/blog_minimal.py | 41 +++++ examples/isaacsim/flying_cube.py | 21 ++- ...rce_osc_xyz_avoid_obstacle path_planner.py | 173 ++++++++++++++++++ .../isaacsim/force_osc_xyz_avoid_obstacle.py | 146 +++++++++++++++ 5 files changed, 408 insertions(+), 14 deletions(-) create mode 100644 examples/isaacsim/blog_minimal.py create mode 100644 examples/isaacsim/force_osc_xyz_avoid_obstacle path_planner.py create mode 100644 examples/isaacsim/force_osc_xyz_avoid_obstacle.py diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 660fbc4a..abd33db2 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -105,8 +105,10 @@ def send_forces(self, u): u : np.array An array of joint torques [Nm] """ - # Apply some torque - self.articulation.set_joint_efforts(u) + # Apply the control signal + #self.articulation.set_joint_efforts(u) + #self.robot.set_joint_efforts(u) + self.robot.set_joint_efforts(u) # move simulation ahead one time step self.world.step(render=True) # execute one physics step and one rendering step @@ -119,8 +121,10 @@ def send_target_angles(self, q): q : numpy.array the target joint angles [radians] """ + print("robot joint pos: ", self.robot.get_joint_positions()) + print("q: ", q) self.robot.set_joint_positions(q) - + # move simulation ahead one time step self.world.step(render=True) # execute one physics step and one rendering step @@ -148,4 +152,33 @@ def get_xyz(self, name): obj = self.world.scene.get_object(name) object_position, object_orientation = obj.get_world_pose() - return object_position \ No newline at end of file + return object_position + + + def get_orientation(self, name): + """Returns the orientation of an object in CoppeliaSim + + the Euler angles [radians] are returned in the relative xyz frame. + http://www.coppeliarobotics.com/helpFiles/en/eulerAngles.htm + + Parameters + ---------- + name : string + the name of the object of interest + """ + + obj = self.world.scene.get_object(name) + object_position, object_orientation = obj.get_world_pose() + return object_orientation + + + def set_xyz(self, name, xyz): + """Set the position of an object in the environment. + + name : string + the name of the object + xyz : np.array + the [x,y,z] location of the target [meters] + """ + _cube = self.world.scene.get_object(name) + _cube.set_world_pose(xyz, np.array([0., 0., 0., 1.])) # set the position and orientation of the object diff --git a/examples/isaacsim/blog_minimal.py b/examples/isaacsim/blog_minimal.py new file mode 100644 index 00000000..73e8bb20 --- /dev/null +++ b/examples/isaacsim/blog_minimal.py @@ -0,0 +1,41 @@ +import numpy as np +from abr_control.arms import ur5 as arm +from abr_control.controllers import OSC +from abr_control.interfaces.nv_isaacsim import IsaacSim +from flying_cube import FlyingCube + +# https://studywolf.wordpress.com/2017/07/01/abr-control-0-1-repo-public-release/ + + +# initialize our robot config for the ur5 +robot_config = arm.Config(use_cython=True) + +# instantiate controller +ctrlr = OSC(robot_config, kp=200, vmax=[0.5, 0]) + + +# create our VREP interface +interface = IsaacSim(robot_config, dt=.001) +interface.connect() + +isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") +target_xyz = np.array([0.2, 0.2, 0.2]) +# set the target object's position in VREP +interface.set_xyz(name='target', xyz=target_xyz) + +count = 0.0 +while count < 4000: # run for 1.5 simulated seconds + # get joint angle and velocity feedback + feedback = interface.get_feedback() + # calculate the control signal + u = ctrlr.generate( + q=feedback['q'], + dq=feedback['dq'], + target=target_xyz) + + + # send forces into VREP, step the sim forward + interface.send_forces(u) + + count += 1 +interface.disconnect() \ No newline at end of file diff --git a/examples/isaacsim/flying_cube.py b/examples/isaacsim/flying_cube.py index f2c6a703..fe9542f4 100644 --- a/examples/isaacsim/flying_cube.py +++ b/examples/isaacsim/flying_cube.py @@ -2,7 +2,7 @@ class FlyingCube(): - def __init__(self, dt, world): + def __init__(self, dt, world, prim_path="/World/random_cube", name="fancy_cube", position=np.array([0, 0, 1.0]), scale=np.array([0.1015, 0.1015, 0.1015]), color=np.array([0, 0, 1.0])): # IsaacSim imports must be done after instantiating the SimulationApp, which is done by the setup in the isaacsim interface from omni.isaac.core.objects import DynamicCuboid # type: ignore from omni.isaac.dynamic_control import _dynamic_control # type: ignore @@ -10,22 +10,23 @@ def __init__(self, dt, world): self.world = world fancy_cube = self.world.scene.add( DynamicCuboid( - prim_path="/World/random_cube", - name="fancy_cube", - position=np.array([0, 0, 1.0]), - scale=np.array([0.1015, 0.1015, 0.1015]), - color=np.array([0, 0, 1.0]), + prim_path=prim_path, + name=name, + position=position, + scale=scale, + color=color, ) ) # Set the cube to be dynamic using dynamic_control self._dc = _dynamic_control.acquire_dynamic_control_interface() - self.cube_handle = self._dc.get_rigid_body("/World/random_cube") - self._cube = self.world.scene.get_object("fancy_cube") + self.cube_handle = self._dc.get_rigid_body(prim_path) + self._cube = self.world.scene.get_object(name) # Add velocity as an attribute of the world so that it is available at callback time self.input_velocity = np.zeros(3) - self.world.add_physics_callback("send_actions", self.send_actions) - self.world.add_physics_callback("make_state", self.make_state) + + self.world.add_physics_callback(name + "_send_actions", self.send_actions) + self.world.add_physics_callback(name + "_make_state", self.make_state) def send_actions(self, dt): self._dc.set_rigid_body_linear_velocity(self.cube_handle, self.input_velocity) diff --git a/examples/isaacsim/force_osc_xyz_avoid_obstacle path_planner.py b/examples/isaacsim/force_osc_xyz_avoid_obstacle path_planner.py new file mode 100644 index 00000000..4dee680f --- /dev/null +++ b/examples/isaacsim/force_osc_xyz_avoid_obstacle path_planner.py @@ -0,0 +1,173 @@ +""" +Move the UR5 CoppeliaSim arm to a target position while avoiding an obstacle. +The simulation ends after 1500 time steps, and the trajectory +of the end-effector is plotted in 3D. +""" +import numpy as np +from flying_cube import FlyingCube +from abr_control.arms import ur5 as arm +from abr_control.controllers import Joint, path_planners +from abr_control.utils import transformations + + + +from abr_control.controllers import OSC, AvoidObstacles, Damping +from abr_control.interfaces.nv_isaacsim import IsaacSim + +# initialize our robot config +robot_config = arm.Config() +# change this flag to False to use position control +use_force_control = False +n_timesteps = 2000 +path_planner = path_planners.InverseKinematics(robot_config) + + + +avoid = AvoidObstacles(robot_config) +# damp the movements of the arm +if use_force_control: + # create an operational space controller + damping = Damping(robot_config, kv=10) + # instantiate the REACH controller with obstacle avoidance + ctrlr = OSC( + robot_config, + kp=200, + null_controllers=[avoid, damping], + vmax=[0.5, 0], # [m/s, rad/s] + # control (x, y, z) out of [x, y, z, alpha, beta, gamma] + ctrlr_dof=[True, True, True, False, False, False], + ) + +# create our CoppeliaSim interface +interface = IsaacSim(robot_config, dt=0.005) +interface.connect() + +# set up lists for tracking data +ee_track = [] +target_track = [] +obstacle_track = [] + +moving_obstacle = True +isaac_obstacle = FlyingCube(interface.dt, interface.world, name ="obstacle", prim_path="/World/obstacle", color=np.array([0, 1.0, 0])) +isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") +obstacle_xyz = np.array([0.09596, -0.2661, 0.64204]) +try: + # get visual position of end point of object + feedback = interface.get_feedback() + start = robot_config.Tx("EE", q=feedback["q"]) + # make the target offset from that start position + target_xyz = start + np.array([0.2, -0.2, 0.0]) + interface.set_xyz(name="target", xyz=target_xyz) + interface.set_xyz(name="obstacle", xyz=obstacle_xyz) + + + count = 0.0 + obs_count = 0.0 + print("\nSimulation starting...\n") + + while count < n_timesteps : + # get joint angle and velocity feedback + feedback = interface.get_feedback() + #target = np.hstack( + # [interface.get_xyz("target"), interface.get_orientation("target")] + #) + + # position control + R = robot_config.R("EE", q=feedback["q"]) + target_orientation = transformations.euler_from_matrix(R, "sxyz") + # can use 3 different methods to calculate inverse kinematics + # see inverse_kinematics.py file for details + path_planner.generate_path( + position=feedback["q"], + target_position=np.hstack(( + [interface.get_xyz("target"), interface.get_orientation("target")])), + method=3, + plot=False, + ) + # returns desired [position, velocity] + target, _ = path_planner.next() + + + + # get obstacle position from IsaacSim + obs_x, obs_y, obs_z = interface.get_xyz("obstacle") # pylint: disable=W0632 + # update avoidance system about obstacle position + avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]]) + if moving_obstacle is True: + obs_x = 0.125 + 0.25 * np.sin(obs_count) + obs_count += 0.05 + interface.set_xyz(name="obstacle", xyz=[obs_x, obs_y, obs_z]) + + if use_force_control: + # calculate the control signal + u = ctrlr.generate( + q=feedback["q"], + dq=feedback["dq"], + target=target, + ) + # send forces into IsaacSim, step the sim forward + interface.send_forces(u) + else: + # use position control, send angles into IsaacSim + interface.send_target_angles(target[: robot_config.N_JOINTS]) + + + + + #print( 'robot_config: ' , robot_config) + #print( 'robot_config.N_JOINTS: ' , robot_config.N_JOINTS) + #print( 'target[: robot_config.N_JOINTS]: ' , target[: robot_config.N_JOINTS]) + + + # calculate end-effector position + ee_xyz = robot_config.Tx("EE", q=feedback["q"]) + # track data + ee_track.append(np.copy(ee_xyz)) + target_track.append(np.copy(target[:3])) + obstacle_track.append(np.copy([obs_x, obs_y, obs_z])) + + count += 1 + +finally: + # stop and reset IsaacSim + interface.disconnect() + + print("Simulation terminated...") + + ee_track = np.array(ee_track) + target_track = np.array(target_track) + obstacle_track = np.array(obstacle_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[0, 0], + target_track[0, 1], + target_track[0, 2], + label="target", + c="g", + ) + ax2.plot( + obstacle_track[:, 0], + obstacle_track[:, 1], + target_track[:, 2], + label="obstacle", + c="r", + ) + ax2.legend() + plt.show() diff --git a/examples/isaacsim/force_osc_xyz_avoid_obstacle.py b/examples/isaacsim/force_osc_xyz_avoid_obstacle.py new file mode 100644 index 00000000..505cea76 --- /dev/null +++ b/examples/isaacsim/force_osc_xyz_avoid_obstacle.py @@ -0,0 +1,146 @@ +""" +Move the UR5 CoppeliaSim arm to a target position while avoiding an obstacle. +The simulation ends after 1500 time steps, and the trajectory +of the end-effector is plotted in 3D. +""" +import numpy as np +from flying_cube import FlyingCube +from abr_control.arms import ur5 as arm + +from abr_control.controllers import OSC, AvoidObstacles, Damping +from abr_control.interfaces.nv_isaacsim import IsaacSim + +# initialize our robot config +robot_config = arm.Config() +# change this flag to False to use position control +use_force_control = False +n_timesteps = 2000 + + +avoid = AvoidObstacles(robot_config) +# damp the movements of the arm +if use_force_control: + # create an operational space controller + damping = Damping(robot_config, kv=10) + # instantiate the REACH controller with obstacle avoidance + ctrlr = OSC( + robot_config, + kp=200, + null_controllers=[avoid, damping], + vmax=[0.5, 0], # [m/s, rad/s] + # control (x, y, z) out of [x, y, z, alpha, beta, gamma] + ctrlr_dof=[True, True, True, False, False, False], + ) + +# create our CoppeliaSim interface +interface = IsaacSim(robot_config, dt=0.005) +interface.connect() + +# set up lists for tracking data +ee_track = [] +target_track = [] +obstacle_track = [] + +moving_obstacle = True +isaac_obstacle = FlyingCube(interface.dt, interface.world, name ="obstacle", prim_path="/World/obstacle", color=np.array([0, 1.0, 0])) +isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") +obstacle_xyz = np.array([0.09596, -0.2661, 0.64204]) +try: + # get visual position of end point of object + feedback = interface.get_feedback() + start = robot_config.Tx("EE", q=feedback["q"]) + # make the target offset from that start position + target_xyz = start + np.array([0.2, -0.2, 0.0]) + interface.set_xyz(name="target", xyz=target_xyz) + interface.set_xyz(name="obstacle", xyz=obstacle_xyz) + + + count = 0.0 + obs_count = 0.0 + print("\nSimulation starting...\n") + + while count < n_timesteps : + # get joint angle and velocity feedback + feedback = interface.get_feedback() + target = np.hstack( + [interface.get_xyz("target"), interface.get_orientation("target")] + ) + + # get obstacle position from IsaacSim + obs_x, obs_y, obs_z = interface.get_xyz("obstacle") # pylint: disable=W0632 + # update avoidance system about obstacle position + avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]]) + if moving_obstacle is True: + obs_x = 0.125 + 0.25 * np.sin(obs_count) + obs_count += 0.05 + interface.set_xyz(name="obstacle", xyz=[obs_x, obs_y, obs_z]) + + if use_force_control: + # calculate the control signal + u = ctrlr.generate( + q=feedback["q"], + dq=feedback["dq"], + target=target, + ) + # send forces into IsaacSim, step the sim forward + interface.send_forces(u) + else: + # use position control, send angles into IsaacSim + interface.send_target_angles(target[: robot_config.N_JOINTS]) + print( 'robot_config: ' , robot_config) + print( 'robot_config.N_JOINTS: ' , robot_config.N_JOINTS) + print( 'target[: robot_config.N_JOINTS]: ' , target[: robot_config.N_JOINTS]) + + + # calculate end-effector position + ee_xyz = robot_config.Tx("EE", q=feedback["q"]) + # track data + ee_track.append(np.copy(ee_xyz)) + target_track.append(np.copy(target[:3])) + obstacle_track.append(np.copy([obs_x, obs_y, obs_z])) + + count += 1 + +finally: + # stop and reset IsaacSim + interface.disconnect() + + print("Simulation terminated...") + + ee_track = np.array(ee_track) + target_track = np.array(target_track) + obstacle_track = np.array(obstacle_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[0, 0], + target_track[0, 1], + target_track[0, 2], + label="target", + c="g", + ) + ax2.plot( + obstacle_track[:, 0], + obstacle_track[:, 1], + target_track[:, 2], + label="obstacle", + c="r", + ) + ax2.legend() + plt.show() From 0362a48f6c3a70c4a17cb8ce6e37d4641f67ecf3 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 13 May 2025 11:20:05 -0400 Subject: [PATCH 09/42] switched urdf import to xml import --- abr_control/interfaces/nv_isaacsim.py | 82 +++++--- .../interfaces/nv_isaacsim_usd_jaco.py | 182 ++++++++++++++++++ examples/isaacsim/jaco_2.py | 37 ++++ ...sition_joint_control_inverse_kinematics.py | 100 +++++----- 4 files changed, 317 insertions(+), 84 deletions(-) create mode 100644 abr_control/interfaces/nv_isaacsim_usd_jaco.py create mode 100644 examples/isaacsim/jaco_2.py diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index abd33db2..17b82531 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -1,6 +1,8 @@ import numpy as np +import os from isaacsim import SimulationApp from .interface import Interface +from abr_control.utils import download_meshes, transformations class IsaacSim(Interface): """An interface for IsaacSim. @@ -17,7 +19,6 @@ class IsaacSim(Interface): def __init__(self, robot_config, dt=0.001): super().__init__(robot_config) - self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called @@ -39,43 +40,42 @@ def __init__(self, robot_config, dt=0.001): ''' - def connect(self, load_scene=True): + def connect(self, load_scene=True, force_download=False): if load_scene: """All initial setup.""" self.simulation_app = SimulationApp({"headless": False}) from isaacsim.core.api import World # type: ignore from isaacsim.asset.importer.urdf import _urdf # type: ignore from omni.isaac.core.robots.robot import Robot # type: ignore + from omni.isaac.core.utils.stage import add_reference_to_stage # type: ignore import omni.kit.commands # type: ignore import omni # Create a world self.world = World(physics_dt=self.dt,rendering_dt=self.dt) self.world.scene.add_default_ground_plane() + + # setting up import configuration: + status, import_config = omni.kit.commands.execute("MJCFCreateImportConfig") + import_config.set_fix_base(False) + import_config.set_make_default_prim(False) + + file_name = self.robot_config.filename.split(".")[0] + self.xml_file = os.path.join( f"{file_name}.xml") + + omni.kit.commands.execute( + "MJCFCreateAsset", + mjcf_path=self.xml_file, + import_config=import_config, + prim_path="/UR5" + ) + - # Acquire the URDF extension interface for parsing and importing URDF files - urdf_interface = _urdf.acquire_urdf_interface() - - # Configure the settings for importing the URDF file - self.import_config = _urdf.ImportConfig() - self.import_config.convex_decomp = False # Disable convex decomposition for simplicity - self.import_config.fix_base = True # Fix the base of the robot to the ground - self.import_config.make_default_prim = True # Make the robot the default prim in the scene - self.import_config.self_collision = False # Disable self-collision for performance - self.import_config.distance_scale = 1 # Set distance scale for the robot - self.import_config.density = 0.0 # Set density to 0 (use default values) - - # Set the path to the URDF file - root_path = '/home/steffen/workspace_common/ur_description/urdf/' - file_name = 'ur5.urdf' - imported_robot = urdf_interface.parse_urdf(root_path, file_name, self.import_config) - self.prim_path = urdf_interface.import_robot(root_path, file_name, imported_robot, self.import_config, "ur5") - self.robot = Robot(self.prim_path) - self.ai_link = self.world.scene.add(self.robot) else: self.world = SimulationApp.getWorld() # Get the articulation from omni.isaac.core.articulations import Articulation, ArticulationSubset # type: ignore + import omni.isaac.core.utils.stage as stage_utils # type: ignore # Resetting the world needs to be called before querying anything related to an articulation specifically. @@ -84,11 +84,12 @@ def connect(self, load_scene=True): # necessary so self.q and self.dq are accessible self.world.initialize_physics() + # Load robot - self.articulation = Articulation(prim_path=self.prim_path, name="ur5") - self.articulation.initialize() - - print("Started Isaacsim as stand alone app...") + #self.articulation = Articulation(prim_path=self.prim_path, name="ur5") + #self.articulation.initialize() + #print("DOF names:", self.articulation.dof_names) + @@ -105,6 +106,8 @@ def send_forces(self, u): u : np.array An array of joint torques [Nm] """ + print("robot joint pos: ", self.robot.get_joint_positions()) + print("u: ", u) # Apply the control signal #self.articulation.set_joint_efforts(u) #self.robot.set_joint_efforts(u) @@ -121,10 +124,27 @@ def send_target_angles(self, q): q : numpy.array the target joint angles [radians] """ - print("robot joint pos: ", self.robot.get_joint_positions()) - print("q: ", q) - self.robot.set_joint_positions(q) + # print("robot joint pos: ", self.robot.get_joint_positions()) + #print("q: ", q) + + # works well + #self.articulation.set_joint_positions(q) + + + # works but just sets robot abruptly to the target angles + #self.robot.set_joint_positions(q) + + + # WORKS AND MOVES ROBOT SLOWLY, issues with time import + from omni.isaac.core.utils.types import ArticulationAction # type: ignore + #self.articulation.apply_action(ArticulationAction(q)) + #self.articulation.apply_action(ArticulationAction(np.array([1.0, 0.0,2.0,3.0,0.0,0.0]))) + + + + + #dof_ptr = self.dci.find_articulation_dof(art, "panda_joint2") # move simulation ahead one time step self.world.step(render=True) # execute one physics step and one rendering step @@ -137,9 +157,9 @@ def get_feedback(self): respectively """ # Get the joint angles and velocities - self.q = self.articulation.get_joint_positions() - self.dq = self.articulation.get_joint_velocities() - return {"q": self.q, "dq": self.dq} + #self.q = self.articulation.get_joint_positions() + #self.dq = self.articulation.get_joint_velocities() + #return {"q": self.q, "dq": self.dq} def get_xyz(self, name): diff --git a/abr_control/interfaces/nv_isaacsim_usd_jaco.py b/abr_control/interfaces/nv_isaacsim_usd_jaco.py new file mode 100644 index 00000000..f886fbe9 --- /dev/null +++ b/abr_control/interfaces/nv_isaacsim_usd_jaco.py @@ -0,0 +1,182 @@ +import numpy as np +from isaacsim import SimulationApp +from .interface import Interface + +class IsaacSim(Interface): + """An interface for IsaacSim. + + Parameters + ---------- + robot_config : class instance + contains all relevant information about the arm + such as: number of joints, number of links, mass information etc. + dt : float, optional (Default: 0.001) + simulation time step in seconds + + """ + def __init__(self, robot_config, dt=0.001, force_download=False): + + super().__init__(robot_config) + self.dt = dt # time step + self.count = 0 # keep track of how many times send forces is called + + self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles + self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities + + self.prim_path = "/World/robot" + self.name = "robot" + + #self.misc_handles = {} # for tracking miscellaneous object handles + + + def connect(self, load_scene=True): + if load_scene: + """All initial setup.""" + self.simulation_app = SimulationApp({"headless": False}) + from isaacsim.core.api import World # type: ignore + import omni.isaac.core.utils.stage as stage_utils # type: ignore + import omni.kit.commands # type: ignore + import omni + from isaacsim.storage.native import get_assets_root_path + from isaacsim.core.api.robots import Robot + + # Create a world + self.world = World(physics_dt=self.dt,rendering_dt=self.dt) + self.world.scene.add_default_ground_plane() + + assets_root_path = get_assets_root_path() + + stage_utils.add_reference_to_stage( + usd_path=assets_root_path + "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd", + # Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd --> 7 DOF arm , not compatible with ABR controller + prim_path=self.prim_path, + ) + + robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) + + + else: + self.world = SimulationApp.getWorld() + + # Get the articulation + from omni.isaac.core.articulations import Articulation# type: ignore + + # Resetting the world needs to be called before querying anything related to an articulation specifically. + # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly + self.world.reset() + # Load robot + self.articulation = Articulation(prim_path=self.prim_path, name="H1") + self.articulation.initialize() + print("DOF names:", self.articulation.dof_names) + + # necessary so self.q and self.dq are accessible + self.world.initialize_physics() + + + + def disconnect(self): + """Any socket closing etc that must be done to properly shut down""" + self.simulation_app.close() # close Isaac Sim + print("IsaacSim connection closed...") + + + def send_forces(self, u): + """Applies the set of torques u to the arm. If interfacing to + a simulation, also moves dynamics forward one time step. + + u : np.array + An array of joint torques [Nm] + """ + print("robot joint pos: ", self.robot.get_joint_positions()) + print("u: ", u) + # Apply the control signal + #self.articulation.set_joint_efforts(u) + #self.robot.set_joint_efforts(u) + self.robot.set_joint_efforts(u) + + # move simulation ahead one time step + self.world.step(render=True) # execute one physics step and one rendering step + + + + def send_target_angles(self, q): + """Moves the arm to the specified joint angles + + q : numpy.array + the target joint angles [radians] + """ + if np.size(q) == 12: + self.articulation.set_joint_positions(q) + if np.size(q) == 6: + fb = self.get_feedback(with_hand=True) + fb_q = fb["q"] + fb_q[:6] = q + self.articulation.set_joint_positions(fb_q) + else: + print("The method send_target_angles of the isaacsim interface does not support the number of joint angles that are attempted to be set.") + + # move simulation ahead one time step + self.world.step(render=True) # execute one physics step and one rendering step + + + + def get_feedback(self, with_hand=False): + """Return a dictionary of information needed by the controller. + + Returns the joint angles and joint velocities in [rad] and [rad/sec], + respectively + """ + q = self.articulation.get_joint_positions() + dq = self.articulation.get_joint_velocities() + if with_hand: + # Get the joint angles and velocities + self.q = q + self.dq = dq + else: + self.q = q[:6] + self.dq = dq[:6] + return {"q": self.q, "dq": self.dq} + + + + def get_xyz(self, name): + """Returns the xyz position of the specified object + + name : string + name of the object you want the xyz position of + """ + print("NAME: ", name) + #TODO check if we need misc handles for this + obj = self.world.scene.get_object(name) + object_position, object_orientation = obj.get_world_pose() + + return object_position + + + def get_orientation(self, name): + """Returns the orientation of an object in CoppeliaSim + + the Euler angles [radians] are returned in the relative xyz frame. + http://www.coppeliarobotics.com/helpFiles/en/eulerAngles.htm + + Parameters + ---------- + name : string + the name of the object of interest + """ + + obj = self.world.scene.get_object(name) + object_position, object_orientation = obj.get_world_pose() + return object_orientation + + + def set_xyz(self, name, xyz): + """Set the position of an object in the environment. + + name : string + the name of the object + xyz : np.array + the [x,y,z] location of the target [meters] + """ + _cube = self.world.scene.get_object(name) + _cube.set_world_pose(xyz, np.array([0., 0., 0., 1.])) # set the position and orientation of the object diff --git a/examples/isaacsim/jaco_2.py b/examples/isaacsim/jaco_2.py new file mode 100644 index 00000000..39710038 --- /dev/null +++ b/examples/isaacsim/jaco_2.py @@ -0,0 +1,37 @@ +""" +Running the joint controller with an inverse kinematics path planner +for a Mujoco simulation. The path planning system will generate +a trajectory in joint space that moves the end effector in a straight line +to the target, which changes every n time steps. +""" +import numpy as np +from abr_control.arms import jaco2 as arm +from abr_control.interfaces.nv_isaacsim_usd_jaco import IsaacSim + + +robot_config = arm.Config() + +# create our path planner +n_timesteps = 2000 +# create our interface +dt = 0.001 +interface = IsaacSim(robot_config, dt=dt) +interface.connect() +feedback = interface.get_feedback() + +try: + print("\nSimulation starting...") + count = 0 + while 1: + + # use position control + #interface.send_target_angles(target[: robot_config.N_JOINTS]) + q=feedback["q"] + interface.send_target_angles(q) + count += 1 + +finally: + # stop and reset the simulation + interface.disconnect() + + print("Simulation terminated...") \ No newline at end of file diff --git a/examples/isaacsim/position_joint_control_inverse_kinematics.py b/examples/isaacsim/position_joint_control_inverse_kinematics.py index 8d98f28f..5296d90c 100644 --- a/examples/isaacsim/position_joint_control_inverse_kinematics.py +++ b/examples/isaacsim/position_joint_control_inverse_kinematics.py @@ -1,60 +1,61 @@ """ Running the joint controller with an inverse kinematics path planner -in IsaacSim. The path planning system will generate +for a Mujoco simulation. The path planning system will generate a trajectory in joint space that moves the end effector in a straight line -to the target, which can be moved by the user. +to the target, which changes every n time steps. """ - import numpy as np -from abr_control.arms import ur5 as arm -from abr_control.controllers import Joint, path_planners - -from abr_control.interfaces.nv_isaacsim import IsaacSim -from flying_cube import FlyingCube +#from abr_control.arms.mujoco_config import MujocoConfig as arm +from abr_control.arms import jaco2 as arm +#import glfw +from abr_control.controllers import path_planners +#from abr_control.interfaces.mujoco import Mujoco +from abr_control.interfaces.nv_isaacsim_usd_jaco import IsaacSim from abr_control.utils import transformations +from flying_cube import FlyingCube -if __name__ == "__main__": - dt = 0.001 +#robot_config = arm("ur5", use_sim_state=False) +robot_config = arm.Config() - # change this flag to False to use position control - use_force_control = False - # Initialize our robot config - robot_config = arm.Config() - if use_force_control: - # create an operational space controller - ctrlr = Joint(robot_config, kp=300, kv=20) - # create our path planner - n_timesteps = 2000 - path_planner = path_planners.InverseKinematics(robot_config) +# create our path planner +n_timesteps = 2000 +path_planner = path_planners.InverseKinematics(robot_config) - # Create our interface - interface = IsaacSim(robot_config, dt=dt) - interface.connect() +# create our interface +dt = 0.001 +interface = IsaacSim(robot_config, dt=dt) +interface.connect() +interface.send_target_angles(robot_config.START_ANGLES) +feedback = interface.get_feedback() +isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") - flying_cube = FlyingCube(dt, interface.world) - feedback = interface.get_feedback() - print ("q : " + str(feedback["q"])) - print ("dq : " + str(feedback["dq"])) - count = 0 - for t in np.arange(0, 100, dt): - # get arm feedback - #velocity = np.array([np.sin(t), np.cos(t), 0.]) - #state = flying_cube.step(velocity) +try: + print("\nSimulation starting...") + print("Click to move the target.\n") - feedback = interface.get_feedback() - hand_xyz = robot_config.Tx("EE", feedback["q"]) + count = 0 + while 1: if count % n_timesteps == 0: - # get pos of flying cube - target_xyz = flying_cube._get_obj_pos() - # print('target_xyz ', target_xyz) - + print("Moving target...") + feedback = interface.get_feedback() + target_xyz = np.array( + [ + np.random.random() * 0.5 - 0.25, + np.random.random() * 0.5 - 0.25, + np.random.random() * 0.5 + 0.5, + ] + ) R = robot_config.R("EE", q=feedback["q"]) - target_orientation = transformations.euler_from_matrix(R, "sxyz") + #R = robot_config.R("wrist_3_joint", q=feedback["q"]) + target_orientation = transformations.euler_from_matrix(R, "sxyz") + # update the position of the target + interface.set_xyz("target", target_xyz) + # can use 3 different methods to calculate inverse kinematics # see inverse_kinematics.py file for details path_planner.generate_path( @@ -67,23 +68,16 @@ ) # returns desired [position, velocity] - target, _ = path_planner.next() - if use_force_control: - # generate an operational space control signal - u = ctrlr.generate( - q=feedback["q"], - dq=feedback["dq"], - target=target, - ) + target = path_planner.next()[0] - # apply the control signal, step the sim forward - interface.send_forces(u) - - else: - # use position control - interface.send_target_angles(target[: robot_config.N_JOINTS]) + # use position control + interface.send_target_angles(target[: robot_config.N_JOINTS]) + #interface.viewer.render() count += 1 + +finally: # stop and reset the simulation interface.disconnect() + print("Simulation terminated...") \ No newline at end of file From 6e1e766b907b5d11afbde7312301f2adc05cb987 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Fri, 16 May 2025 19:36:28 -0400 Subject: [PATCH 10/42] interface adapted to h1, locked robote pelvis orientation so it does not fall --- abr_control/interfaces/nv_isaacsim.py | 135 +++++++++++++++-------- examples/isaacsim/h1_lock_orientation.py | 40 +++++++ 2 files changed, 128 insertions(+), 47 deletions(-) create mode 100644 examples/isaacsim/h1_lock_orientation.py diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 17b82531..51f64ef6 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -1,8 +1,7 @@ import numpy as np -import os from isaacsim import SimulationApp from .interface import Interface -from abr_control.utils import download_meshes, transformations + class IsaacSim(Interface): """An interface for IsaacSim. @@ -16,7 +15,7 @@ class IsaacSim(Interface): simulation time step in seconds """ - def __init__(self, robot_config, dt=0.001): + def __init__(self, robot_config, dt=0.001, force_download=False): super().__init__(robot_config) self.dt = dt # time step @@ -25,70 +24,73 @@ def __init__(self, robot_config, dt=0.001): self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities - self.import_config = None - self.world = None - self.prim_path = None - self.robot = None - self.ai_link = None - #self.misc_handles = {} # for tracking miscellaneous object handles + self.prim_path = "/World/robot" + self.name = "robot" - ''' - # joint target velocities, as part of the torque limiting control - # these need to be super high so that the joints are always moving - # at the maximum allowed torque - self.joint_target_velocities = np.ones(robot_config.N_JOINTS) * 10000.0 - ''' + + #self.misc_handles = {} # for tracking miscellaneous object handles - def connect(self, load_scene=True, force_download=False): + def connect(self, load_scene=True): if load_scene: """All initial setup.""" self.simulation_app = SimulationApp({"headless": False}) from isaacsim.core.api import World # type: ignore - from isaacsim.asset.importer.urdf import _urdf # type: ignore - from omni.isaac.core.robots.robot import Robot # type: ignore - from omni.isaac.core.utils.stage import add_reference_to_stage # type: ignore + import omni.isaac.core.utils.stage as stage_utils # type: ignore import omni.kit.commands # type: ignore import omni + from isaacsim.robot.policy.examples.robots.h1 import H1FlatTerrainPolicy + from isaacsim.storage.native import get_assets_root_path + + # Create a world self.world = World(physics_dt=self.dt,rendering_dt=self.dt) + self.stage = omni.usd.get_context().get_stage() + self.world.add_physics_callback("send_actions", self.send_actions) self.world.scene.add_default_ground_plane() - # setting up import configuration: - status, import_config = omni.kit.commands.execute("MJCFCreateImportConfig") - import_config.set_fix_base(False) - import_config.set_make_default_prim(False) - - file_name = self.robot_config.filename.split(".")[0] - self.xml_file = os.path.join( f"{file_name}.xml") - - omni.kit.commands.execute( - "MJCFCreateAsset", - mjcf_path=self.xml_file, - import_config=import_config, - prim_path="/UR5" + + + assets_root_path = get_assets_root_path() + + + self.h1 = H1FlatTerrainPolicy( + prim_path=self.prim_path, + name=self.name, + usd_path=assets_root_path + "/Isaac/Robots/Unitree/H1/h1.usd", + position=np.array([0, 0, 1.05]), ) - + stage_utils.add_reference_to_stage( + usd_path=assets_root_path + "/Isaac/Robots/Unitree/H1/h1.usd", + prim_path=self.prim_path, + ) else: self.world = SimulationApp.getWorld() # Get the articulation - from omni.isaac.core.articulations import Articulation, ArticulationSubset # type: ignore - + from omni.isaac.core.articulations import Articulation# type: ignore import omni.isaac.core.utils.stage as stage_utils # type: ignore # Resetting the world needs to be called before querying anything related to an articulation specifically. # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly + + self.world.reset() + # Load robot + self.articulation = Articulation(prim_path=self.prim_path, name=self.name) + self.articulation.initialize() + print("DOF names:", self.articulation.dof_names) + + # necessary so self.q and self.dq are accessible self.world.initialize_physics() - - # Load robot - #self.articulation = Articulation(prim_path=self.prim_path, name="ur5") - #self.articulation.initialize() - #print("DOF names:", self.articulation.dof_names) + + + + + @@ -129,8 +131,17 @@ def send_target_angles(self, q): # works well - #self.articulation.set_joint_positions(q) - + self.articulation.set_joint_positions(q) + ''' + tar = np.zeros(17) + self.articulation.set_joint_positions(tar) + + self.articulation.set_joint_positions([0., 0., 0., 0., + 0., 0., 0., 0., + 0., 0., 0., 0., + 0., 0., 0., 0., + 0., 0., 0., 0.]) + ''' # works but just sets robot abruptly to the target angles #self.robot.set_joint_positions(q) @@ -149,17 +160,36 @@ def send_target_angles(self, q): self.world.step(render=True) # execute one physics step and one rendering step - - def get_feedback(self): + + def get_feedback(self, arm_only=True): """Return a dictionary of information needed by the controller. Returns the joint angles and joint velocities in [rad] and [rad/sec], respectively """ # Get the joint angles and velocities - #self.q = self.articulation.get_joint_positions() - #self.dq = self.articulation.get_joint_velocities() - #return {"q": self.q, "dq": self.dq} + q = self.articulation.get_joint_positions() + dq = self.articulation.get_joint_velocities() + + # get all 12 DOF of the robot + if arm_only == False: + self.q = q + self.dq = dq + else: + # only get the DOF of the (right) arm + self.q[0] = q[6] # right_shoulder_pitch_joint + self.q[1] = q[10] # right_shoulder_roll_joint + self.q[2] = q[14] # right_shoulder_yaw_joint + self.q[3] = q[18] # right_elbow_joint + + self.dq[0] = dq[6] # right_shoulder_pitch_joint + self.dq[1] = dq[10] # right_shoulder_roll_joint + self.dq[2] = dq[14] # right_shoulder_yaw_joint + self.dq[3] = dq[18] # right_elbow_joint + + return {"q": self.q, "dq": self.dq} + + def get_xyz(self, name): @@ -202,3 +232,14 @@ def set_xyz(self, name, xyz): """ _cube = self.world.scene.get_object(name) _cube.set_world_pose(xyz, np.array([0., 0., 0., 1.])) # set the position and orientation of the object + + + + # method for keep_standing + def send_actions(self, dt): + pelvis_prim_path = '/World/robot/pelvis' + from pxr import Gf # type: ignore + prim=self.stage.GetPrimAtPath(pelvis_prim_path) + prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0 ,0.0 ,0.0 ,0.0)) + #prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(0.70711 ,0.70711 ,0.0 ,0.0)) + \ No newline at end of file diff --git a/examples/isaacsim/h1_lock_orientation.py b/examples/isaacsim/h1_lock_orientation.py new file mode 100644 index 00000000..5a9eba87 --- /dev/null +++ b/examples/isaacsim/h1_lock_orientation.py @@ -0,0 +1,40 @@ +""" +Running the joint controller with an inverse kinematics path planner +for a Mujoco simulation. The path planning system will generate +a trajectory in joint space that moves the end effector in a straight line +to the target, which changes every n time steps. +""" +import numpy as np +#from abr_control.arms.mujoco_config import MujocoConfig as arm +from abr_control.arms import jaco2 as arm +from abr_control.interfaces.nv_isaacsim import IsaacSim + + +robot_config = arm.Config() + +# create our path planner +n_timesteps = 2000 +# create our interface +dt = 0.001 +interface = IsaacSim(robot_config, dt=dt) +interface.connect() + + +try: + print("\nSimulation starting...") + count = 0 + while 1: + + feedback = interface.get_feedback(arm_only=False) + q=feedback["q"] + #print("Q: ", q) + + interface.send_target_angles(q) + count += 1 + + +finally: + # stop and reset the simulation + interface.disconnect() + + print("Simulation terminated...") \ No newline at end of file From 8149846a70ab7f9a65c843044d12b5f22b28d857 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 20 May 2025 09:49:37 -0400 Subject: [PATCH 11/42] fixed getfeedback to work only for arm DOFs --- abr_control/interfaces/nv_isaacsim.py | 85 ++++++++++++------------ examples/isaacsim/h1_lock_orientation.py | 8 ++- 2 files changed, 49 insertions(+), 44 deletions(-) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 51f64ef6..922536aa 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -124,40 +124,38 @@ def send_target_angles(self, q): """Moves the arm to the specified joint angles q : numpy.array - the target joint angles [radians] + the target joint angles [radians] """ - # print("robot joint pos: ", self.robot.get_joint_positions()) - #print("q: ", q) + #TODO make more general, with flag + #TODO use same flag for jaco2 and H1 - - # works well - self.articulation.set_joint_positions(q) - ''' - tar = np.zeros(17) - self.articulation.set_joint_positions(tar) - - self.articulation.set_joint_positions([0., 0., 0., 0., - 0., 0., 0., 0., - 0., 0., 0., 0., - 0., 0., 0., 0., - 0., 0., 0., 0.]) - ''' - - # works but just sets robot abruptly to the target angles - #self.robot.set_joint_positions(q) - - - # WORKS AND MOVES ROBOT SLOWLY, issues with time import - from omni.isaac.core.utils.types import ArticulationAction # type: ignore - #self.articulation.apply_action(ArticulationAction(q)) - #self.articulation.apply_action(ArticulationAction(np.array([1.0, 0.0,2.0,3.0,0.0,0.0]))) - - - - - #dof_ptr = self.dci.find_articulation_dof(art, "panda_joint2") + print("LENGTH q:", len(q)) + # full jaco2 arm with hand DOF + if np.size(q) == 12: + self.articulation.set_joint_positions(q) + # full H1 body with all DOF + if np.size(q) == 19: + self.articulation.set_joint_positions(q) + # only jaco2 arm + if np.size(q) == 6: + fb = self.get_feedback(with_hand=True) + fb_q = fb["q"] + fb_q[:6] = q + self.articulation.set_joint_positions(fb_q) + # only H1 right arm + if np.size(q) == 4: + fb = self.get_feedback(arm_only=False) + fb_q = fb["q"] + fb_q [6] = q[0] # right_shoulder_pitch_joint + fb_q [10] = q[1] # right_shoulder_roll_joint + fb_q [14] = q[2] # right_shoulder_yaw_joint + fb_q [18] = q[3] # right_elbow_joint + self.articulation.set_joint_positions(fb_q) + else: + print("The method send_target_angles of the isaacsim interface does not support the number of joint angles that are attempted to be set.") # move simulation ahead one time step self.world.step(render=True) # execute one physics step and one rendering step + @@ -171,27 +169,28 @@ def get_feedback(self, arm_only=True): q = self.articulation.get_joint_positions() dq = self.articulation.get_joint_velocities() - # get all 12 DOF of the robot + # get all DOF of the robot if arm_only == False: self.q = q self.dq = dq + # get only DOF of the arm else: - # only get the DOF of the (right) arm - self.q[0] = q[6] # right_shoulder_pitch_joint - self.q[1] = q[10] # right_shoulder_roll_joint - self.q[2] = q[14] # right_shoulder_yaw_joint - self.q[3] = q[18] # right_elbow_joint - - self.dq[0] = dq[6] # right_shoulder_pitch_joint - self.dq[1] = dq[10] # right_shoulder_roll_joint - self.dq[2] = dq[14] # right_shoulder_yaw_joint - self.dq[3] = dq[18] # right_elbow_joint - + tmp = np.zeros(4) + tmp[0] = q[6] # right_shoulder_pitch_joint + tmp[1] = q[10] # right_shoulder_roll_joint + tmp[2] = q[14] # right_shoulder_yaw_joint + tmp[3] = q[18] # right_elbow_joint + self.q = tmp + + tmp[0] = dq[6] # right_shoulder_pitch_joint + tmp[1] = dq[10] # right_shoulder_roll_joint + tmp[2] = dq[14] # right_shoulder_yaw_joint + tmp[3] = dq[18] # right_elbow_joint + self.dq = tmp return {"q": self.q, "dq": self.dq} - def get_xyz(self, name): """Returns the xyz position of the specified object diff --git a/examples/isaacsim/h1_lock_orientation.py b/examples/isaacsim/h1_lock_orientation.py index 5a9eba87..bdb76300 100644 --- a/examples/isaacsim/h1_lock_orientation.py +++ b/examples/isaacsim/h1_lock_orientation.py @@ -19,17 +19,23 @@ interface = IsaacSim(robot_config, dt=dt) interface.connect() +feedback = interface.get_feedback() +q=feedback["q"] + +interface.send_target_angles(q) try: print("\nSimulation starting...") count = 0 while 1: - feedback = interface.get_feedback(arm_only=False) + feedback = interface.get_feedback() q=feedback["q"] #print("Q: ", q) interface.send_target_angles(q) + #interface.world.step(render=True) # execute one physics step and one rendering step + count += 1 From 4e70b1a5d147983ccf2630822d2c547f7f9aaafd Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Wed, 21 May 2025 12:38:13 -0400 Subject: [PATCH 12/42] added config file for H1 --- abr_control/arms/unitree_H1/__init__.py | 1 + abr_control/arms/unitree_H1/config.py | 273 ++++++++++++++++++++++++ 2 files changed, 274 insertions(+) create mode 100644 abr_control/arms/unitree_H1/__init__.py create mode 100644 abr_control/arms/unitree_H1/config.py diff --git a/abr_control/arms/unitree_H1/__init__.py b/abr_control/arms/unitree_H1/__init__.py new file mode 100644 index 00000000..cca5d9bd --- /dev/null +++ b/abr_control/arms/unitree_H1/__init__.py @@ -0,0 +1 @@ +from .config import Config diff --git a/abr_control/arms/unitree_H1/config.py b/abr_control/arms/unitree_H1/config.py new file mode 100644 index 00000000..0693f55b --- /dev/null +++ b/abr_control/arms/unitree_H1/config.py @@ -0,0 +1,273 @@ +import numpy as np +import sympy as sp + +from ..base_config import BaseConfig + + +class Config(BaseConfig): + """Robot config file for the right H1 arm for IsaacSim + + Attributes + ---------- + START_ANGLES : numpy.array + The joint angles for a safe home or rest position + _M_LINKS : sympy.diag + inertia matrix of the links + _M_JOINTS : sympy.diag + inertia matrix of the joints + L : numpy.array + segment lengths of arm [meters] + KZ : sympy.Matrix + z isolation vector in orientational part of Jacobian + + Transform Naming Convention: Tpoint1point2 + ex: Tj1l1 transforms from joint 1 to link 1 + + Transforms are broken up into two matrices for simplification + ex: Tj0l1a and Tj0l1b where the former transform accounts for + joint rotations and the latter accounts for static rotations + and translations + """ + + def __init__(self, **kwargs): + + super().__init__(N_JOINTS=4, N_LINKS=5, ROBOT_NAME="unitree_H1", **kwargs) + + self._T = {} # dictionary for storing calculated transforms + + self.JOINT_NAMES = [f"H1_right_arm_joint{ii}" for ii in range(self.N_JOINTS)] + + # for the null space controller, keep arm near these angles + self.START_ANGLES = np.array( + [np.pi / 4.0, np.pi / 4.0, np.pi / 4.0, np.pi / 4.0], dtype="float32" + ) + + # TODO adapt this + # create the inertia matrices for each link of the three joint + + + + + # TODO: identify the actual values for these links + self._M_LINKS.append(sp.zeros(6, 6)) # link0 + self._M_LINKS.append(sp.diag(1.98, 1.98, 1.98, 0.0, 0.0, 10.0)) # link1 + self._M_LINKS.append(sp.diag(1.32, 1.32, 1.32, 0.0, 0.0, 10.0)) # link2 + self._M_LINKS.append(sp.diag(0.8, 0.8, 0.8, 0.0, 0.0, 10.0)) # link3 + #TODO LS vals in next line + self._M_LINKS.append(sp.diag(0.8, 0.8, 0.8, 0.0, 0.0, 10.0)) # link4 + + # the joints don't weigh anything + self._M_JOINTS = [sp.zeros(6, 6) for ii in range(self.N_JOINTS)] + + # segment lengths associated with each joint + # [x, y, z], Ignoring lengths < 1e-04 + + self.L = np.array( + [ + [0, 0, 0], # from origin to l0 (non-existent) + [0, 0, 0], # from l0 to j0 + [1.0, 0, 0], # from j0 to l1 COM + [1.0, 0, 0], # from l1 COM to j1 + [0.6, 0, 0], # from j1 to l2 COM + [0.6, 0, 0], # from l2 COM to j2 + [0.35, 0, 0], # from j2 to l3 COM + [0.35, 0, 0], # from l3 COM to j3 + #TODO LS vals in next lines + [0.35, 0, 0], # from j3 to l4 COM + [0.35, 0, 0], + ], # from l4 COM to EE + dtype="float32", + ) + + # Transform matrix : origin -> link 0 + # no change of axes, account for offsets + self.Torgl0 = sp.Matrix( + [ + [1, 0, 0, self.L[0, 0]], + [0, 1, 0, self.L[0, 1]], + [0, 0, 1, self.L[0, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : link 0 -> joint 0 + # no change of axes, account for offsets + self.Tl0j0 = sp.Matrix( + [ + [1, 0, 0, self.L[1, 0]], + [0, 1, 0, self.L[1, 1]], + [0, 0, 1, self.L[1, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : joint 0 -> link 1 + # account for rotation of q + self.Tj0l1a = sp.Matrix( + [ + [sp.cos(self.q[0]), -sp.sin(self.q[0]), 0, 0], + [sp.sin(self.q[0]), sp.cos(self.q[0]), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + # no change of axes, account for offsets + self.Tj0l1b = sp.Matrix( + [ + [1, 0, 0, self.L[2, 0]], + [0, 1, 0, self.L[2, 1]], + [0, 0, 1, self.L[2, 2]], + [0, 0, 0, 1], + ] + ) + self.Tj0l1 = self.Tj0l1a * self.Tj0l1b + + # Transform matrix : link 1 -> joint 1 + # no change of axes, account for offsets + self.Tl1j1 = sp.Matrix( + [ + [1, 0, 0, self.L[3, 0]], + [0, 1, 0, self.L[3, 1]], + [0, 0, 1, self.L[3, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : joint 1 -> link 2 + # account for rotation of q + self.Tj1l2a = sp.Matrix( + [ + [sp.cos(self.q[1]), -sp.sin(self.q[1]), 0, 0], + [sp.sin(self.q[1]), sp.cos(self.q[1]), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + # no change of axes, account for offsets + self.Tj1l2b = sp.Matrix( + [ + [1, 0, 0, self.L[4, 0]], + [0, 1, 0, self.L[4, 1]], + [0, 0, 1, self.L[4, 2]], + [0, 0, 0, 1], + ] + ) + self.Tj1l2 = self.Tj1l2a * self.Tj1l2b + + # Transform matrix : link 2 -> joint 2 + # no change of axes, account for offsets + self.Tl2j2 = sp.Matrix( + [ + [1, 0, 0, self.L[5, 0]], + [0, 1, 0, self.L[5, 1]], + [0, 0, 1, self.L[5, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : joint 2 -> link 3 + # account for rotation of q + self.Tj2l3a = sp.Matrix( + [ + [sp.cos(self.q[2]), -sp.sin(self.q[2]), 0, 0], + [sp.sin(self.q[2]), sp.cos(self.q[2]), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + # no change of axes, account for offsets + self.Tj2l3b = sp.Matrix( + [ + [1, 0, 0, self.L[6, 0]], + [0, 1, 0, self.L[6, 1]], + [0, 0, 1, self.L[6, 2]], + [0, 0, 0, 1], + ] + ) + self.Tj2l3 = self.Tj2l3a * self.Tj2l3b + + # Transform matrix : link 3 -> end-effector + # no change of axes, account for offsets + self.Tl3j3 = sp.Matrix( + [ + [1, 0, 0, self.L[7, 0]], + [0, 1, 0, self.L[7, 1]], + [0, 0, 1, self.L[7, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : joint 3 -> link 4 + # account for rotation of q + self.Tj3l4a = sp.Matrix( + [ + [sp.cos(self.q[3]), -sp.sin(self.q[3]), 0, 0], + [sp.sin(self.q[3]), sp.cos(self.q[3]), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + # no change of axes, account for offsets + self.Tj3l4b = sp.Matrix( + [ + [1, 0, 0, self.L[8, 0]], + [0, 1, 0, self.L[8, 1]], + [0, 0, 1, self.L[8, 2]], + [0, 0, 0, 1], + ] + ) + self.Tj3l4 = self.Tj3l4a * self.Tj3l4b + + # Transform matrix : link 4 -> end-effector + # no change of axes, account for offsets + self.Tl4ee = sp.Matrix( + [ + [1, 0, 0, self.L[9, 0]], + [0, 1, 0, self.L[9, 1]], + [0, 0, 1, self.L[9, 2]], + [0, 0, 0, 1], + ] + ) + + + # orientation part of the Jacobian (compensating for angular velocity) + self.J_orientation = [ + self._calc_T("joint0")[:3, :3] * self._KZ, # joint 0 orientation + self._calc_T("joint1")[:3, :3] * self._KZ, # joint 1 orientation + self._calc_T("joint2")[:3, :3] * self._KZ, # joint 2 orientation + self._calc_T("joint3")[:3, :3] * self._KZ, + ] # joint 3 orientation + + def _calc_T(self, name): + """Uses Sympy to generate the transform for a joint or link + + name : string + name of the joint, link, or end-effector + """ + + if self._T.get(name, None) is None: + if name == "link0": + self._T[name] = self.Torgl0 + elif name == "joint0": + self._T[name] = self._calc_T("link0") * self.Tl0j0 + elif name == "link1": + self._T[name] = self._calc_T("joint0") * self.Tj0l1 + elif name == "joint1": + self._T[name] = self._calc_T("link1") * self.Tl1j1 + elif name == "link2": + self._T[name] = self._calc_T("joint1") * self.Tj1l2 + elif name == "joint2": + self._T[name] = self._calc_T("link2") * self.Tl2j2 + elif name == "link3": + self._T[name] = self._calc_T("joint2") * self.Tj2l3 + elif name == "joint3": + self._T[name] = self._calc_T("link3") * self.Tl3j3 + elif name == "link4": + self._T[name] = self._calc_T("joint3") * self.Tj3l4 + elif name == "EE": + self._T[name] = self._calc_T("link4") * self.Tl4ee + + else: + raise Exception(f"Invalid transformation name: {name}") + + return self._T[name] From 25ab5ca306be28c075d2138cef3bca4cf68f5499 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Fri, 23 May 2025 11:11:28 -0400 Subject: [PATCH 13/42] added matrix for segment lengths of arm --- abr_control/arms/unitree_H1/config.py | 66 ++++++++++++++++----------- 1 file changed, 40 insertions(+), 26 deletions(-) diff --git a/abr_control/arms/unitree_H1/config.py b/abr_control/arms/unitree_H1/config.py index 0693f55b..27c924c6 100644 --- a/abr_control/arms/unitree_H1/config.py +++ b/abr_control/arms/unitree_H1/config.py @@ -38,23 +38,21 @@ def __init__(self, **kwargs): self.JOINT_NAMES = [f"H1_right_arm_joint{ii}" for ii in range(self.N_JOINTS)] # for the null space controller, keep arm near these angles + #self.START_ANGLES = np.array( + # [np.pi / 4.0, np.pi / 4.0, np.pi / 4.0, np.pi / 4.0], dtype="float32" + #) self.START_ANGLES = np.array( - [np.pi / 4.0, np.pi / 4.0, np.pi / 4.0, np.pi / 4.0], dtype="float32" + [0.0, 0.0, 0.0, 0.0], dtype="float32" ) # TODO adapt this # create the inertia matrices for each link of the three joint - - - - # TODO: identify the actual values for these links self._M_LINKS.append(sp.zeros(6, 6)) # link0 - self._M_LINKS.append(sp.diag(1.98, 1.98, 1.98, 0.0, 0.0, 10.0)) # link1 - self._M_LINKS.append(sp.diag(1.32, 1.32, 1.32, 0.0, 0.0, 10.0)) # link2 - self._M_LINKS.append(sp.diag(0.8, 0.8, 0.8, 0.0, 0.0, 10.0)) # link3 - #TODO LS vals in next line - self._M_LINKS.append(sp.diag(0.8, 0.8, 0.8, 0.0, 0.0, 10.0)) # link4 + self._M_LINKS.append(sp.diag(0.001, 0.0, 0.0, 0.001, 0.0, 0.001)) # link1 - right_shoulder_pitch_link + self._M_LINKS.append(sp.diag(0.002, 0.0, 0.0, 0.002, 0.0, 0.001)) # link2 - right_shoulder_roll_link + self._M_LINKS.append(sp.diag(0.004, 0.0, 0.0, 0.004, 0.0, 0.0)) # link3 - right_shoulder_yaw_link + self._M_LINKS.append(sp.diag(0.0, 0.0, 0.0, 0.006, 0.0, 0.006)) # link4 - right_elbow_link # the joints don't weigh anything self._M_JOINTS = [sp.zeros(6, 6) for ii in range(self.N_JOINTS)] @@ -62,22 +60,38 @@ def __init__(self, **kwargs): # segment lengths associated with each joint # [x, y, z], Ignoring lengths < 1e-04 - self.L = np.array( - [ - [0, 0, 0], # from origin to l0 (non-existent) - [0, 0, 0], # from l0 to j0 - [1.0, 0, 0], # from j0 to l1 COM - [1.0, 0, 0], # from l1 COM to j1 - [0.6, 0, 0], # from j1 to l2 COM - [0.6, 0, 0], # from l2 COM to j2 - [0.35, 0, 0], # from j2 to l3 COM - [0.35, 0, 0], # from l3 COM to j3 - #TODO LS vals in next lines - [0.35, 0, 0], # from j3 to l4 COM - [0.35, 0, 0], - ], # from l4 COM to EE - dtype="float32", - ) + + self.L = [ + [0.0, 0.0, 0.0], # base offset (optional) + [0.0055, -0.15535, 0.42999], # right_shoulder_pitch_joint + [0.005045, -0.053657, -0.015715], # right_shoulder_pitch_link + [-0.0055, -0.0565, -0.0165], # right_shoulder_roll_joint + [0.000679, -0.00115, -0.094076], # right_shoulder_roll_link + [0.0, 0.0, -0.1343], # right_shoulder_yaw_joint + [0.01365, -0.002767, -0.16266], # right_shoulder_yaw_link + [0.0185, 0.0, -0.198], # right_elbow_joint + [0.164862, -0.000118, -0.015734], # right_elbow_link + [0.0, 0.0, 0.09], # offset to end of hand/fingers (assumed) + ] + self.L = np.array(self.L) + + + + ''' + self.L = [ + [0.0, 0.0, 0.0], # base offset (optional, can be torso_link) + [0.0055, -0.15535, 0.42999], # right_shoulder_pitch_joint (from torso_link) + [0.005045, -0.053657, -0.015715], # right_shoulder_pitch_link (inertial origin) + [-0.0055, -0.0565, -0.0165], # right_shoulder_roll_joint (from shoulder_pitch_link) + [0.000679, -0.00115, -0.094076], # right_shoulder_roll_link (inertial origin) + [0.0, 0.0, -0.1343], # right_shoulder_yaw_joint (from shoulder_roll_link) + [0.01365, -0.002767, -0.16266], # right_shoulder_yaw_link (inertial origin) + [0.0185, 0.0, -0.198], # right_elbow_joint (from shoulder_yaw_link) + [0.164862, -0.000118, -0.015734], # right_elbow_link (inertial origin) + ] + ''' + + # Transform matrix : origin -> link 0 # no change of axes, account for offsets From 59d39253c14e29567c98490b0e500fc867c5808f Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Fri, 23 May 2025 11:12:23 -0400 Subject: [PATCH 14/42] added articulation view --- abr_control/interfaces/nv_isaacsim.py | 194 ++++++++++++------ ...ion_joint_control_inverse_kinematics_H1.py | 117 +++++++++++ 2 files changed, 253 insertions(+), 58 deletions(-) create mode 100644 examples/isaacsim/position_joint_control_inverse_kinematics_H1.py diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 922536aa..e87fcfda 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -21,71 +21,138 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called - self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles - self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities + #self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles + #self.initial_q = np.zeros(self.robot_config.N_JOINTS) # joint angles + #self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities self.prim_path = "/World/robot" - self.name = "robot" + self.name = self.robot_config.xml_dir.rsplit('/', 1)[-1] + #self.name = "robot" + #robot_name = self.robot_config.xml_dir.rsplit('/', 1)[-1] #self.misc_handles = {} # for tracking miscellaneous object handles - def connect(self, load_scene=True): - if load_scene: - """All initial setup.""" - self.simulation_app = SimulationApp({"headless": False}) - from isaacsim.core.api import World # type: ignore - import omni.isaac.core.utils.stage as stage_utils # type: ignore - import omni.kit.commands # type: ignore - import omni - from isaacsim.robot.policy.examples.robots.h1 import H1FlatTerrainPolicy - from isaacsim.storage.native import get_assets_root_path - - - # Create a world - self.world = World(physics_dt=self.dt,rendering_dt=self.dt) - self.stage = omni.usd.get_context().get_stage() - self.world.add_physics_callback("send_actions", self.send_actions) - self.world.scene.add_default_ground_plane() + def connect(self, joint_names=None): + """ + joint_names: list, optional (Default: None) + list of joint names to send control signal to and get feedback from + if None, the joints in the kinematic tree connecting the end-effector + to the world are used + """ + + + """All initial setup.""" + self.simulation_app = SimulationApp({"headless": False}) + from isaacsim.core.api import World # type: ignore + import omni.isaac.core.utils.stage as stage_utils # type: ignore + import omni.kit.commands # type: ignore + import omni + from isaacsim.robot.policy.examples.robots.h1 import H1FlatTerrainPolicy + from isaacsim.storage.native import get_assets_root_path + from omni.isaac.core.articulations import Articulation, ArticulationView# type: ignore + import omni.isaac.core.utils.stage as stage_utils # type: ignore + from isaacsim.core.api.robots import Robot + from isaacsim.core.api.controllers.articulation_controller import ArticulationController + # Create a world + self.world = World(physics_dt=self.dt,rendering_dt=self.dt) + self.context = omni.usd.get_context() + self.stage = self.context.get_stage() + self.world.add_physics_callback("send_actions", self.send_actions) + self.world.scene.add_default_ground_plane() + + + ''' + assets_root_path = get_assets_root_path() - assets_root_path = get_assets_root_path() - + # Load Jaco2 robot + stage_utils.add_reference_to_stage( + usd_path=assets_root_path + "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd", + # Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd --> 7 DOF arm , not compatible with ABR controller + prim_path=self.prim_path, + ) + + robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) + ''' + + + ''' + # Load H1 robot + - self.h1 = H1FlatTerrainPolicy( - prim_path=self.prim_path, - name=self.name, - usd_path=assets_root_path + "/Isaac/Robots/Unitree/H1/h1.usd", - position=np.array([0, 0, 1.05]), - ) - stage_utils.add_reference_to_stage( + self.h1 = H1FlatTerrainPolicy( + prim_path=self.prim_path, + name=self.name, + usd_path=assets_root_path + "/Isaac/Robots/Unitree/H1/h1.usd", + position=np.array([0, 0, 1.05]), + ) + stage_utils.add_reference_to_stage( usd_path=assets_root_path + "/Isaac/Robots/Unitree/H1/h1.usd", prim_path=self.prim_path, ) - - else: - self.world = SimulationApp.getWorld() - - # Get the articulation - from omni.isaac.core.articulations import Articulation# type: ignore - import omni.isaac.core.utils.stage as stage_utils # type: ignore - + ''' + # Resetting the world needs to be called before querying anything related to an articulation specifically. # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly - - self.world.reset() # Load robot - self.articulation = Articulation(prim_path=self.prim_path, name=self.name) + self.articulation = Articulation(prim_path=self.prim_path, name=self.name + "_articulation") self.articulation.initialize() print("DOF names:", self.articulation.dof_names) - + self.articulation_view = ArticulationView(prim_paths_expr=self.prim_path, name=self.name + "_view") + self.world.scene.add(self.articulation_view) + + articulation_controller = ArticulationController() + articulation_controller.initialize(self.articulation_view) + + # necessary so self.q and self.dq are accessible self.world.initialize_physics() + # get the robot's initial joint angle + #TODO maybe replace with articulation.get_default_state() + self.initial_q = self.get_feedback(arm_only=False)["q"] + + + self.joint_pos_addrs = [] + self.joint_vel_addrs = [] + self.joint_dyn_addrs = [] + + + + if joint_names is None: + # if no joint names provided, get addresses of joints in the kinematic + # tree from end-effector (EE) to world body + #bodyid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "EE") + + #bodyid = self.articulation.get_dof_index('right_elbow_joint') + #l_id = self.articulation.get_link_index("right_elbow_joint") # get the link index of the right elbow joint + #bodyid = self.articulation.get_link_index("EE") + dof_names = self.articulation.dof_names + print("DOF names:", dof_names) + #bodid = self.articulation.dof_names.index("EE") + #bodyid = self.articulation_controller + #bodyid = self.articulation.get_dof_index("EE") + + # and working back to the world body + #while self.model.body_parentid[bodyid] != 0: + while bodyid > 1: + print (bodyid) + #first_joint = self.articulation.get_joint_index("EE") + #num_joints = self.model.body_jntnum[bodyid] + + + + self.robot_config._connect( + #self.articulation, + self.articulation_view, + self.joint_pos_addrs, + self.joint_vel_addrs, + ) @@ -129,7 +196,6 @@ def send_target_angles(self, q): #TODO make more general, with flag #TODO use same flag for jaco2 and H1 - print("LENGTH q:", len(q)) # full jaco2 arm with hand DOF if np.size(q) == 12: self.articulation.set_joint_positions(q) @@ -138,19 +204,24 @@ def send_target_angles(self, q): self.articulation.set_joint_positions(q) # only jaco2 arm if np.size(q) == 6: - fb = self.get_feedback(with_hand=True) + fb = self.get_feedback(arm_only=False) fb_q = fb["q"] fb_q[:6] = q self.articulation.set_joint_positions(fb_q) - # only H1 right arm + # only H1 right arm if np.size(q) == 4: - fb = self.get_feedback(arm_only=False) - fb_q = fb["q"] + fb_q = self.initial_q + + #fb = self.get_feedback(arm_only=False) + #fb_q = fb["q"] + fb_q [6] = q[0] # right_shoulder_pitch_joint fb_q [10] = q[1] # right_shoulder_roll_joint fb_q [14] = q[2] # right_shoulder_yaw_joint fb_q [18] = q[3] # right_elbow_joint self.articulation.set_joint_positions(fb_q) + + else: print("The method send_target_angles of the isaacsim interface does not support the number of joint angles that are attempted to be set.") # move simulation ahead one time step @@ -175,18 +246,24 @@ def get_feedback(self, arm_only=True): self.dq = dq # get only DOF of the arm else: - tmp = np.zeros(4) - tmp[0] = q[6] # right_shoulder_pitch_joint - tmp[1] = q[10] # right_shoulder_roll_joint - tmp[2] = q[14] # right_shoulder_yaw_joint - tmp[3] = q[18] # right_elbow_joint - self.q = tmp - - tmp[0] = dq[6] # right_shoulder_pitch_joint - tmp[1] = dq[10] # right_shoulder_roll_joint - tmp[2] = dq[14] # right_shoulder_yaw_joint - tmp[3] = dq[18] # right_elbow_joint - self.dq = tmp + if self.name == "jaco2": + # get only the right arm DOF of the jaco 2 without hand + self.q = q[:6] + self.dq = dq[:6] + elif self.name == "h1": + # get only the right arm DOF of the H1 robot + tmp = np.zeros(4) + tmp[0] = q[6] # right_shoulder_pitch_joint + tmp[1] = q[10] # right_shoulder_roll_joint + tmp[2] = q[14] # right_shoulder_yaw_joint + tmp[3] = q[18] # right_elbow_joint + self.q = tmp + + tmp[0] = dq[6] # right_shoulder_pitch_joint + tmp[1] = dq[10] # right_shoulder_roll_joint + tmp[2] = dq[14] # right_shoulder_yaw_joint + tmp[3] = dq[18] # right_elbow_joint + self.dq = tmp return {"q": self.q, "dq": self.dq} @@ -240,5 +317,6 @@ def send_actions(self, dt): from pxr import Gf # type: ignore prim=self.stage.GetPrimAtPath(pelvis_prim_path) 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 ,0.02)) #prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(0.70711 ,0.70711 ,0.0 ,0.0)) \ No newline at end of file diff --git a/examples/isaacsim/position_joint_control_inverse_kinematics_H1.py b/examples/isaacsim/position_joint_control_inverse_kinematics_H1.py new file mode 100644 index 00000000..eb1133e3 --- /dev/null +++ b/examples/isaacsim/position_joint_control_inverse_kinematics_H1.py @@ -0,0 +1,117 @@ +""" +Running the joint controller with an inverse kinematics path planner +for a Mujoco simulation. The path planning system will generate +a trajectory in joint space that moves the end effector in a straight line +to the target, which changes every n time steps. +""" +import numpy as np +#from abr_control.arms.mujoco_config import MujocoConfig as arm +from abr_control.arms import unitree_H1 as arm +#import glfw +from abr_control.controllers import path_planners +#from abr_control.interfaces.mujoco import Mujoco +from abr_control.interfaces.nv_isaacsim import IsaacSim +from abr_control.utils import transformations +from flying_cube import FlyingCube + +''' +from abr_control.arms import ur5 as arm1 +ur5 = arm1.Config() +print("ur5: ", ur5.N_JOINTS) +print("L: ", len(ur5.L)) +from abr_control.arms import jaco2 as arm2 +jaco = arm2.Config() +print("jaco: ", jaco.N_JOINTS) +print("L: ", len(jaco.L)) + +from abr_control.arms import onejoint as arm3 +onejoint = arm3.Config() +print("onejoint: ", onejoint.N_JOINTS) +print("L: ", len(onejoint.L)) + +from abr_control.arms import twojoint as arm4 +twojoint = arm4.Config() +print("twojoint: ", twojoint.N_JOINTS) + +print("L: ", len(twojoint.L)) + +from abr_control.arms import threejoint as arm5 +threejoint = arm5.Config() +print("threejoint: ", threejoint.N_JOINTS) +print("L: ", len(threejoint.L)) +''' + +robot_config = arm.Config() +#print("h1: ", robot_config.N_JOINTS) +#print("L: ", len(robot_config.L)) + + +# create our path planner +n_timesteps = 2000 +path_planner = path_planners.InverseKinematics(robot_config) + +# create our interface +dt = 0.001 +interface = IsaacSim(robot_config, dt=dt) +interface.connect() +interface.send_target_angles(robot_config.START_ANGLES) +feedback = interface.get_feedback() +isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") + + + + +#inertias = interface.articulation_view.get_body_inertias() + +#inertias_right_shoulder_pitch_joint = interface.articulation_view.get_body_inertias(body_indices=np.array([6])) + + +try: + print("\nSimulation starting...") + print("Click to move the target.\n") + + count = 0 + while 1: + + if count % n_timesteps == 0: + print("Moving target...") + feedback = interface.get_feedback() + target_xyz = np.array( + [ + np.random.random() * 0.5 + 0.2, + np.random.random() * 1.1 - 0.9, + np.random.random() * 1.0 + 0.9, + ] + ) + R = robot_config.R("EE", q=feedback["q"]) + #R = robot_config.R("wrist_3_joint", q=feedback["q"]) + + target_orientation = transformations.euler_from_matrix(R, "sxyz") + # update the position of the target + interface.set_xyz("target", target_xyz) + + # can use 3 different methods to calculate inverse kinematics + # see inverse_kinematics.py file for details + path_planner.generate_path( + position=feedback["q"], + target_position=np.hstack([target_xyz, target_orientation]), + method=3, + dt=0.005, + n_timesteps=n_timesteps, + plot=False, + ) + + # returns desired [position, velocity] + target = path_planner.next()[0] + + # use position control + interface.send_target_angles(target[: robot_config.N_JOINTS]) + #interface.world.step(render=True) # execute one physics step and one rendering step + + count += 1 + +finally: + # stop and reset the simulation + interface.disconnect() + + print("Simulation terminated...") \ No newline at end of file From fb9445b56c652bd33d1aab42cf8da31ef31e88b4 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 17 Jun 2025 13:37:18 -0400 Subject: [PATCH 15/42] working example with floating robot --- abr_control/arms/isaacsim_config.py | 524 ++++++++++++++++++ abr_control/interfaces/nv_isaacsim.py | 163 ++---- examples/IsaacSim/force_floating_control.py | 85 +++ examples/isaacsim/blog_minimal.py | 41 -- examples/isaacsim/cube.py | 48 -- examples/isaacsim/flying_cube.py | 55 -- ...rce_osc_xyz_avoid_obstacle path_planner.py | 173 ------ .../isaacsim/force_osc_xyz_avoid_obstacle.py | 146 ----- examples/isaacsim/h1_lock_orientation.py | 46 -- examples/isaacsim/jaco_2.py | 37 -- ...sition_joint_control_inverse_kinematics.py | 83 --- ...ion_joint_control_inverse_kinematics_H1.py | 117 ---- examples/isaacsim/ur10_moving_cube.py | 27 - examples/isaacsim/ur5.py | 30 - 14 files changed, 641 insertions(+), 934 deletions(-) create mode 100644 abr_control/arms/isaacsim_config.py create mode 100644 examples/IsaacSim/force_floating_control.py delete mode 100644 examples/isaacsim/blog_minimal.py delete mode 100644 examples/isaacsim/cube.py delete mode 100644 examples/isaacsim/flying_cube.py delete mode 100644 examples/isaacsim/force_osc_xyz_avoid_obstacle path_planner.py delete mode 100644 examples/isaacsim/force_osc_xyz_avoid_obstacle.py delete mode 100644 examples/isaacsim/h1_lock_orientation.py delete mode 100644 examples/isaacsim/jaco_2.py delete mode 100644 examples/isaacsim/position_joint_control_inverse_kinematics.py delete mode 100644 examples/isaacsim/position_joint_control_inverse_kinematics_H1.py delete mode 100644 examples/isaacsim/ur10_moving_cube.py delete mode 100644 examples/isaacsim/ur5.py diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py new file mode 100644 index 00000000..d2959c4e --- /dev/null +++ b/abr_control/arms/isaacsim_config.py @@ -0,0 +1,524 @@ +import os +from xml.etree import ElementTree +import numpy as np +from abr_control.utils import download_meshes +import omni + +class IsaacsimConfig: + """A wrapper on the Mujoco simulator to generate all the kinematics and + dynamics calculations necessary for controllers. + """ + + # https://nvidia-omniverse.github.io/PhysX/physx/5.1.0/docs/Joints.html + JNT_POS_LENGTH_ISAACSIM = { + "free": 7, # 3 (translation) + 4 (quaternion rotation), usually not used in articulated chains + "spherical": 4, # Represented as quaternion in PxArticulationReducedCoordinate + "prismatic": 1, # Linear motion in one axis + "revolute": 1, # Rotational motion in one axis + } + ''' + from omni.isaac.core.articulations import ArticulationJointType + JNT_POS_LENGTH = { + ArticulationJointType.FREE: 7, # 3 for position + 4 for quaternion orientation + ArticulationJointType.BALL: 4, # quaternion orientation + ArticulationJointType.PRISMATIC: 1, # 1 DoF linear + ArticulationJointType.REVOLUTE: 1, # 1 DoF rotational + } + ''' + + JNT_DYN_LENGTH_ISAACSIM = { + "free": 6, # 3 linear + 3 angular velocity (used for root link only, not in articulated chains) + "spherical": 3, # Angular velocity vector (3D) + "prismatic": 1, # Linear velocity along one axis + "revolute": 1, # Angular velocity around one axis + } + + + + + def __init__(self, xml_file, folder=None, use_sim_state=True, force_download=False): + """Loads the Isaacsim model from the specified xml file + + Parameters + ---------- + xml_file: string + the name of the arm model to load. If folder remains as None, + the string passed in is parsed such that everything up to the first + underscore is used for the arm directory, and the full string is + used to load the xml within that folder. + + EX: 'myArm' and 'myArm_with_gripper' will both look in the + 'myArm' directory, however they will load myArm.xml and + myArm_with_gripper.xml, respectively + + If a folder is passed in, then folder/xml_file is used + folder: string, Optional (Default: None) + specifies what folder to find the xml_file, if None specified will + checking in abr_control/arms/xml_file (see above for xml_file) + use_sim_state: Boolean, optional (Default: True) + If set False, the state information is provided by the user, which + is then used to calculate the corresponding dynamics values. + The state is then set back to the sim state prior to the user + provided state. + If set true, any q and dq values passed in to the functions are + ignored, and the current state of the simulator is used to + calculate all functions. This can speed up the simulation, because + the step of resetting the state on every call is omitted. + force_download: boolean, Optional (Default: False) + True to force downloading the mesh and texture files, useful when new files + are added that may be missing. + False: if the meshes folder is missing it will ask the user whether they + want to download them + """ + if folder is None: + arm_dir = xml_file.split("_")[0] + current_dir = os.path.dirname(__file__) + self.xml_file = os.path.join(current_dir, arm_dir, f"{xml_file}.xml") + self.xml_dir = f"{current_dir}/{arm_dir}" + else: + self.xml_dir = f"{folder}" + self.xml_file = os.path.join(self.xml_dir, xml_file) + + self.N_GRIPPER_JOINTS = 0 + + # get access to some of our custom arm parameters from the xml definition + tree = ElementTree.parse(self.xml_file) + root = tree.getroot() + for custom in root.findall("custom/numeric"): + name = custom.get("name") + if name == "START_ANGLES": + START_ANGLES = custom.get("data").split(" ") + self.START_ANGLES = np.array([float(angle) for angle in START_ANGLES]) + elif name == "N_GRIPPER_JOINTS": + self.N_GRIPPER_JOINTS = int(custom.get("data")) + + # check for google_id specifying download location of robot mesh files + self.google_id = None + for custom in root.findall("custom/text"): + name = custom.get("name") + if name == "google_id": + self.google_id = custom.get("data") + + actuators = root.find(f'actuator') + self.joint_names = [actuator.get("joint") for actuator in actuators] + + # check if the user has downloaded the required mesh files + # if not prompt them to do so + if self.google_id is not None: + # get list of expected files to check if all have been downloaded + files = [] + for asset in root.findall("asset/mesh"): + files.append(asset.get("file")) + + for asset in root.findall("asset/texture"): + # assuming that texture are placed in the meshes folder + files.append(asset.get("file").split("/")[1]) + + # check if our mesh folder exists, then check we have all the files + download_meshes.check_and_download( + name=self.xml_dir + "/meshes", + google_id=self.google_id, + force_download=force_download, + files=files, + ) + + self.use_sim_state = use_sim_state + + def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path): + + """Called by the interface once the Mujoco simulation is created, + this connects the config to the simulator so it can access the + kinematics and dynamics information calculated by Mujoco. + + Parameters + ---------- + sim: MjSim + The Mujoco Simulator object created by the Mujoco Interface class + joint_pos_addrs: np.array of ints + The index of the robot joints in the Mujoco simulation data joint + position array + joint_vel_addrs: np.array of ints + The index of the robot joints in the Mujoco simulation data joint + Jacobian, inertia matrix, and gravity vector + """ + + # get access to the Isaac simulation + self.world = world + self.stage = stage + self.articulation = articulation + self.articulation_view = articulation_view + self.joint_pos_addrs = np.copy(joint_pos_addrs) + self.joint_vel_addrs = np.copy(joint_vel_addrs) + self.prim_path = prim_path + + + # number of controllable joints in the robot arm + self.N_JOINTS = len(self.joint_vel_addrs) + print (f"Number of controllable joints: {self.N_JOINTS}") + # number of joints in the IsaacSim simulation + N_ALL_JOINTS = self.articulation_view.num_dof + print (f"Number of ALL joints in simulation: {N_ALL_JOINTS}") + + # need to calculate the joint_vel_addrs indices in flat vectors returned + # for the Jacobian + self.jac_indices = np.hstack( + # 6 because position and rotation Jacobians are 3 x N_JOINTS + [self.joint_vel_addrs + (ii * N_ALL_JOINTS) for ii in range(3)] + ) + + # for the inertia matrix + self.M_indices = [ + ii * N_ALL_JOINTS + jj + for jj in self.joint_vel_addrs + for ii in self.joint_vel_addrs + ] + + # a place to store data returned from Mujoco + self._g = np.zeros(self.N_JOINTS) + self._J3NP = np.zeros((3, N_ALL_JOINTS)) + self._J3NR = np.zeros((3, N_ALL_JOINTS)) + self._J6N = np.zeros((6, self.N_JOINTS)) + self._MNN = np.zeros((N_ALL_JOINTS, N_ALL_JOINTS)) + self._R9 = np.zeros(9) + self._R = np.zeros((3, 3)) + self._x = np.ones(4) + self.N_ALL_JOINTS = N_ALL_JOINTS + + def _load_state(self, q, dq=None, u=None): + """Change the current joint angles + + Parameters + ---------- + q: np.array + The set of joint angles to move the arm to [rad] + dq: np.array + The set of joint velocities to move the arm to [rad/sec] + u: np.array + The set of joint forces to apply to the arm joints [Nm] + """ + # save current state + old_q = np.copy(self.articulation.get_joint_positions()) + old_dq = np.copy(self.articulation.get_joint_velocities()) + old_u = np.copy(self.articulation.get_applied_joint_efforts()) + + # update positions to specified state + #TODO this is a (jaco2 specific) hack to set the first 6 joints + new_q = np.copy(old_q) + new_q[:6] = q + self.articulation.set_joint_positions(new_q) # set the joint positions in the articulation view + + if dq is not None: + self.data.qvel[self.joint_vel_addrs] = np.copy(dq) + if u is not None: + self.data.ctrl[:] = np.copy(u) + + # move simulation forward to calculate new kinematic information + self.world.step(render=True) # execute one physics step and one rendering step + + return old_q, old_dq, old_u + + + + def g(self, q=None): + """ + Returns the joint-space forces due to gravity, Coriolis, and centrifugal effects + in Isaac Sim (equivalent to MuJoCo's qfrc_bias). + + Parameters + ---------- + q: np.ndarray, optional (Default: None) + Joint positions to compute the bias forces at. If None, uses current sim state. + """ + if q is not None: + old_q = self.articulation_view.get_joint_positions() + old_dq = self.articulation_view.get_joint_velocities() + + # Set new state (velocities to zero to isolate gravity) + self.articulation_view.set_joint_positions(q) + self.articulation_view.set_joint_velocities(np.zeros_like(q)) + + self.world.step(render=False) + + # Compute gravity and Coriolis/centrifugal separately + gravity = self.articulation_view.get_generalized_gravity_forces() + coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces() + + # Total generalized bias forces + g = gravity + coriolis + # print("GRAVITY ", gravity) + # print("CORIOLIS ", coriolis) + + if not q is not None: + # Restore old state + self.articulation_view.set_joint_positions(old_q) + self.articulation_view.set_joint_velocities(old_dq) + self.world.step(render=False) + + return -g # match MuJoCo's negative qfrc_bias convention + + + + def dJ(self, name, q=None, dq=None, x=None): + """Returns the derivative of the Jacobian wrt to time + + Parameters + ---------- + name: string + The name of the Mujoco body to retrieve the Jacobian for + q: float numpy.array, optional (Default: None) + The joint angles of the robot. If None the current state is + retrieved from the Mujoco simulator + dq: float numpy.array, optional (Default: None) + The joint velocities of the robot. If None the current state is + retrieved from the Mujoco simulator + x: float numpy.array, optional (Default: None) + """ + # TODO if ever required + # Note from Emo in Mujoco forums: + # 'You would have to use a finate-difference approximation in the + # general case, check differences.cpp' + raise NotImplementedError + + def J(self, name, q=None, x=None, object_type="body"): + """Returns the Jacobian for the specified Mujoco object + + Parameters + ---------- + name: string + The name of the Mujoco body to retrieve the Jacobian for + q: float numpy.array, optional (Default: None) + The joint angles of the robot. If None the current state is + retrieved from the Mujoco simulator + x: float numpy.array, optional (Default: None) + object_type: string, the Mujoco object type, optional (Default: body) + options: body, geom, site + """ + if x is not None and not np.allclose(x, 0): + raise Exception("x offset currently not supported, set to None") + + if not self.use_sim_state and q is not None: + old_q, old_dq, old_u = self._load_state(q) + + if object_type == "body": + # TODO: test if using this function is faster than the old way + # NOTE: for bodies, the Jacobian for the COM is returned + mujoco.mj_jacBodyCom( + self.model, + self.data, + self._J3NP, + self._J3NR, + mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, name), + ) + else: + if object_type == "geom": + jacp = self.data.get_geom_jacp + jacr = self.data.get_geom_jacr + elif object_type == "site": + jacp = self.data.get_site_jacp + jacr = self.data.get_site_jacr + else: + raise Exception("Invalid object type specified: ", object_type) + + jacp(name, self._J3NP) # [self.jac_indices] # pylint: disable=W0106 + jacr(name, self._J3NR) # [self.jac_indices] # pylint: disable=W0106 + + # get the position Jacobian hstacked (1 x N_JOINTS*3) + self._J6N[:3] = self._J3NP[:, self.joint_vel_addrs].reshape((3, self.N_JOINTS)) + # get the rotation Jacobian hstacked (1 x N_JOINTS*3) + self._J6N[3:] = self._J3NR[:, self.joint_vel_addrs].reshape((3, self.N_JOINTS)) + + if not self.use_sim_state and q is not None: + self._load_state(old_q, old_dq, old_u) + + return np.copy(self._J6N) + + def M(self, q=None): + """ + Return the joint-space inertia matrix M(q) using Isaac Sim. + + Parameters + ---------- + q : np.ndarray, optional + Joint positions. If None, use current sim state. + + Returns + ------- + np.ndarray + Dense inertia matrix (DoF x DoF) + """ + + if not self.use_sim_state and q is not None: + # Save current joint state + old_q = self.articulation_view.get_joint_positions() + self.articulation_view.set_joint_positions(q) + self.world.step(render=False) # required to update PhysX buffers + + # Get mass matrix + M = self.articulation_view.get_mass_matrices() + #print(f"Mass matrix M: {M}") + + if not self.use_sim_state and q is not None: + # Restore previous state + self.articulation_view.set_joint_positions(old_q) + self.world.step(render=False) + + return M + + def R(self, name, q=None, object_type="body"): + """ + Returns the rotation matrix of the specified object in Isaac Sim. + + Parameters + ---------- + name : str + The name of the object (body, geom, or site) in the USD stage. + q : np.ndarray, optional + Joint positions (not used here unless you want to simulate a different state). + object_type : str + One of "body", "geom", or "site". + """ + print(f"R: {name}, q: {q}, object_type: {object_type}") + + import omni.isaac.core.utils.prims as prims_utils + prims = prims_utils.get_all_matching_child_prims(self.prim_path) + print(f"prims: {prims}") + # Construct the full prim path + prim_path = f"/World/{name}" + + + + def quaternion(self, name, q=None): + """Returns the quaternion of the specified body + Parameters + ---------- + + name: string + The name of the Mujoco body to retrieve the Jacobian for + q: float numpy.array, optional (Default: None) + The joint angles of the robot. If None the current state is + retrieved from the Mujoco simulator + """ + if not self.use_sim_state and q is not None: + old_q, old_dq, old_u = self._load_state(q) + + quaternion = np.copy(self.data.body(name).xquat) + + if not self.use_sim_state and q is not None: + self._load_state(old_q, old_dq, old_u) + + return quaternion + + def C(self, q=None, dq=None): + """NOTE: The Coriolis and centrifugal effects (and gravity) are + already accounted for by Mujoco in the qfrc_bias variable. There's + no easy way to separate these, so all are returned by the g function. + To prevent accounting for these effects twice, this function will + return an error instead of qfrc_bias again. + """ + raise NotImplementedError( + "Coriolis and centrifugal effects already accounted " + + "for in the term return by the gravity function." + ) + + def T(self, name, q=None, x=None): + """Get the transform matrix of the specified body. + + Parameters + ---------- + name: string + The name of the Mujoco body to retrieve the Jacobian for + q: float numpy.array, optional (Default: None) + The joint angles of the robot. If None the current state is + retrieved from the Mujoco simulator + x: float numpy.array, optional (Default: None) + """ + # TODO if ever required + raise NotImplementedError + + def Tx(self, name, q=None, x=None, object_type="body"): + """ Returns the world-frame Cartesian position of a named link, joint, or site. + + Parameters + ---------- + name : str + Name of the link, joint, or site (e.g., "j2n6s300_link_6"). + q : np.ndarray, optional + Joint positions to temporarily set before computing position. + object_type : str + "body" (link), "joint", or custom prim. + + Returns + ------- + np.ndarray + World position [x, y, z] of the object. + """ + if x is not None and not np.allclose(x, 0): + raise Exception("x offset currently not supported: ", x) + + # Optionally set joint state + if not self.use_sim_state and q is not None: + old_q = self.articulation_view.get_joint_positions() + self.articulation_view.set_joint_positions(q) + self.world.step(render=False) + + prim_path = None + print(f"Tx: name={name}, q={q}, object_type={object_type}") + + + if object_type == "body": + print(f"Finding body with name: {name}") + # Find all RigidBody prims (physics bodies) + print("\nAll Body prims:") + for prim in self.stage.Traverse(): + # Check if the prim path ends with the desired name + if str(prim.GetPath()).endswith(name): + prim_path = prim.GetPath() + break + print(f"Found prim path: {prim_path}") + + + elif object_type == "joint": + # Use joint index to find parent link's prim path + try: + joint_index = self.articulation_view.joint_names.index(name) + link_index = self.articulation_view.joint_parent_indices[joint_index] + prim_path = self.articulation_view._prim_paths[link_index] + except ValueError: + raise RuntimeError(f"Joint name '{name}' not found in articulation.") + + else: + raise ValueError(f"Unsupported object_type: {object_type}") + + if prim_path is None: + raise RuntimeError(f"Could not find prim for name '{name}' with type '{object_type}'.") + + # Get world transform matrix + prim = self.stage.GetPrimAtPath(prim_path) + if not prim.IsValid(): + raise RuntimeError(f"Invalid prim at path: {prim_path}") + + matrix = omni.usd.utils.get_world_transform_matrix(prim) + position = matrix.ExtractTranslation() + + # Restore state if needed + if not self.use_sim_state and q is not None: + self.articulation_view.set_joint_positions(old_q) + self.world.step(render=False) + + Tx =np.array([position[0], position[1], position[2]]) + return Tx + + def T_inv(self, name, q=None, x=None): + """Get the inverse transform matrix of the specified body. + + Parameters + ---------- + name: string + The name of the Mujoco body to retrieve the Jacobian for + q: float numpy.array, optional (Default: None) + The joint angles of the robot. If None the current state is + retrieved from the Mujoco simulator + x: float numpy.array, optional (Default: None) + """ + # TODO if ever required + raise NotImplementedError diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index e87fcfda..0fed5c7b 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -2,7 +2,6 @@ from isaacsim import SimulationApp from .interface import Interface - class IsaacSim(Interface): """An interface for IsaacSim. @@ -18,6 +17,7 @@ class IsaacSim(Interface): def __init__(self, robot_config, dt=0.001, force_download=False): super().__init__(robot_config) + self.robot_config = robot_config self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called @@ -51,38 +51,40 @@ def connect(self, joint_names=None): import omni from isaacsim.robot.policy.examples.robots.h1 import H1FlatTerrainPolicy from isaacsim.storage.native import get_assets_root_path - from omni.isaac.core.articulations import Articulation, ArticulationView# type: ignore + from omni.isaac.core.articulations import Articulation, ArticulationView # type: ignore import omni.isaac.core.utils.stage as stage_utils # type: ignore from isaacsim.core.api.robots import Robot - from isaacsim.core.api.controllers.articulation_controller import ArticulationController + from pxr import UsdLux, Sdf, Gf, UsdPhysics # Create a world self.world = World(physics_dt=self.dt,rendering_dt=self.dt) self.context = omni.usd.get_context() self.stage = self.context.get_stage() - self.world.add_physics_callback("send_actions", self.send_actions) + #TODO necessary for H1 robot + #self.world.add_physics_callback("send_actions", self.send_actions) self.world.scene.add_default_ground_plane() - - - + + # enable physics + scene = UsdPhysics.Scene.Define(self.stage, Sdf.Path("/physicsScene")) + + # set gravity + scene.CreateGravityDirectionAttr().Set(Gf.Vec3f(0.0, 0.0, -1.0)) + scene.CreateGravityMagnitudeAttr().Set(981.0) + + # add lighting + distantLight = UsdLux.DistantLight.Define(self.stage, Sdf.Path("/DistantLight")) + distantLight.CreateIntensityAttr(500) - ''' + ## LOAD Jaco2 robot assets_root_path = get_assets_root_path() - - # Load Jaco2 robot stage_utils.add_reference_to_stage( usd_path=assets_root_path + "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd", # Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd --> 7 DOF arm , not compatible with ABR controller prim_path=self.prim_path, ) - robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - ''' - - + ''' # Load H1 robot - - self.h1 = H1FlatTerrainPolicy( prim_path=self.prim_path, name=self.name, @@ -93,75 +95,40 @@ def connect(self, joint_names=None): usd_path=assets_root_path + "/Isaac/Robots/Unitree/H1/h1.usd", prim_path=self.prim_path, ) - ''' - + ''' # Resetting the world needs to be called before querying anything related to an articulation specifically. # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly self.world.reset() + # Load robot self.articulation = Articulation(prim_path=self.prim_path, name=self.name + "_articulation") self.articulation.initialize() - print("DOF names:", self.articulation.dof_names) + self.world.scene.add(self.articulation) # Add to scene if not already added by higher-level env self.articulation_view = ArticulationView(prim_paths_expr=self.prim_path, name=self.name + "_view") self.world.scene.add(self.articulation_view) - - articulation_controller = ArticulationController() - articulation_controller.initialize(self.articulation_view) - - + self.articulation_view.initialize() + + self.world.reset() # necessary so self.q and self.dq are accessible self.world.initialize_physics() - # get the robot's initial joint angle - #TODO maybe replace with articulation.get_default_state() - self.initial_q = self.get_feedback(arm_only=False)["q"] - - self.joint_pos_addrs = [] self.joint_vel_addrs = [] self.joint_dyn_addrs = [] - - - if joint_names is None: - # if no joint names provided, get addresses of joints in the kinematic - # tree from end-effector (EE) to world body - #bodyid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "EE") - - #bodyid = self.articulation.get_dof_index('right_elbow_joint') - #l_id = self.articulation.get_link_index("right_elbow_joint") # get the link index of the right elbow joint - #bodyid = self.articulation.get_link_index("EE") - dof_names = self.articulation.dof_names - print("DOF names:", dof_names) - #bodid = self.articulation.dof_names.index("EE") - #bodyid = self.articulation_controller - #bodyid = self.articulation.get_dof_index("EE") - - # and working back to the world body - #while self.model.body_parentid[bodyid] != 0: - while bodyid > 1: - print (bodyid) - #first_joint = self.articulation.get_joint_index("EE") - #num_joints = self.model.body_jntnum[bodyid] - - - + print("Connecting to robot config...") self.robot_config._connect( - #self.articulation, + self.world, + self.stage, + self.articulation, self.articulation_view, self.joint_pos_addrs, self.joint_vel_addrs, + self.prim_path ) - - - - - - - def disconnect(self): """Any socket closing etc that must be done to properly shut down""" self.simulation_app.close() # close Isaac Sim @@ -175,58 +142,19 @@ def send_forces(self, u): u : np.array An array of joint torques [Nm] """ - print("robot joint pos: ", self.robot.get_joint_positions()) - print("u: ", u) # Apply the control signal - #self.articulation.set_joint_efforts(u) - #self.robot.set_joint_efforts(u) - self.robot.set_joint_efforts(u) + self.articulation_view.set_joint_efforts(u) # move simulation ahead one time step self.world.step(render=True) # execute one physics step and one rendering step - def send_target_angles(self, q): """Moves the arm to the specified joint angles q : numpy.array the target joint angles [radians] """ - #TODO make more general, with flag - #TODO use same flag for jaco2 and H1 - - # full jaco2 arm with hand DOF - if np.size(q) == 12: - self.articulation.set_joint_positions(q) - # full H1 body with all DOF - if np.size(q) == 19: - self.articulation.set_joint_positions(q) - # only jaco2 arm - if np.size(q) == 6: - fb = self.get_feedback(arm_only=False) - fb_q = fb["q"] - fb_q[:6] = q - self.articulation.set_joint_positions(fb_q) - # only H1 right arm - if np.size(q) == 4: - fb_q = self.initial_q - - #fb = self.get_feedback(arm_only=False) - #fb_q = fb["q"] - - fb_q [6] = q[0] # right_shoulder_pitch_joint - fb_q [10] = q[1] # right_shoulder_roll_joint - fb_q [14] = q[2] # right_shoulder_yaw_joint - fb_q [18] = q[3] # right_elbow_joint - self.articulation.set_joint_positions(fb_q) - - - else: - print("The method send_target_angles of the isaacsim interface does not support the number of joint angles that are attempted to be set.") - # move simulation ahead one time step - self.world.step(render=True) # execute one physics step and one rendering step - @@ -237,36 +165,9 @@ def get_feedback(self, arm_only=True): respectively """ # Get the joint angles and velocities - q = self.articulation.get_joint_positions() - dq = self.articulation.get_joint_velocities() - - # get all DOF of the robot - if arm_only == False: - self.q = q - self.dq = dq - # get only DOF of the arm - else: - if self.name == "jaco2": - # get only the right arm DOF of the jaco 2 without hand - self.q = q[:6] - self.dq = dq[:6] - elif self.name == "h1": - # get only the right arm DOF of the H1 robot - tmp = np.zeros(4) - tmp[0] = q[6] # right_shoulder_pitch_joint - tmp[1] = q[10] # right_shoulder_roll_joint - tmp[2] = q[14] # right_shoulder_yaw_joint - tmp[3] = q[18] # right_elbow_joint - self.q = tmp - - tmp[0] = dq[6] # right_shoulder_pitch_joint - tmp[1] = dq[10] # right_shoulder_roll_joint - tmp[2] = dq[14] # right_shoulder_yaw_joint - tmp[3] = dq[18] # right_elbow_joint - self.dq = tmp + self.q = self.articulation.get_joint_positions() + self.dq = self.articulation.get_joint_velocities() return {"q": self.q, "dq": self.dq} - - def get_xyz(self, name): """Returns the xyz position of the specified object diff --git a/examples/IsaacSim/force_floating_control.py b/examples/IsaacSim/force_floating_control.py new file mode 100644 index 00000000..41d1124a --- /dev/null +++ b/examples/IsaacSim/force_floating_control.py @@ -0,0 +1,85 @@ +""" +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" + arm_model = "jaco2" +# initialize our robot config +robot_config = arm(arm_model) +# create the IsaacSim interface and connect up +interface = IsaacSim(robot_config, dt=0.001) +interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) +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_name = "end_effector" # EE +ee_track = [] +q_track = [] + +try: + # get the end-effector's initial position + feedback = interface.get_feedback() + start = robot_config.Tx(ee_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 Mujoco + interface.send_forces(u) + + # calculate the position of the hand + hand_xyz = robot_config.Tx(ee_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/blog_minimal.py b/examples/isaacsim/blog_minimal.py deleted file mode 100644 index 73e8bb20..00000000 --- a/examples/isaacsim/blog_minimal.py +++ /dev/null @@ -1,41 +0,0 @@ -import numpy as np -from abr_control.arms import ur5 as arm -from abr_control.controllers import OSC -from abr_control.interfaces.nv_isaacsim import IsaacSim -from flying_cube import FlyingCube - -# https://studywolf.wordpress.com/2017/07/01/abr-control-0-1-repo-public-release/ - - -# initialize our robot config for the ur5 -robot_config = arm.Config(use_cython=True) - -# instantiate controller -ctrlr = OSC(robot_config, kp=200, vmax=[0.5, 0]) - - -# create our VREP interface -interface = IsaacSim(robot_config, dt=.001) -interface.connect() - -isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") -target_xyz = np.array([0.2, 0.2, 0.2]) -# set the target object's position in VREP -interface.set_xyz(name='target', xyz=target_xyz) - -count = 0.0 -while count < 4000: # run for 1.5 simulated seconds - # get joint angle and velocity feedback - feedback = interface.get_feedback() - # calculate the control signal - u = ctrlr.generate( - q=feedback['q'], - dq=feedback['dq'], - target=target_xyz) - - - # send forces into VREP, step the sim forward - interface.send_forces(u) - - count += 1 -interface.disconnect() \ No newline at end of file diff --git a/examples/isaacsim/cube.py b/examples/isaacsim/cube.py deleted file mode 100644 index 0214fcbe..00000000 --- a/examples/isaacsim/cube.py +++ /dev/null @@ -1,48 +0,0 @@ -import numpy as np -from abr_control.arms import ur5 as arm -from abr_control.interfaces.nv_isaacsim import IsaacSim -from abr_control.utils import transformations - -# Sim step size -dt = 0.005 - -# Initialize our robot config -robot_config = arm.Config() - -# Create our interface -interface = IsaacSim(robot_config, dt=dt) -interface.connect() -# Imports must be done after instantiating the SimulationApp, which is done by the setup in the isaacsim interface -from isaacsim.core.api import World # type: ignore -from isaacsim.core.api.objects import DynamicCuboid # type: ignore - -# Create a world -world = World(physics_dt=dt,rendering_dt=dt) -world.scene.add_default_ground_plane() -fancy_cube = world.scene.add( - - DynamicCuboid( - prim_path="/World/random_cube", - name="fancy_cube", - position=np.array([0, 0, 1.0]), - scale=np.array([0.5015, 0.5015, 0.5015]), - color=np.array([0, 0, 1.0]), - )) - - -world.reset() -for i in range(500): - - position, orientation = fancy_cube.get_world_pose() - linear_velocity = fancy_cube.get_linear_velocity() - - # will be shown on terminal - print("Cube position is : " + str(position)) - print("Cube's orientation is : " + str(orientation)) - print("Cube's linear velocity is : " + str(linear_velocity)) - - # we have control over stepping physics and rendering in this workflow - # things run in sync - world.step(render=True) # execute one physics step and one rendering step - -interface.disconnect() \ No newline at end of file diff --git a/examples/isaacsim/flying_cube.py b/examples/isaacsim/flying_cube.py deleted file mode 100644 index fe9542f4..00000000 --- a/examples/isaacsim/flying_cube.py +++ /dev/null @@ -1,55 +0,0 @@ -import numpy as np - -class FlyingCube(): - - def __init__(self, dt, world, prim_path="/World/random_cube", name="fancy_cube", position=np.array([0, 0, 1.0]), scale=np.array([0.1015, 0.1015, 0.1015]), color=np.array([0, 0, 1.0])): - # IsaacSim imports must be done after instantiating the SimulationApp, which is done by the setup in the isaacsim interface - from omni.isaac.core.objects import DynamicCuboid # type: ignore - from omni.isaac.dynamic_control import _dynamic_control # type: ignore - - self.world = world - fancy_cube = self.world.scene.add( - DynamicCuboid( - prim_path=prim_path, - name=name, - position=position, - scale=scale, - color=color, - ) - ) - # Set the cube to be dynamic using dynamic_control - self._dc = _dynamic_control.acquire_dynamic_control_interface() - self.cube_handle = self._dc.get_rigid_body(prim_path) - self._cube = self.world.scene.get_object(name) - - # Add velocity as an attribute of the world so that it is available at callback time - self.input_velocity = np.zeros(3) - - self.world.add_physics_callback(name + "_send_actions", self.send_actions) - self.world.add_physics_callback(name + "_make_state", self.make_state) - - def send_actions(self, dt): - self._dc.set_rigid_body_linear_velocity(self.cube_handle, self.input_velocity) - - def make_state(self, dt): - position, orientation = self._cube.get_world_pose() - linear_velocity = self._cube.get_linear_velocity() - return { - 'position': np.array([[0., 0., 1.]]), - 'velocity': np.array([[0., 0., 0.]]), - 'orientation': np.array([[0., 0., 0., 0.]]) - } - - def step(self, velocity): - # Update the input velocity - self.input_velocity = velocity - self.world.step() - - # Get the cube's current state - position, orientation = self._cube.get_world_pose() - linear_velocity = self._cube.get_linear_velocity() - return np.concatenate([position, linear_velocity, orientation], axis=0) - - def _get_obj_pos(self): - position, orientation = self._cube.get_world_pose() - return position \ No newline at end of file diff --git a/examples/isaacsim/force_osc_xyz_avoid_obstacle path_planner.py b/examples/isaacsim/force_osc_xyz_avoid_obstacle path_planner.py deleted file mode 100644 index 4dee680f..00000000 --- a/examples/isaacsim/force_osc_xyz_avoid_obstacle path_planner.py +++ /dev/null @@ -1,173 +0,0 @@ -""" -Move the UR5 CoppeliaSim arm to a target position while avoiding an obstacle. -The simulation ends after 1500 time steps, and the trajectory -of the end-effector is plotted in 3D. -""" -import numpy as np -from flying_cube import FlyingCube -from abr_control.arms import ur5 as arm -from abr_control.controllers import Joint, path_planners -from abr_control.utils import transformations - - - -from abr_control.controllers import OSC, AvoidObstacles, Damping -from abr_control.interfaces.nv_isaacsim import IsaacSim - -# initialize our robot config -robot_config = arm.Config() -# change this flag to False to use position control -use_force_control = False -n_timesteps = 2000 -path_planner = path_planners.InverseKinematics(robot_config) - - - -avoid = AvoidObstacles(robot_config) -# damp the movements of the arm -if use_force_control: - # create an operational space controller - damping = Damping(robot_config, kv=10) - # instantiate the REACH controller with obstacle avoidance - ctrlr = OSC( - robot_config, - kp=200, - null_controllers=[avoid, damping], - vmax=[0.5, 0], # [m/s, rad/s] - # control (x, y, z) out of [x, y, z, alpha, beta, gamma] - ctrlr_dof=[True, True, True, False, False, False], - ) - -# create our CoppeliaSim interface -interface = IsaacSim(robot_config, dt=0.005) -interface.connect() - -# set up lists for tracking data -ee_track = [] -target_track = [] -obstacle_track = [] - -moving_obstacle = True -isaac_obstacle = FlyingCube(interface.dt, interface.world, name ="obstacle", prim_path="/World/obstacle", color=np.array([0, 1.0, 0])) -isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") -obstacle_xyz = np.array([0.09596, -0.2661, 0.64204]) -try: - # get visual position of end point of object - feedback = interface.get_feedback() - start = robot_config.Tx("EE", q=feedback["q"]) - # make the target offset from that start position - target_xyz = start + np.array([0.2, -0.2, 0.0]) - interface.set_xyz(name="target", xyz=target_xyz) - interface.set_xyz(name="obstacle", xyz=obstacle_xyz) - - - count = 0.0 - obs_count = 0.0 - print("\nSimulation starting...\n") - - while count < n_timesteps : - # get joint angle and velocity feedback - feedback = interface.get_feedback() - #target = np.hstack( - # [interface.get_xyz("target"), interface.get_orientation("target")] - #) - - # position control - R = robot_config.R("EE", q=feedback["q"]) - target_orientation = transformations.euler_from_matrix(R, "sxyz") - # can use 3 different methods to calculate inverse kinematics - # see inverse_kinematics.py file for details - path_planner.generate_path( - position=feedback["q"], - target_position=np.hstack(( - [interface.get_xyz("target"), interface.get_orientation("target")])), - method=3, - plot=False, - ) - # returns desired [position, velocity] - target, _ = path_planner.next() - - - - # get obstacle position from IsaacSim - obs_x, obs_y, obs_z = interface.get_xyz("obstacle") # pylint: disable=W0632 - # update avoidance system about obstacle position - avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]]) - if moving_obstacle is True: - obs_x = 0.125 + 0.25 * np.sin(obs_count) - obs_count += 0.05 - interface.set_xyz(name="obstacle", xyz=[obs_x, obs_y, obs_z]) - - if use_force_control: - # calculate the control signal - u = ctrlr.generate( - q=feedback["q"], - dq=feedback["dq"], - target=target, - ) - # send forces into IsaacSim, step the sim forward - interface.send_forces(u) - else: - # use position control, send angles into IsaacSim - interface.send_target_angles(target[: robot_config.N_JOINTS]) - - - - - #print( 'robot_config: ' , robot_config) - #print( 'robot_config.N_JOINTS: ' , robot_config.N_JOINTS) - #print( 'target[: robot_config.N_JOINTS]: ' , target[: robot_config.N_JOINTS]) - - - # calculate end-effector position - ee_xyz = robot_config.Tx("EE", q=feedback["q"]) - # track data - ee_track.append(np.copy(ee_xyz)) - target_track.append(np.copy(target[:3])) - obstacle_track.append(np.copy([obs_x, obs_y, obs_z])) - - count += 1 - -finally: - # stop and reset IsaacSim - interface.disconnect() - - print("Simulation terminated...") - - ee_track = np.array(ee_track) - target_track = np.array(target_track) - obstacle_track = np.array(obstacle_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[0, 0], - target_track[0, 1], - target_track[0, 2], - label="target", - c="g", - ) - ax2.plot( - obstacle_track[:, 0], - obstacle_track[:, 1], - target_track[:, 2], - label="obstacle", - c="r", - ) - ax2.legend() - plt.show() diff --git a/examples/isaacsim/force_osc_xyz_avoid_obstacle.py b/examples/isaacsim/force_osc_xyz_avoid_obstacle.py deleted file mode 100644 index 505cea76..00000000 --- a/examples/isaacsim/force_osc_xyz_avoid_obstacle.py +++ /dev/null @@ -1,146 +0,0 @@ -""" -Move the UR5 CoppeliaSim arm to a target position while avoiding an obstacle. -The simulation ends after 1500 time steps, and the trajectory -of the end-effector is plotted in 3D. -""" -import numpy as np -from flying_cube import FlyingCube -from abr_control.arms import ur5 as arm - -from abr_control.controllers import OSC, AvoidObstacles, Damping -from abr_control.interfaces.nv_isaacsim import IsaacSim - -# initialize our robot config -robot_config = arm.Config() -# change this flag to False to use position control -use_force_control = False -n_timesteps = 2000 - - -avoid = AvoidObstacles(robot_config) -# damp the movements of the arm -if use_force_control: - # create an operational space controller - damping = Damping(robot_config, kv=10) - # instantiate the REACH controller with obstacle avoidance - ctrlr = OSC( - robot_config, - kp=200, - null_controllers=[avoid, damping], - vmax=[0.5, 0], # [m/s, rad/s] - # control (x, y, z) out of [x, y, z, alpha, beta, gamma] - ctrlr_dof=[True, True, True, False, False, False], - ) - -# create our CoppeliaSim interface -interface = IsaacSim(robot_config, dt=0.005) -interface.connect() - -# set up lists for tracking data -ee_track = [] -target_track = [] -obstacle_track = [] - -moving_obstacle = True -isaac_obstacle = FlyingCube(interface.dt, interface.world, name ="obstacle", prim_path="/World/obstacle", color=np.array([0, 1.0, 0])) -isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") -obstacle_xyz = np.array([0.09596, -0.2661, 0.64204]) -try: - # get visual position of end point of object - feedback = interface.get_feedback() - start = robot_config.Tx("EE", q=feedback["q"]) - # make the target offset from that start position - target_xyz = start + np.array([0.2, -0.2, 0.0]) - interface.set_xyz(name="target", xyz=target_xyz) - interface.set_xyz(name="obstacle", xyz=obstacle_xyz) - - - count = 0.0 - obs_count = 0.0 - print("\nSimulation starting...\n") - - while count < n_timesteps : - # get joint angle and velocity feedback - feedback = interface.get_feedback() - target = np.hstack( - [interface.get_xyz("target"), interface.get_orientation("target")] - ) - - # get obstacle position from IsaacSim - obs_x, obs_y, obs_z = interface.get_xyz("obstacle") # pylint: disable=W0632 - # update avoidance system about obstacle position - avoid.set_obstacles([[obs_x, obs_y, obs_z, 0.05]]) - if moving_obstacle is True: - obs_x = 0.125 + 0.25 * np.sin(obs_count) - obs_count += 0.05 - interface.set_xyz(name="obstacle", xyz=[obs_x, obs_y, obs_z]) - - if use_force_control: - # calculate the control signal - u = ctrlr.generate( - q=feedback["q"], - dq=feedback["dq"], - target=target, - ) - # send forces into IsaacSim, step the sim forward - interface.send_forces(u) - else: - # use position control, send angles into IsaacSim - interface.send_target_angles(target[: robot_config.N_JOINTS]) - print( 'robot_config: ' , robot_config) - print( 'robot_config.N_JOINTS: ' , robot_config.N_JOINTS) - print( 'target[: robot_config.N_JOINTS]: ' , target[: robot_config.N_JOINTS]) - - - # calculate end-effector position - ee_xyz = robot_config.Tx("EE", q=feedback["q"]) - # track data - ee_track.append(np.copy(ee_xyz)) - target_track.append(np.copy(target[:3])) - obstacle_track.append(np.copy([obs_x, obs_y, obs_z])) - - count += 1 - -finally: - # stop and reset IsaacSim - interface.disconnect() - - print("Simulation terminated...") - - ee_track = np.array(ee_track) - target_track = np.array(target_track) - obstacle_track = np.array(obstacle_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[0, 0], - target_track[0, 1], - target_track[0, 2], - label="target", - c="g", - ) - ax2.plot( - obstacle_track[:, 0], - obstacle_track[:, 1], - target_track[:, 2], - label="obstacle", - c="r", - ) - ax2.legend() - plt.show() diff --git a/examples/isaacsim/h1_lock_orientation.py b/examples/isaacsim/h1_lock_orientation.py deleted file mode 100644 index bdb76300..00000000 --- a/examples/isaacsim/h1_lock_orientation.py +++ /dev/null @@ -1,46 +0,0 @@ -""" -Running the joint controller with an inverse kinematics path planner -for a Mujoco simulation. The path planning system will generate -a trajectory in joint space that moves the end effector in a straight line -to the target, which changes every n time steps. -""" -import numpy as np -#from abr_control.arms.mujoco_config import MujocoConfig as arm -from abr_control.arms import jaco2 as arm -from abr_control.interfaces.nv_isaacsim import IsaacSim - - -robot_config = arm.Config() - -# create our path planner -n_timesteps = 2000 -# create our interface -dt = 0.001 -interface = IsaacSim(robot_config, dt=dt) -interface.connect() - -feedback = interface.get_feedback() -q=feedback["q"] - -interface.send_target_angles(q) - -try: - print("\nSimulation starting...") - count = 0 - while 1: - - feedback = interface.get_feedback() - q=feedback["q"] - #print("Q: ", q) - - interface.send_target_angles(q) - #interface.world.step(render=True) # execute one physics step and one rendering step - - count += 1 - - -finally: - # stop and reset the simulation - interface.disconnect() - - print("Simulation terminated...") \ No newline at end of file diff --git a/examples/isaacsim/jaco_2.py b/examples/isaacsim/jaco_2.py deleted file mode 100644 index 39710038..00000000 --- a/examples/isaacsim/jaco_2.py +++ /dev/null @@ -1,37 +0,0 @@ -""" -Running the joint controller with an inverse kinematics path planner -for a Mujoco simulation. The path planning system will generate -a trajectory in joint space that moves the end effector in a straight line -to the target, which changes every n time steps. -""" -import numpy as np -from abr_control.arms import jaco2 as arm -from abr_control.interfaces.nv_isaacsim_usd_jaco import IsaacSim - - -robot_config = arm.Config() - -# create our path planner -n_timesteps = 2000 -# create our interface -dt = 0.001 -interface = IsaacSim(robot_config, dt=dt) -interface.connect() -feedback = interface.get_feedback() - -try: - print("\nSimulation starting...") - count = 0 - while 1: - - # use position control - #interface.send_target_angles(target[: robot_config.N_JOINTS]) - q=feedback["q"] - interface.send_target_angles(q) - count += 1 - -finally: - # stop and reset the simulation - interface.disconnect() - - print("Simulation terminated...") \ No newline at end of file diff --git a/examples/isaacsim/position_joint_control_inverse_kinematics.py b/examples/isaacsim/position_joint_control_inverse_kinematics.py deleted file mode 100644 index 5296d90c..00000000 --- a/examples/isaacsim/position_joint_control_inverse_kinematics.py +++ /dev/null @@ -1,83 +0,0 @@ -""" -Running the joint controller with an inverse kinematics path planner -for a Mujoco simulation. The path planning system will generate -a trajectory in joint space that moves the end effector in a straight line -to the target, which changes every n time steps. -""" -import numpy as np -#from abr_control.arms.mujoco_config import MujocoConfig as arm -from abr_control.arms import jaco2 as arm -#import glfw -from abr_control.controllers import path_planners -#from abr_control.interfaces.mujoco import Mujoco -from abr_control.interfaces.nv_isaacsim_usd_jaco import IsaacSim -from abr_control.utils import transformations -from flying_cube import FlyingCube - - -#robot_config = arm("ur5", use_sim_state=False) -robot_config = arm.Config() - - -# create our path planner -n_timesteps = 2000 -path_planner = path_planners.InverseKinematics(robot_config) - -# create our interface -dt = 0.001 -interface = IsaacSim(robot_config, dt=dt) -interface.connect() -interface.send_target_angles(robot_config.START_ANGLES) -feedback = interface.get_feedback() -isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") - - -try: - print("\nSimulation starting...") - print("Click to move the target.\n") - - count = 0 - while 1: - - if count % n_timesteps == 0: - print("Moving target...") - feedback = interface.get_feedback() - target_xyz = np.array( - [ - np.random.random() * 0.5 - 0.25, - np.random.random() * 0.5 - 0.25, - np.random.random() * 0.5 + 0.5, - ] - ) - R = robot_config.R("EE", q=feedback["q"]) - #R = robot_config.R("wrist_3_joint", q=feedback["q"]) - - target_orientation = transformations.euler_from_matrix(R, "sxyz") - # update the position of the target - interface.set_xyz("target", target_xyz) - - # can use 3 different methods to calculate inverse kinematics - # see inverse_kinematics.py file for details - path_planner.generate_path( - position=feedback["q"], - target_position=np.hstack([target_xyz, target_orientation]), - method=3, - dt=0.005, - n_timesteps=n_timesteps, - plot=False, - ) - - # returns desired [position, velocity] - target = path_planner.next()[0] - - # use position control - interface.send_target_angles(target[: robot_config.N_JOINTS]) - #interface.viewer.render() - - count += 1 - -finally: - # stop and reset the simulation - interface.disconnect() - - print("Simulation terminated...") \ No newline at end of file diff --git a/examples/isaacsim/position_joint_control_inverse_kinematics_H1.py b/examples/isaacsim/position_joint_control_inverse_kinematics_H1.py deleted file mode 100644 index eb1133e3..00000000 --- a/examples/isaacsim/position_joint_control_inverse_kinematics_H1.py +++ /dev/null @@ -1,117 +0,0 @@ -""" -Running the joint controller with an inverse kinematics path planner -for a Mujoco simulation. The path planning system will generate -a trajectory in joint space that moves the end effector in a straight line -to the target, which changes every n time steps. -""" -import numpy as np -#from abr_control.arms.mujoco_config import MujocoConfig as arm -from abr_control.arms import unitree_H1 as arm -#import glfw -from abr_control.controllers import path_planners -#from abr_control.interfaces.mujoco import Mujoco -from abr_control.interfaces.nv_isaacsim import IsaacSim -from abr_control.utils import transformations -from flying_cube import FlyingCube - -''' -from abr_control.arms import ur5 as arm1 -ur5 = arm1.Config() -print("ur5: ", ur5.N_JOINTS) -print("L: ", len(ur5.L)) -from abr_control.arms import jaco2 as arm2 -jaco = arm2.Config() -print("jaco: ", jaco.N_JOINTS) -print("L: ", len(jaco.L)) - -from abr_control.arms import onejoint as arm3 -onejoint = arm3.Config() -print("onejoint: ", onejoint.N_JOINTS) -print("L: ", len(onejoint.L)) - -from abr_control.arms import twojoint as arm4 -twojoint = arm4.Config() -print("twojoint: ", twojoint.N_JOINTS) - -print("L: ", len(twojoint.L)) - -from abr_control.arms import threejoint as arm5 -threejoint = arm5.Config() -print("threejoint: ", threejoint.N_JOINTS) -print("L: ", len(threejoint.L)) -''' - -robot_config = arm.Config() -#print("h1: ", robot_config.N_JOINTS) -#print("L: ", len(robot_config.L)) - - -# create our path planner -n_timesteps = 2000 -path_planner = path_planners.InverseKinematics(robot_config) - -# create our interface -dt = 0.001 -interface = IsaacSim(robot_config, dt=dt) -interface.connect() -interface.send_target_angles(robot_config.START_ANGLES) -feedback = interface.get_feedback() -isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") - - - - -#inertias = interface.articulation_view.get_body_inertias() - -#inertias_right_shoulder_pitch_joint = interface.articulation_view.get_body_inertias(body_indices=np.array([6])) - - -try: - print("\nSimulation starting...") - print("Click to move the target.\n") - - count = 0 - while 1: - - if count % n_timesteps == 0: - print("Moving target...") - feedback = interface.get_feedback() - target_xyz = np.array( - [ - np.random.random() * 0.5 + 0.2, - np.random.random() * 1.1 - 0.9, - np.random.random() * 1.0 + 0.9, - ] - ) - R = robot_config.R("EE", q=feedback["q"]) - #R = robot_config.R("wrist_3_joint", q=feedback["q"]) - - target_orientation = transformations.euler_from_matrix(R, "sxyz") - # update the position of the target - interface.set_xyz("target", target_xyz) - - # can use 3 different methods to calculate inverse kinematics - # see inverse_kinematics.py file for details - path_planner.generate_path( - position=feedback["q"], - target_position=np.hstack([target_xyz, target_orientation]), - method=3, - dt=0.005, - n_timesteps=n_timesteps, - plot=False, - ) - - # returns desired [position, velocity] - target = path_planner.next()[0] - - # use position control - interface.send_target_angles(target[: robot_config.N_JOINTS]) - #interface.world.step(render=True) # execute one physics step and one rendering step - - count += 1 - -finally: - # stop and reset the simulation - interface.disconnect() - - print("Simulation terminated...") \ No newline at end of file diff --git a/examples/isaacsim/ur10_moving_cube.py b/examples/isaacsim/ur10_moving_cube.py deleted file mode 100644 index 16c617c8..00000000 --- a/examples/isaacsim/ur10_moving_cube.py +++ /dev/null @@ -1,27 +0,0 @@ -import numpy as np -from abr_control.arms import ur5 as arm -from abr_control.interfaces.nv_isaacsim import IsaacSim -from flying_cube import FlyingCube - -if __name__ == "__main__": - dt = 0.01 - - # Initialize our robot config - robot_config = arm.Config() - - # Create our interface - interface = IsaacSim(robot_config, dt=dt) - interface.connect() - - flying_cube = FlyingCube(dt, interface.world) - - interface.send_target_angles(np.array([1.0, 0.0,2.0,3.0,0.0,0.0])) - feedback = interface.get_feedback() - print ("q : " + str(feedback["q"])) - print ("dq : " + str(feedback["dq"])) - - # Example: Apply sinusoidal velocity to the cube - for t in np.arange(0, 10, dt): - velocity = np.array([np.sin(t), np.cos(t), 0.]) - state = flying_cube.step(velocity) - #print(f"Time: {t:.2f}, State: {state}") \ No newline at end of file diff --git a/examples/isaacsim/ur5.py b/examples/isaacsim/ur5.py deleted file mode 100644 index 4527942a..00000000 --- a/examples/isaacsim/ur5.py +++ /dev/null @@ -1,30 +0,0 @@ -import numpy as np -from abr_control.arms import ur5 as arm -from abr_control.interfaces.nv_isaacsim import IsaacSim -from abr_control.utils import transformations - -# Sim step size -dt = 0.005 - -# Initialize our robot config -robot_config = arm.Config() - -# Create our interface -interface = IsaacSim(robot_config, dt=dt) -interface.connect() -feedback = interface.get_feedback() -print ("q : " + str(feedback["q"])) -print ("dq : " + str(feedback["dq"])) - -count = 0 - -for i in range(5000): - val = count % 6 - interface.send_target_angles(np.array([val, 5.6,1.0,0.0,0.0,0.0])) - - count += 1 - import time - time.sleep(5) - - -interface.disconnect() \ No newline at end of file From 0d70a58ff01e7505993281b33dfba55a0d8c9265 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Fri, 20 Jun 2025 10:11:07 -0400 Subject: [PATCH 16/42] implementation method quaternion, Tx, R --- abr_control/arms/isaacsim_config.py | 174 ++++++++++++------ abr_control/interfaces/nv_isaacsim.py | 106 ++++++++++- examples/IsaacSim/flying_cube.py | 64 +++++++ ...sition_joint_control_inverse_kinematics.py | 140 ++++++++++++++ 4 files changed, 426 insertions(+), 58 deletions(-) create mode 100644 examples/IsaacSim/flying_cube.py create mode 100644 examples/IsaacSim/position_joint_control_inverse_kinematics.py diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index d2959c4e..a5d011bd 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -4,6 +4,7 @@ from abr_control.utils import download_meshes import omni + class IsaacsimConfig: """A wrapper on the Mujoco simulator to generate all the kinematics and dynamics calculations necessary for controllers. @@ -153,7 +154,9 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr # number of controllable joints in the robot arm - self.N_JOINTS = len(self.joint_vel_addrs) + #TODO: this is a hack for the jaco2 robot, which has 12 joints + #self.N_JOINTS = len(self.joint_vel_addrs) + self.N_JOINTS = self.articulation_view.num_dof print (f"Number of controllable joints: {self.N_JOINTS}") # number of joints in the IsaacSim simulation N_ALL_JOINTS = self.articulation_view.num_dof @@ -196,21 +199,20 @@ def _load_state(self, q, dq=None, u=None): u: np.array The set of joint forces to apply to the arm joints [Nm] """ - # save current state + old_q = np.copy(self.articulation.get_joint_positions()) old_dq = np.copy(self.articulation.get_joint_velocities()) old_u = np.copy(self.articulation.get_applied_joint_efforts()) - # update positions to specified state - #TODO this is a (jaco2 specific) hack to set the first 6 joints - new_q = np.copy(old_q) - new_q[:6] = q - self.articulation.set_joint_positions(new_q) # set the joint positions in the articulation view + # update positions to specified state + self.articulation.set_joint_positions(q) # set the joint positions in the articulation view if dq is not None: - self.data.qvel[self.joint_vel_addrs] = np.copy(dq) + #self.data.qvel[self.joint_vel_addrs] = np.copy(dq) + self.articulation_view.set_joint_velocities(dq) if u is not None: - self.data.ctrl[:] = np.copy(u) + #self.data.ctrl[:] = np.copy(u) + self.articulation_view.set_joint_efforts(u) # move simulation forward to calculate new kinematic information self.world.step(render=True) # execute one physics step and one rendering step @@ -229,9 +231,8 @@ def g(self, q=None): q: np.ndarray, optional (Default: None) Joint positions to compute the bias forces at. If None, uses current sim state. """ - if q is not None: - old_q = self.articulation_view.get_joint_positions() - old_dq = self.articulation_view.get_joint_velocities() + if not self.use_sim_state and q is not None: + old_q, old_dq, old_u = self._load_state(q) # Set new state (velocities to zero to isolate gravity) self.articulation_view.set_joint_positions(q) @@ -280,7 +281,9 @@ def dJ(self, name, q=None, dq=None, x=None): raise NotImplementedError def J(self, name, q=None, x=None, object_type="body"): - """Returns the Jacobian for the specified Mujoco object + """Returns the Jacobian for the specified link, + computed at the origin of the link's rigid body frame, + which in Isaac Sim coincides with the link’s center of mass. Parameters ---------- @@ -293,6 +296,10 @@ def J(self, name, q=None, x=None, object_type="body"): object_type: string, the Mujoco object type, optional (Default: body) options: body, geom, site """ + #TODO outsource this to a common function and check is can be qued for EE and + # end_effector at the same time, or checked which is used for the current robot + if name == "EE": name = "end_effector" + if x is not None and not np.allclose(x, 0): raise Exception("x offset currently not supported, set to None") @@ -300,32 +307,31 @@ def J(self, name, q=None, x=None, object_type="body"): old_q, old_dq, old_u = self._load_state(q) if object_type == "body": - # TODO: test if using this function is faster than the old way - # NOTE: for bodies, the Jacobian for the COM is returned - mujoco.mj_jacBodyCom( - self.model, - self.data, - self._J3NP, - self._J3NR, - mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, name), - ) + try: + #TODO maybe outsource if also needed elsewhere + link_list = self.articulation_view.body_names + prefix = link_list[0].split('_')[0] # Get the prefix from the first link name + full_name = f"{prefix}_{name}" # Construct the full name + link_index = link_list.index(full_name) + except ValueError: + raise RuntimeError(f"Link '{name}' not found in articulation.") + jacobians = self.articulation_view.get_jacobians(clone=True) # shape: (num_links, 6, num_dofs) + J = jacobians[0, link_index, :, :] + + else: if object_type == "geom": - jacp = self.data.get_geom_jacp - jacr = self.data.get_geom_jacr + raise Exception("Calculating the Jacobian is not yet implemented for 'geom'.") elif object_type == "site": - jacp = self.data.get_site_jacp - jacr = self.data.get_site_jacr + raise Exception("Calculating the Jacobian is not yet implemented for 'site'.") else: raise Exception("Invalid object type specified: ", object_type) - jacp(name, self._J3NP) # [self.jac_indices] # pylint: disable=W0106 - jacr(name, self._J3NR) # [self.jac_indices] # pylint: disable=W0106 + # get the position Jacobian (linear velocity Jacobian (Jv)) + self._J6N[:3] = J[:3, :] + # get the rotation Jacobian (angular velocity Jacobian (Jω)) + self._J6N[3:] = J[3:, :] - # get the position Jacobian hstacked (1 x N_JOINTS*3) - self._J6N[:3] = self._J3NP[:, self.joint_vel_addrs].reshape((3, self.N_JOINTS)) - # get the rotation Jacobian hstacked (1 x N_JOINTS*3) - self._J6N[3:] = self._J3NR[:, self.joint_vel_addrs].reshape((3, self.N_JOINTS)) if not self.use_sim_state and q is not None: self._load_state(old_q, old_dq, old_u) @@ -377,14 +383,41 @@ def R(self, name, q=None, object_type="body"): object_type : str One of "body", "geom", or "site". """ - print(f"R: {name}, q: {q}, object_type: {object_type}") - - import omni.isaac.core.utils.prims as prims_utils - prims = prims_utils.get_all_matching_child_prims(self.prim_path) - print(f"prims: {prims}") - # Construct the full prim path - prim_path = f"/World/{name}" - + #print(f"R: {name}, q: {q}, object_type: {object_type}") + if not self.use_sim_state and q is not None: + old_q = self.articulation_view.get_joint_positions() + self.articulation_view.set_joint_positions(q) + #self._world.step(render=False) + + if object_type == "body": + print(f"Finding body with name: {name}") + # Look for a link prim ending in the given name + prim_path = self._get_prim_path(name) + elif object_type in ["site", "geom"]: + # Assume full path is given or fixed base path + prim_path = f"{self.prim_path}/{name}" + else: + raise ValueError(f"Unsupported object type: {object_type}") + + prim = self.stage.GetPrimAtPath(prim_path) + if not prim.IsValid(): + raise RuntimeError(f"Prim '{prim_path}' not found") + + # Get 4x4 world transform matrix + matrix = omni.usd.get_world_transform_matrix(prim) + + # Convert to 3x3 rotation matrix using numpy + 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]] + ]) + if not self.use_sim_state and q is not None: + self.articulation_view.set_joint_positions(old_q) + #self._world.step(render=False) + + return R + def quaternion(self, name, q=None): @@ -398,15 +431,29 @@ def quaternion(self, name, q=None): The joint angles of the robot. If None the current state is retrieved from the Mujoco simulator """ + #TODO outsource this to a common function and check is can be qued for EE and + # end_effector at the same time, or checked which is used for the current robot + if name == "EE": name = "end_effector" if not self.use_sim_state and q is not None: old_q, old_dq, old_u = self._load_state(q) - quaternion = np.copy(self.data.body(name).xquat) + prim_path = self._get_prim_path(name) + prim = self.stage.GetPrimAtPath(prim_path) + + # Get 4x4 world transform matrix + matrix = omni.usd.get_world_transform_matrix(prim) + + from pxr import Gf + # Extract quaternion (as Gf.Quatf or Gf.Quatd) + quat = matrix.ExtractRotationQuat() # returns Gf.Quatd or Gf.Quatf + + # Convert to [w, x, y, z] NumPy array + quat_np = np.array([quat.GetReal(), *quat.GetImaginary()]) if not self.use_sim_state and q is not None: self._load_state(old_q, old_dq, old_u) - return quaternion + return quat_np def C(self, q=None, dq=None): """NOTE: The Coriolis and centrifugal effects (and gravity) are @@ -452,6 +499,7 @@ def Tx(self, name, q=None, x=None, object_type="body"): np.ndarray World position [x, y, z] of the object. """ + if name == "EE": name = "end_effector" if x is not None and not np.allclose(x, 0): raise Exception("x offset currently not supported: ", x) @@ -462,20 +510,12 @@ def Tx(self, name, q=None, x=None, object_type="body"): self.world.step(render=False) prim_path = None - print(f"Tx: name={name}, q={q}, object_type={object_type}") + #print(f"Tx: name={name}, q={q}, object_type={object_type}") - + #TODO similar to others like R, ousource if object_type == "body": - print(f"Finding body with name: {name}") - # Find all RigidBody prims (physics bodies) - print("\nAll Body prims:") - for prim in self.stage.Traverse(): - # Check if the prim path ends with the desired name - if str(prim.GetPath()).endswith(name): - prim_path = prim.GetPath() - break - print(f"Found prim path: {prim_path}") - + prim_path = self._get_prim_path(name) + elif object_type == "joint": # Use joint index to find parent link's prim path @@ -522,3 +562,29 @@ def T_inv(self, name, q=None, x=None): """ # TODO if ever required raise NotImplementedError + + + #TODO remove or see if useful + ''' + def save_current_state(self): + old_q = np.copy(self.articulation.get_joint_positions()) + old_dq = np.copy(self.articulation.get_joint_velocities()) + old_u = np.copy(self.articulation.get_applied_joint_efforts()) + + return old_q, old_dq, old_u + ''' + + #TODO remove or use + def restore_state(self, state): + old_q, old_dq, old_u = state + self.articulation.set_joint_positions(old_q) + self.articulation.set_joint_velocities(old_dq) + self.articulation.set_applied_joint_efforts(old_u) + + def _get_prim_path(self, name): + for prim in self.stage.Traverse(): + # Check if the prim path ends with the desired name + if str(prim.GetPath()).endswith(name): + return prim.GetPath() + return None + diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 0fed5c7b..4a471d89 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -20,7 +20,7 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.robot_config = robot_config self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called - + self.ee_name = "end_effector" # EE #self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles #self.initial_q = np.zeros(self.robot_config.N_JOINTS) # joint angles #self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities @@ -55,6 +55,7 @@ def connect(self, joint_names=None): import omni.isaac.core.utils.stage as stage_utils # type: ignore from isaacsim.core.api.robots import Robot from pxr import UsdLux, Sdf, Gf, UsdPhysics + # Create a world self.world = World(physics_dt=self.dt,rendering_dt=self.dt) self.context = omni.usd.get_context() @@ -73,7 +74,29 @@ def connect(self, joint_names=None): # add lighting distantLight = UsdLux.DistantLight.Define(self.stage, Sdf.Path("/DistantLight")) distantLight.CreateIntensityAttr(500) - + ''' + # setting up import configuration: + status, import_config = omni.kit.commands.execute("MJCFCreateImportConfig") + import_config.set_fix_base(True) # fix the base of the robot + import_config.set_make_default_prim(False) + + # Get path to extension data: + ext_manager = omni.kit.app.get_app().get_extension_manager() + ext_id = ext_manager.get_enabled_extension_id("isaacsim.asset.importer.mjcf") + extension_path = ext_manager.get_extension_path(ext_id) + # import MJCF + omni.kit.commands.execute( + "MJCFCreateAsset", + #mjcf_path=extension_path + "/data/mjcf/nv_ant.xml", + mjcf_path=extension_path + "/data/mjcf/nv_humanoid.xml", + #mjcf_path=self.robot_config.xml_file, + import_config=import_config, + prim_path=self.prim_path + ) + + + + ''' ## LOAD Jaco2 robot assets_root_path = get_assets_root_path() stage_utils.add_reference_to_stage( @@ -108,15 +131,19 @@ def connect(self, joint_names=None): self.articulation_view = ArticulationView(prim_paths_expr=self.prim_path, name=self.name + "_view") self.world.scene.add(self.articulation_view) self.articulation_view.initialize() + self.world.reset() # necessary so self.q and self.dq are accessible self.world.initialize_physics() + + self.world.step(render=False) self.joint_pos_addrs = [] self.joint_vel_addrs = [] self.joint_dyn_addrs = [] + print("Connecting to robot config...") self.robot_config._connect( self.world, @@ -134,6 +161,33 @@ def disconnect(self): self.simulation_app.close() # close Isaac Sim print("IsaacSim connection closed...") + #TODO adapt to IsaacSim + def get_joint_pos_addrs(self, jntadr): + # store the data.qpos indices associated with this joint + first_pos = self.model.jnt_qposadr[jntadr] + posvec_length = self.robot_config.JNT_POS_LENGTH[self.model.jnt_type[jntadr]] + joint_pos_addr = list(range(first_pos, first_pos + posvec_length))[::-1] + return joint_pos_addr + + + def get_joint_vel_addrs(self, joint_name): + if self.articulation_view is None: + raise RuntimeError("Robot ArticulationView not set up.") + + #dof_indices = self.articulation_view.get_dof_indices(joint_name) + #print('dof_indices:', dof_indices) + # Get all DOF names in the articulation + dof_names = self.articulation_view.dof_names + dof_name_to_index = {name: i for i, name in enumerate(dof_names)} + + if joint_name.endswith(self.ee_name): + index = None + else: + index = dof_name_to_index[joint_name] + + return index + + def send_forces(self, u): """Applies the set of torques u to the arm. If interfacing to @@ -158,17 +212,22 @@ def send_target_angles(self, q): - def get_feedback(self, arm_only=True): + def get_feedback(self): """Return a dictionary of information needed by the controller. Returns the joint angles and joint velocities in [rad] and [rad/sec], respectively """ # Get the joint angles and velocities + self.q = self.articulation.get_joint_positions() + #self.q = q[:self.robot_config.N_JOINTS] # only take the first N_JOINTS self.dq = self.articulation.get_joint_velocities() + #self.dq = dq[:self.robot_config.N_JOINTS] # only take the first N_JOINTS return {"q": self.q, "dq": self.dq} + + def get_xyz(self, name): """Returns the xyz position of the specified object @@ -181,7 +240,7 @@ def get_xyz(self, name): return object_position - + #TODO check if overlap to def quaternion def get_orientation(self, name): """Returns the orientation of an object in CoppeliaSim @@ -197,8 +256,33 @@ def get_orientation(self, name): obj = self.world.scene.get_object(name) object_position, object_orientation = obj.get_world_pose() return object_orientation + + #TODO change method name in 'set_named_prim' or something as mocap is mujoco thing + def set_mocap_xyz(self, name, xyz): + """ + Set the world position of a named prim (used like a mocap target). + Parameters + ---------- + name : str + Name of the prim (e.g. site or target object) + xyz : np.ndarray + Target world position [x, y, z] in meters + """ + # Assume prim is under robot or scene root + #prim_path = f"{self.prim_path}/{name}" + world_path = "/World" + prim_path = f"{world_path}/{name}" + + print("prim_path:", prim_path) + #from omni.isaac.core.utils.prims import set_prim_world_position + #set_prim_world_position(prim_path, xyz) + + + + + #TODO remove as is the same as above def set_xyz(self, name, xyz): """Set the position of an object in the environment. @@ -220,4 +304,18 @@ def send_actions(self, dt): 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 ,0.02)) #prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(0.70711 ,0.70711 ,0.0 ,0.0)) + + + + #TODO use for R and Tx in isaacsim_config + def get_prim_ends_with_name(self, name): + prim_path = None + for prim in self.stage.Traverse(): + print("prim: ", prim) + print("prim.GetPath(): ", prim.GetPath()) + print("name: ", name) + if str(prim.GetPath()).endswith(name): + prim_path = prim.GetPath() + break + return prim_path \ No newline at end of file diff --git a/examples/IsaacSim/flying_cube.py b/examples/IsaacSim/flying_cube.py new file mode 100644 index 00000000..2c484008 --- /dev/null +++ b/examples/IsaacSim/flying_cube.py @@ -0,0 +1,64 @@ +import numpy as np + +class FlyingCube(): + + def __init__(self, dt, world, has_collision = True, prim_path="/World/random_cube", name="fancy_cube", position=np.array([0, 0, 1.0]), scale=np.array([0.1015, 0.1015, 0.1015]), color=np.array([0, 0, 1.0])): + # IsaacSim imports must be done after instantiating the SimulationApp, which is done by the setup in the isaacsim interface + from omni.isaac.core.objects import DynamicCuboid # type: ignore + from omni.isaac.dynamic_control import _dynamic_control # type: ignore + + self.world = world + if has_collision: + fancy_cube = self.world.scene.add( + DynamicCuboid( + prim_path=prim_path, + name=name, + position=position, + scale=scale, + color=color, + ) + ) + # Set the cube to be dynamic using dynamic_control + self._dc = _dynamic_control.acquire_dynamic_control_interface() + self.cube_handle = self._dc.get_rigid_body(prim_path) + self._cube = self.world.scene.get_object(name) + + # Add velocity as an attribute of the world so that it is available at callback time + self.input_velocity = np.zeros(3) + + self.world.add_physics_callback(name + "_send_actions", self.send_actions) + self.world.add_physics_callback(name + "_make_state", self.make_state) + else: + prim_path = "/World/VisualOnlyCube" + prim_type = "Cube" + # Create a basic cube prim + + + + + + def send_actions(self, dt): + self._dc.set_rigid_body_linear_velocity(self.cube_handle, self.input_velocity) + + def make_state(self, dt): + position, orientation = self._cube.get_world_pose() + linear_velocity = self._cube.get_linear_velocity() + return { + 'position': np.array([[0., 0., 1.]]), + 'velocity': np.array([[0., 0., 0.]]), + 'orientation': np.array([[0., 0., 0., 0.]]) + } + + def step(self, velocity): + # Update the input velocity + self.input_velocity = velocity + self.world.step() + + # Get the cube's current state + position, orientation = self._cube.get_world_pose() + linear_velocity = self._cube.get_linear_velocity() + return np.concatenate([position, linear_velocity, orientation], axis=0) + + def _get_obj_pos(self): + position, orientation = self._cube.get_world_pose() + return position \ No newline at end of file diff --git a/examples/IsaacSim/position_joint_control_inverse_kinematics.py b/examples/IsaacSim/position_joint_control_inverse_kinematics.py new file mode 100644 index 00000000..f7422cac --- /dev/null +++ b/examples/IsaacSim/position_joint_control_inverse_kinematics.py @@ -0,0 +1,140 @@ +""" +Running the joint controller with an inverse kinematics path planner +for a Mujoco simulation. The path planning system will generate +a trajectory in joint space that moves the end effector in a straight line +to the target, which changes every n time steps. +""" +import numpy as np + +#from abr_control.arms.mujoco_config import MujocoConfig as arm +from abr_control.arms.isaacsim_config import IsaacsimConfig as arm +from abr_control.controllers import path_planners +#from abr_control.interfaces.mujoco import Mujoco +from abr_control.interfaces.nv_isaacsim import IsaacSim +from abr_control.utils import transformations +from flying_cube import FlyingCube + + +# initialize our robot config for the jaco2 +robot_config = arm("ur5", use_sim_state=False) + +# create our path planner +n_timesteps = 2000 +path_planner = path_planners.InverseKinematics(robot_config) + +# create our interface +dt = 0.001 +interface = IsaacSim(robot_config, dt=dt) +interface.connect() +interface.send_target_angles(robot_config.START_ANGLES) +feedback = interface.get_feedback() +isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") +ee_name = "end_effector" # EE + + + + + + +print("\nSimulation starting...") +print("Click to move the target.\n") + +count = 0 +while 1: + + if count % n_timesteps == 0: + feedback = interface.get_feedback() + target_xyz = np.array( + [ + np.random.random() * 0.5 - 0.25, + np.random.random() * 0.5 - 0.25, + np.random.random() * 0.5 + 0.5, + ] + ) + R = robot_config.R(ee_name, q=feedback["q"]) + target_orientation = transformations.euler_from_matrix(R, "sxyz") + # update the position of the target + #interface.set_mocap_xyz("target", target_xyz) + interface.set_xyz("target", target_xyz) + + # can use 3 different methods to calculate inverse kinematics + # see inverse_kinematics.py file for details + print('target pos: ', target_xyz) + print('target_orientation: ', target_orientation) + path_planner.generate_path( + position=feedback["q"], + target_position=np.hstack([target_xyz, target_orientation]), + method=3, + dt=0.005, + n_timesteps=n_timesteps, + plot=False, + ) + + # returns desired [position, velocity] + target = path_planner.next()[0] + + # use position control + interface.send_target_angles(target[: robot_config.N_JOINTS]) + interface.viewer.render() + + count += 1 + + + + + + + + + + + + + + +try: + print("\nSimulation starting...") + print("Click to move the target.\n") + + count = 0 + while 1: + + if count % n_timesteps == 0: + feedback = interface.get_feedback() + target_xyz = np.array( + [ + np.random.random() * 0.5 - 0.25, + np.random.random() * 0.5 - 0.25, + np.random.random() * 0.5 + 0.5, + ] + ) + R = robot_config.R(ee_name, q=feedback["q"]) + target_orientation = transformations.euler_from_matrix(R, "sxyz") + # update the position of the target + interface.set_mocap_xyz("target", target_xyz) + + # can use 3 different methods to calculate inverse kinematics + # see inverse_kinematics.py file for details + path_planner.generate_path( + position=feedback["q"], + target_position=np.hstack([target_xyz, target_orientation]), + method=3, + dt=0.005, + n_timesteps=n_timesteps, + plot=False, + ) + + # returns desired [position, velocity] + target = path_planner.next()[0] + + # use position control + interface.send_target_angles(target[: robot_config.N_JOINTS]) + interface.viewer.render() + + count += 1 + +finally: + # stop and reset the simulation + interface.disconnect() + + print("Simulation terminated...") \ No newline at end of file From adfa03d4682b5def3d317944cff8f5680405901e Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 24 Jun 2025 10:58:19 -0400 Subject: [PATCH 17/42] floating control now works for ur5 and jaco2 --- abr_control/arms/isaacsim_config.py | 228 ++++++++++++++++++-------- abr_control/interfaces/nv_isaacsim.py | 86 ++++++++-- 2 files changed, 236 insertions(+), 78 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index a5d011bd..f5656ac7 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -123,9 +123,11 @@ def __init__(self, xml_file, folder=None, use_sim_state=True, force_download=Fal files=files, ) + #TODO fix above to get away from xml + self.robot = xml_file self.use_sim_state = use_sim_state - def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path): + def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path, ee_name): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -151,41 +153,43 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.joint_pos_addrs = np.copy(joint_pos_addrs) self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path + self.ee_name = ee_name # number of controllable joints in the robot arm #TODO: this is a hack for the jaco2 robot, which has 12 joints #self.N_JOINTS = len(self.joint_vel_addrs) - self.N_JOINTS = self.articulation_view.num_dof + #self.N_JOINTS = self.articulation_view.num_dof + self.N_JOINTS = 6 print (f"Number of controllable joints: {self.N_JOINTS}") # number of joints in the IsaacSim simulation - N_ALL_JOINTS = self.articulation_view.num_dof - print (f"Number of ALL joints in simulation: {N_ALL_JOINTS}") + self.N_ALL_JOINTS = self.articulation_view.num_dof + print (f"Number of ALL joints in simulation: {self.N_ALL_JOINTS}") # need to calculate the joint_vel_addrs indices in flat vectors returned # for the Jacobian self.jac_indices = np.hstack( # 6 because position and rotation Jacobians are 3 x N_JOINTS - [self.joint_vel_addrs + (ii * N_ALL_JOINTS) for ii in range(3)] + [self.joint_vel_addrs + (ii * self.N_ALL_JOINTS) for ii in range(3)] ) # for the inertia matrix self.M_indices = [ - ii * N_ALL_JOINTS + jj + ii * self.N_ALL_JOINTS + jj for jj in self.joint_vel_addrs for ii in self.joint_vel_addrs ] # a place to store data returned from Mujoco self._g = np.zeros(self.N_JOINTS) - self._J3NP = np.zeros((3, N_ALL_JOINTS)) - self._J3NR = np.zeros((3, N_ALL_JOINTS)) + self._J3NP = np.zeros((3, self.N_ALL_JOINTS)) + self._J3NR = np.zeros((3, self.N_ALL_JOINTS)) self._J6N = np.zeros((6, self.N_JOINTS)) - self._MNN = np.zeros((N_ALL_JOINTS, N_ALL_JOINTS)) + self._MNN = np.zeros((self.N_ALL_JOINTS, self.N_ALL_JOINTS)) self._R9 = np.zeros(9) self._R = np.zeros((3, 3)) self._x = np.ones(4) - self.N_ALL_JOINTS = N_ALL_JOINTS + self.N_ALL_JOINTS = self.N_ALL_JOINTS def _load_state(self, q, dq=None, u=None): """Change the current joint angles @@ -238,7 +242,7 @@ def g(self, q=None): self.articulation_view.set_joint_positions(q) self.articulation_view.set_joint_velocities(np.zeros_like(q)) - self.world.step(render=False) + ### self.world.step(render=False) # Compute gravity and Coriolis/centrifugal separately gravity = self.articulation_view.get_generalized_gravity_forces() @@ -253,7 +257,7 @@ def g(self, q=None): # Restore old state self.articulation_view.set_joint_positions(old_q) self.articulation_view.set_joint_velocities(old_dq) - self.world.step(render=False) + ### self.world.step(render=False) return -g # match MuJoCo's negative qfrc_bias convention @@ -280,63 +284,145 @@ def dJ(self, name, q=None, dq=None, x=None): # general case, check differences.cpp' raise NotImplementedError - def J(self, name, q=None, x=None, object_type="body"): - """Returns the Jacobian for the specified link, - computed at the origin of the link's rigid body frame, - which in Isaac Sim coincides with the link’s center of mass. - + def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): + """Returns the Jacobian for the specified link, + computed at the origin of the link's rigid body frame, + which in Isaac Sim coincides with the link's center of mass. + Parameters ---------- name: string - The name of the Mujoco body to retrieve the Jacobian for + The name of the link/body to retrieve the Jacobian for q: float numpy.array, optional (Default: None) The joint angles of the robot. If None the current state is - retrieved from the Mujoco simulator + retrieved from the Isaac Sim simulator x: float numpy.array, optional (Default: None) - object_type: string, the Mujoco object type, optional (Default: body) - options: body, geom, site + Offset from link origin (currently not supported) + object_type: string, optional (Default: "body") + The object type - options: body, geom, site + controlled_dofs: list, optional (Default: None) + List of DOF indices to include in Jacobian. If None, uses first 6 DOFs + + Returns + ------- + numpy.array + 6xN Jacobian matrix where N is number of controlled DOFs + First 3 rows: linear velocity Jacobian (Jv) + Last 3 rows: angular velocity Jacobian (Jω) """ - #TODO outsource this to a common function and check is can be qued for EE and - # end_effector at the same time, or checked which is used for the current robot - if name == "EE": name = "end_effector" - + + # Handle special case mappings + if name == "EE": + name = self.ee_name + + # Check for unsupported features if x is not None and not np.allclose(x, 0): raise Exception("x offset currently not supported, set to None") - - if not self.use_sim_state and q is not None: - old_q, old_dq, old_u = self._load_state(q) - + + # Set controlled DOFs (default to first 6 for arm control) + if controlled_dofs is not None: + self.controlled_dof_indices = controlled_dofs + elif not hasattr(self, 'controlled_dof_indices'): + self.controlled_dof_indices = list(range(6)) # Default: first 6 DOFs + + ''' + # Handle joint state setting if provided + if q is not None: + # Store current state if we need to restore it + old_positions = self.articulation_view.get_joint_positions(clone=True) + # Set new joint positions + self.articulation_view.set_joint_positions(q.reshape(1, -1)) + # Forward the simulation to update Jacobians + # You might need to call a simulation step here depending on your setup + ''' + if object_type == "body": - try: - #TODO maybe outsource if also needed elsewhere - link_list = self.articulation_view.body_names - prefix = link_list[0].split('_')[0] # Get the prefix from the first link name - full_name = f"{prefix}_{name}" # Construct the full name - link_index = link_list.index(full_name) - except ValueError: - raise RuntimeError(f"Link '{name}' not found in articulation.") - jacobians = self.articulation_view.get_jacobians(clone=True) # shape: (num_links, 6, num_dofs) - J = jacobians[0, link_index, :, :] + # Get Jacobians from articulation view + jacobians = self.articulation_view.get_jacobians(clone=True) + + # Check if ArticulationView has any environments + if jacobians.shape[0] == 0: + raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") + + # Get link index + link_index = self._get_link_index(name) + + # Extract Jacobian for specific link + env_idx = 0 # Assuming single environment + + # Handle different Jacobian tensor shapes + if len(jacobians.shape) == 4: + # Shape: (num_envs, num_bodies, 6, num_dofs) + J_full = jacobians[env_idx, link_index, :, :] + elif len(jacobians.shape) == 3: + # Shape: (num_envs, num_bodies * 6, num_dofs) + start_row = link_index * 6 + end_row = start_row + 6 + J_full = jacobians[env_idx, start_row:end_row, :] + else: + raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") + + # Extract only the columns for controllable DOFs + J = J_full[:, self.controlled_dof_indices] + + # Verify dimensions + if J.shape[0] != 6: + raise RuntimeError(f"Expected Jacobian to have 6 rows, got {J.shape[0]}") + if J.shape[1] != len(self.controlled_dof_indices): + raise RuntimeError(f"Expected Jacobian to have {len(self.controlled_dof_indices)} columns, got {J.shape[1]}") + + # Assign to internal storage + # Linear velocity Jacobian (first 3 rows) + self._J6N[:3, :] = J[:3, :] + # Angular velocity Jacobian (last 3 rows) + self._J6N[3:, :] = J[3:, :] + + elif object_type == "geom": + raise NotImplementedError("Calculating the Jacobian for 'geom' is not yet implemented.") + elif object_type == "site": + raise NotImplementedError("Calculating the Jacobian for 'site' is not yet implemented.") + else: + raise ValueError(f"Invalid object type specified: {object_type}") + + return np.copy(self._J6N) - else: - if object_type == "geom": - raise Exception("Calculating the Jacobian is not yet implemented for 'geom'.") - elif object_type == "site": - raise Exception("Calculating the Jacobian is not yet implemented for 'site'.") - else: - raise Exception("Invalid object type specified: ", object_type) - # get the position Jacobian (linear velocity Jacobian (Jv)) - self._J6N[:3] = J[:3, :] - # get the rotation Jacobian (angular velocity Jacobian (Jω)) - self._J6N[3:] = J[3:, :] + def _get_link_index(self, name): + """Helper function to get link index from name.""" + try: + # Method 1: Direct lookup if body_names includes simple names + if hasattr(self.articulation_view, 'body_names'): + link_list = self.articulation_view.body_names + #print("link_list: ", link_list) + + # Try direct name first + if name in link_list: + return link_list.index(name) + + # Try with prefix (your original approach) + if link_list and '_' in link_list[0]: + prefix = link_list[0].split('_')[0] + full_name = f"{prefix}_{name}" + if full_name in link_list: + return link_list.index(full_name) + + # Method 2: Use articulation API if available + if hasattr(self.articulation, 'get_body_index'): + return self.articulation.get_body_index(name) + + # Method 3: Manual search through body names + for i, body_name in enumerate(link_list): + if body_name.endswith(name) or name in body_name: + return i + + raise ValueError(f"Link '{name}' not found in articulation.") + + except Exception as e: + raise RuntimeError(f"Error finding link '{name}': {str(e)}") - if not self.use_sim_state and q is not None: - self._load_state(old_q, old_dq, old_u) - return np.copy(self._J6N) def M(self, q=None): """ @@ -357,7 +443,7 @@ def M(self, q=None): # Save current joint state old_q = self.articulation_view.get_joint_positions() self.articulation_view.set_joint_positions(q) - self.world.step(render=False) # required to update PhysX buffers + ### self.world.step(render=False) # required to update PhysX buffers # Get mass matrix M = self.articulation_view.get_mass_matrices() @@ -366,7 +452,7 @@ def M(self, q=None): if not self.use_sim_state and q is not None: # Restore previous state self.articulation_view.set_joint_positions(old_q) - self.world.step(render=False) + ### self.world.step(render=False) return M @@ -383,14 +469,16 @@ def R(self, name, q=None, object_type="body"): object_type : str One of "body", "geom", or "site". """ - #print(f"R: {name}, q: {q}, object_type: {object_type}") + ''' + + print(f"R: {name}, q: {q}, object_type: {object_type}") if not self.use_sim_state and q is not None: old_q = self.articulation_view.get_joint_positions() self.articulation_view.set_joint_positions(q) #self._world.step(render=False) + ''' if object_type == "body": - print(f"Finding body with name: {name}") # Look for a link prim ending in the given name prim_path = self._get_prim_path(name) elif object_type in ["site", "geom"]: @@ -412,9 +500,12 @@ def R(self, name, q=None, object_type="body"): [matrix[1][0], matrix[1][1], matrix[1][2]], [matrix[2][0], matrix[2][1], matrix[2][2]] ]) + + ''' if not self.use_sim_state and q is not None: self.articulation_view.set_joint_positions(old_q) #self._world.step(render=False) + ''' return R @@ -433,10 +524,11 @@ def quaternion(self, name, q=None): """ #TODO outsource this to a common function and check is can be qued for EE and # end_effector at the same time, or checked which is used for the current robot - if name == "EE": name = "end_effector" + if name == "EE": name = self.ee_name + ''' if not self.use_sim_state and q is not None: old_q, old_dq, old_u = self._load_state(q) - + ''' prim_path = self._get_prim_path(name) prim = self.stage.GetPrimAtPath(prim_path) @@ -449,10 +541,10 @@ def quaternion(self, name, q=None): # Convert to [w, x, y, z] NumPy array quat_np = np.array([quat.GetReal(), *quat.GetImaginary()]) - + ''' if not self.use_sim_state and q is not None: self._load_state(old_q, old_dq, old_u) - + ''' return quat_np def C(self, q=None, dq=None): @@ -499,7 +591,8 @@ def Tx(self, name, q=None, x=None, object_type="body"): np.ndarray World position [x, y, z] of the object. """ - if name == "EE": name = "end_effector" + if name == "EE": name = self.ee_name + ''' if x is not None and not np.allclose(x, 0): raise Exception("x offset currently not supported: ", x) @@ -507,8 +600,8 @@ def Tx(self, name, q=None, x=None, object_type="body"): if not self.use_sim_state and q is not None: old_q = self.articulation_view.get_joint_positions() self.articulation_view.set_joint_positions(q) - self.world.step(render=False) - + ### self.world.step(render=False) + ''' prim_path = None #print(f"Tx: name={name}, q={q}, object_type={object_type}") @@ -539,12 +632,12 @@ def Tx(self, name, q=None, x=None, object_type="body"): matrix = omni.usd.utils.get_world_transform_matrix(prim) position = matrix.ExtractTranslation() - + ''' # Restore state if needed if not self.use_sim_state and q is not None: self.articulation_view.set_joint_positions(old_q) - self.world.step(render=False) - + ### self.world.step(render=False) + ''' Tx =np.array([position[0], position[1], position[2]]) return Tx @@ -584,6 +677,7 @@ def restore_state(self, state): def _get_prim_path(self, name): for prim in self.stage.Traverse(): # Check if the prim path ends with the desired name + #print(f"Checking prim: {prim.GetPath()}") if str(prim.GetPath()).endswith(name): return prim.GetPath() return None diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 4a471d89..b6404aba 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -20,7 +20,10 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.robot_config = robot_config self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called - self.ee_name = "end_effector" # EE + + # ee_name = "ft_frame" for ur2 + # #"end_effector" for jaco2 + #self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles #self.initial_q = np.zeros(self.robot_config.N_JOINTS) # joint angles #self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities @@ -93,10 +96,40 @@ def connect(self, joint_names=None): import_config=import_config, prim_path=self.prim_path ) + ''' + + + if self.robot_config.robot == "ur5": + robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd" + self.ee_name = "flange" #"ft_frame" # end-effector name for UR5 + elif self.robot_config.robot == "jaco2": + robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" + self.ee_name = "end_effector" # end-effector name for Jaco2 + elif self.robot_config.robot == "h1": + robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" + #TODO check this + self.ee_name = "EE" # end-effector name for H1 + + assets_root_path = get_assets_root_path() + stage_utils.add_reference_to_stage( + usd_path=assets_root_path + robot_path, + prim_path=self.prim_path, + ) + robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) ''' + ## LOAD UR5 robot + assets_root_path = get_assets_root_path() + stage_utils.add_reference_to_stage( + usd_path=assets_root_path + "/Isaac/Robots/UniversalRobots/ur5/ur5.usd", + # Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd --> 7 DOF arm , not compatible with ABR controller + prim_path=self.prim_path, + ) + robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) + + ## LOAD Jaco2 robot assets_root_path = get_assets_root_path() stage_utils.add_reference_to_stage( @@ -106,7 +139,7 @@ def connect(self, joint_names=None): ) robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - ''' + # Load H1 robot self.h1 = H1FlatTerrainPolicy( prim_path=self.prim_path, @@ -152,7 +185,8 @@ def connect(self, joint_names=None): self.articulation_view, self.joint_pos_addrs, self.joint_vel_addrs, - self.prim_path + self.prim_path, + self.ee_name ) @@ -208,22 +242,48 @@ def send_target_angles(self, q): q : numpy.array the target joint angles [radians] + + print("q: ", q) + print("robot joint pos: ", self.articulation.get_joint_positions()) + q_all = self.get_feedback()["q"] + print("q_all: ", q_all) + #TODO change that to variable number of joints + #q_all = q_all[:self.robot_config.N_JOINTS] # + q_all[:6] = q + print("result: ", q_all) + self.articulation_view.set_joint_positions(q_all) """ + print("len(q) : ", len(q)) + print("robot_config.N_JOINTS: ", self.robot_config.N_JOINTS) + # Check if the length of q is greater than the number of joints + if len(q) > self.robot_config.N_JOINTS: + q_new = q[:self.robot_config.N_JOINTS] + self.articulation_view.set_joint_positions(q_new) + elif self.robot_config.N_ALL_JOINTS > self.robot_config.N_JOINTS: + q_new = self.articulation_view.get_joint_positions() # Shape: (1, 12) + q_new[0, :self.robot_config.N_JOINTS] = q # Update first N_JOINTS for environment 0 + self.articulation_view.set_joint_positions(q_new) + else: + self.articulation_view.set_joint_positions(q) + - def get_feedback(self): + def get_feedback(self, all_joints=False): """Return a dictionary of information needed by the controller. Returns the joint angles and joint velocities in [rad] and [rad/sec], respectively """ - # Get the joint angles and velocities + if not all_joints: + # Get the joint angles and velocities + self.q = self.articulation.get_joint_positions()[:self.robot_config.N_JOINTS] # only take the first N_JOINTS + self.dq = self.articulation.get_joint_velocities()[:self.robot_config.N_JOINTS] + else: + # Get the joint angles and velocities for all joints + self.q = self.articulation.get_joint_positions() + self.dq = self.articulation.get_joint_velocities() - self.q = self.articulation.get_joint_positions() - #self.q = q[:self.robot_config.N_JOINTS] # only take the first N_JOINTS - self.dq = self.articulation.get_joint_velocities() - #self.dq = dq[:self.robot_config.N_JOINTS] # only take the first N_JOINTS return {"q": self.q, "dq": self.dq} @@ -270,10 +330,14 @@ def set_mocap_xyz(self, name, xyz): xyz : np.ndarray Target world position [x, y, z] in meters """ - # Assume prim is under robot or scene root - #prim_path = f"{self.prim_path}/{name}" + world_path = "/World" prim_path = f"{world_path}/{name}" + print("prim_path:", prim_path) + prim = self.stage.GetPrimAtPath(prim_path) + + prim.set_world_pose(xyz, np.array([0., 0., 0., 1.])) # set the position and orientation of the object + print("prim_path:", prim_path) #from omni.isaac.core.utils.prims import set_prim_world_position From 9e412b88e3241445013d0a9bc64fd7b64c73b123 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 24 Jun 2025 14:59:13 -0400 Subject: [PATCH 18/42] adapted tu num of controlled DOF --- abr_control/arms/isaacsim_config.py | 89 ++++++++++++++++++++++++--- abr_control/interfaces/nv_isaacsim.py | 36 +++++++++-- 2 files changed, 113 insertions(+), 12 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index f5656ac7..d96b78a7 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -127,7 +127,7 @@ def __init__(self, xml_file, folder=None, use_sim_state=True, force_download=Fal self.robot = xml_file self.use_sim_state = use_sim_state - def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path, ee_name): + def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path, ee_name, joint_names): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -160,7 +160,9 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr #TODO: this is a hack for the jaco2 robot, which has 12 joints #self.N_JOINTS = len(self.joint_vel_addrs) #self.N_JOINTS = self.articulation_view.num_dof - self.N_JOINTS = 6 + print("len(joint_names): ", len(joint_names)) + #TODO always only considers the first N joints + self.N_JOINTS = len(joint_names) print (f"Number of controllable joints: {self.N_JOINTS}") # number of joints in the IsaacSim simulation self.N_ALL_JOINTS = self.articulation_view.num_dof @@ -224,7 +226,46 @@ def _load_state(self, q, dq=None, u=None): return old_q, old_dq, old_u + + def g(self, q=None): + """ + Returns the joint-space forces due to gravity, Coriolis, and centrifugal effects + in Isaac Sim (equivalent to MuJoCo's qfrc_bias). + Parameters + ---------- + q: np.ndarray, optional (Default: None) + Joint positions to compute the bias forces at. If None, uses current sim state. + """ + # Compute gravity and Coriolis/centrifugal separately + gravity = self.articulation_view.get_generalized_gravity_forces() + coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces() + + print(f"Raw gravity shape: {gravity.shape}") + print(f"Raw coriolis shape: {coriolis.shape}") + + # Total generalized bias forces + g_full = gravity + coriolis + print(f"Combined g_full shape: {g_full.shape}") + + # Handle batch dimension if present + if g_full.ndim == 2: + print(f"Detected batch dimension, original shape: {g_full.shape}") + if g_full.shape[0] == 1: + g_full = g_full[0] # Remove batch dimension + print(f"After removing batch dimension: {g_full.shape}") + + if q is not None: + print(f"q shape: {q.shape}") + # If q is provided, ensure g matches the size of q + if len(g_full) != len(q): + print(f"Truncating g from {len(g_full)} to {len(q)} elements") + g_full = g_full[:len(q)] + + print(f"Final g shape: {g_full.shape}") + return -g_full + ''' + def g(self, q=None): """ Returns the joint-space forces due to gravity, Coriolis, and centrifugal effects @@ -260,8 +301,7 @@ def g(self, q=None): ### self.world.step(render=False) return -g # match MuJoCo's negative qfrc_bias convention - - + ''' def dJ(self, name, q=None, dq=None, x=None): """Returns the derivative of the Jacobian wrt to time @@ -438,21 +478,27 @@ def M(self, q=None): np.ndarray Dense inertia matrix (DoF x DoF) """ - + ''' + if not self.use_sim_state and q is not None: # Save current joint state old_q = self.articulation_view.get_joint_positions() self.articulation_view.set_joint_positions(q) ### self.world.step(render=False) # required to update PhysX buffers - + ''' + # Get mass matrix M = self.articulation_view.get_mass_matrices() - #print(f"Mass matrix M: {M}") + if q is not None: + M = M[0, :len(q), :len(q)] # Ensure M is square and matches q size + + ''' if not self.use_sim_state and q is not None: # Restore previous state self.articulation_view.set_joint_positions(old_q) ### self.world.step(render=False) + ''' return M @@ -574,7 +620,34 @@ def T(self, name, q=None, x=None): # TODO if ever required raise NotImplementedError - def Tx(self, name, q=None, x=None, object_type="body"): + def Tx(self, name, q=None, object_type="body"): + """Simplified version that only gets current position without state changes.""" + #TODO handle q + if name == "EE": + name = self.ee_name + + # Get prim path + if object_type in ["body", "link"]: + prim_path = self._get_prim_path(name) + elif object_type == "joint": + # For joints, you might want the parent link position + prim_path = self._get_prim_path(name) + else: + raise ValueError(f"Unsupported object_type: {object_type}") + + # Get world position + prim = self.stage.GetPrimAtPath(prim_path) + if not prim.IsValid(): + raise RuntimeError(f"Invalid prim at path: {prim_path}") + + 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 Tx_old(self, name, q=None, x=None, object_type="body"): """ Returns the world-frame Cartesian position of a named link, joint, or site. Parameters diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index b6404aba..693bfcf0 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -186,7 +186,8 @@ def connect(self, joint_names=None): self.joint_pos_addrs, self.joint_vel_addrs, self.prim_path, - self.ee_name + self.ee_name, + joint_names ) @@ -220,9 +221,24 @@ def get_joint_vel_addrs(self, joint_name): index = dof_name_to_index[joint_name] return index + ''' + def send_forces(self, u): + """Applies the set of torques u to the arm.""" - - + # Create full torque vector for all DOFs + total_dofs = self.articulation_view.num_dof + full_torques = np.zeros(total_dofs) + + # Apply control torques to the controlled joints + full_torques[:len(u)] = 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_forces(self, u): """Applies the set of torques u to the arm. If interfacing to a simulation, also moves dynamics forward one time step. @@ -235,7 +251,7 @@ def send_forces(self, u): # move simulation ahead one time step self.world.step(render=True) # execute one physics step and one rendering step - + def send_target_angles(self, q): """Moves the arm to the specified joint angles @@ -256,6 +272,17 @@ def send_target_angles(self, q): print("len(q) : ", len(q)) print("robot_config.N_JOINTS: ", self.robot_config.N_JOINTS) # Check if the length of q is greater than the number of joints + if len(q) > self.robot_config.N_JOINTS: + q_new = q[:self.robot_config.N_JOINTS] + self.articulation_view.set_joint_positions(q_new) + elif len(q) < self.robot_config.N_ALL_JOINTS : + q_new = self.articulation_view.get_joint_positions() # Shape: (1, 12) + q_new[0, :len(q)] = q # Update first N_JOINTS for environment 0 + self.articulation_view.set_joint_positions(q_new) + else: + self.articulation_view.set_joint_positions(q) + + ''' if len(q) > self.robot_config.N_JOINTS: q_new = q[:self.robot_config.N_JOINTS] self.articulation_view.set_joint_positions(q_new) @@ -265,6 +292,7 @@ def send_target_angles(self, q): self.articulation_view.set_joint_positions(q_new) else: self.articulation_view.set_joint_positions(q) + ''' From bc7a01eee2c387074ad100d254a596c2e66d997a Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 24 Jun 2025 15:01:21 -0400 Subject: [PATCH 19/42] generalized send u --- abr_control/arms/isaacsim_config.py | 109 +---------- abr_control/interfaces/nv_isaacsim.py | 62 ++---- .../interfaces/nv_isaacsim_usd_jaco.py | 182 ------------------ examples/IsaacSim/force_floating_control.py | 15 +- 4 files changed, 39 insertions(+), 329 deletions(-) delete mode 100644 abr_control/interfaces/nv_isaacsim_usd_jaco.py diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index d96b78a7..296a9e0c 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -127,7 +127,7 @@ def __init__(self, xml_file, folder=None, use_sim_state=True, force_download=Fal self.robot = xml_file self.use_sim_state = use_sim_state - def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path, ee_name, joint_names): + def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path, ee_link_name): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -153,16 +153,10 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.joint_pos_addrs = np.copy(joint_pos_addrs) self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path - self.ee_name = ee_name + self.ee_link_name = ee_link_name - # number of controllable joints in the robot arm - #TODO: this is a hack for the jaco2 robot, which has 12 joints - #self.N_JOINTS = len(self.joint_vel_addrs) - #self.N_JOINTS = self.articulation_view.num_dof - print("len(joint_names): ", len(joint_names)) - #TODO always only considers the first N joints - self.N_JOINTS = len(joint_names) + self.N_JOINTS = len(self.joint_vel_addrs) print (f"Number of controllable joints: {self.N_JOINTS}") # number of joints in the IsaacSim simulation self.N_ALL_JOINTS = self.articulation_view.num_dof @@ -193,37 +187,6 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self._x = np.ones(4) self.N_ALL_JOINTS = self.N_ALL_JOINTS - def _load_state(self, q, dq=None, u=None): - """Change the current joint angles - - Parameters - ---------- - q: np.array - The set of joint angles to move the arm to [rad] - dq: np.array - The set of joint velocities to move the arm to [rad/sec] - u: np.array - The set of joint forces to apply to the arm joints [Nm] - """ - - old_q = np.copy(self.articulation.get_joint_positions()) - old_dq = np.copy(self.articulation.get_joint_velocities()) - old_u = np.copy(self.articulation.get_applied_joint_efforts()) - - # update positions to specified state - self.articulation.set_joint_positions(q) # set the joint positions in the articulation view - - if dq is not None: - #self.data.qvel[self.joint_vel_addrs] = np.copy(dq) - self.articulation_view.set_joint_velocities(dq) - if u is not None: - #self.data.ctrl[:] = np.copy(u) - self.articulation_view.set_joint_efforts(u) - - # move simulation forward to calculate new kinematic information - self.world.step(render=True) # execute one physics step and one rendering step - - return old_q, old_dq, old_u @@ -241,67 +204,22 @@ def g(self, q=None): gravity = self.articulation_view.get_generalized_gravity_forces() coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces() - print(f"Raw gravity shape: {gravity.shape}") - print(f"Raw coriolis shape: {coriolis.shape}") - # Total generalized bias forces g_full = gravity + coriolis - print(f"Combined g_full shape: {g_full.shape}") # Handle batch dimension if present if g_full.ndim == 2: - print(f"Detected batch dimension, original shape: {g_full.shape}") if g_full.shape[0] == 1: g_full = g_full[0] # Remove batch dimension - print(f"After removing batch dimension: {g_full.shape}") if q is not None: - print(f"q shape: {q.shape}") # If q is provided, ensure g matches the size of q if len(g_full) != len(q): - print(f"Truncating g from {len(g_full)} to {len(q)} elements") g_full = g_full[:len(q)] - print(f"Final g shape: {g_full.shape}") return -g_full - ''' + - def g(self, q=None): - """ - Returns the joint-space forces due to gravity, Coriolis, and centrifugal effects - in Isaac Sim (equivalent to MuJoCo's qfrc_bias). - - Parameters - ---------- - q: np.ndarray, optional (Default: None) - Joint positions to compute the bias forces at. If None, uses current sim state. - """ - if not self.use_sim_state and q is not None: - old_q, old_dq, old_u = self._load_state(q) - - # Set new state (velocities to zero to isolate gravity) - self.articulation_view.set_joint_positions(q) - self.articulation_view.set_joint_velocities(np.zeros_like(q)) - - ### self.world.step(render=False) - - # Compute gravity and Coriolis/centrifugal separately - gravity = self.articulation_view.get_generalized_gravity_forces() - coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces() - - # Total generalized bias forces - g = gravity + coriolis - # print("GRAVITY ", gravity) - # print("CORIOLIS ", coriolis) - - if not q is not None: - # Restore old state - self.articulation_view.set_joint_positions(old_q) - self.articulation_view.set_joint_velocities(old_dq) - ### self.world.step(render=False) - - return -g # match MuJoCo's negative qfrc_bias convention - ''' def dJ(self, name, q=None, dq=None, x=None): """Returns the derivative of the Jacobian wrt to time @@ -353,7 +271,7 @@ def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): # Handle special case mappings if name == "EE": - name = self.ee_name + name = self.ee_link_name # Check for unsupported features if x is not None and not np.allclose(x, 0): @@ -552,7 +470,6 @@ def R(self, name, q=None, object_type="body"): self.articulation_view.set_joint_positions(old_q) #self._world.step(render=False) ''' - return R @@ -570,11 +487,8 @@ def quaternion(self, name, q=None): """ #TODO outsource this to a common function and check is can be qued for EE and # end_effector at the same time, or checked which is used for the current robot - if name == "EE": name = self.ee_name - ''' - if not self.use_sim_state and q is not None: - old_q, old_dq, old_u = self._load_state(q) - ''' + if name == "EE": name = self.ee_link_name + prim_path = self._get_prim_path(name) prim = self.stage.GetPrimAtPath(prim_path) @@ -587,10 +501,7 @@ def quaternion(self, name, q=None): # Convert to [w, x, y, z] NumPy array quat_np = np.array([quat.GetReal(), *quat.GetImaginary()]) - ''' - if not self.use_sim_state and q is not None: - self._load_state(old_q, old_dq, old_u) - ''' + return quat_np def C(self, q=None, dq=None): @@ -624,7 +535,7 @@ def Tx(self, name, q=None, object_type="body"): """Simplified version that only gets current position without state changes.""" #TODO handle q if name == "EE": - name = self.ee_name + name = self.ee_link_name # Get prim path if object_type in ["body", "link"]: @@ -664,7 +575,7 @@ def Tx_old(self, name, q=None, x=None, object_type="body"): np.ndarray World position [x, y, z] of the object. """ - if name == "EE": name = self.ee_name + if name == "EE": name = self.ee_link_name ''' if x is not None and not np.allclose(x, 0): raise Exception("x offset currently not supported: ", x) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 693bfcf0..3dbe32df 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -91,24 +91,28 @@ def connect(self, joint_names=None): omni.kit.commands.execute( "MJCFCreateAsset", #mjcf_path=extension_path + "/data/mjcf/nv_ant.xml", - mjcf_path=extension_path + "/data/mjcf/nv_humanoid.xml", - #mjcf_path=self.robot_config.xml_file, + #mjcf_path=extension_path + "/data/mjcf/nv_humanoid.xml", + mjcf_path=self.robot_config.xml_file, import_config=import_config, prim_path=self.prim_path ) + self.ee_name = "flange" #"ft_frame" # end-effector name for UR5 ''' if self.robot_config.robot == "ur5": robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd" self.ee_name = "flange" #"ft_frame" # end-effector name for UR5 + robot_joint_nr = 6 # UR5 has 6 joints elif self.robot_config.robot == "jaco2": robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" self.ee_name = "end_effector" # end-effector name for Jaco2 + robot_joint_nr = 6 # Jaco2 has 6 joints elif self.robot_config.robot == "h1": robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" #TODO check this self.ee_name = "EE" # end-effector name for H1 + robot_joint_nr = 6 # H1 has 6 joints assets_root_path = get_assets_root_path() @@ -117,41 +121,7 @@ def connect(self, joint_names=None): prim_path=self.prim_path, ) robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - - - ''' - ## LOAD UR5 robot - assets_root_path = get_assets_root_path() - stage_utils.add_reference_to_stage( - usd_path=assets_root_path + "/Isaac/Robots/UniversalRobots/ur5/ur5.usd", - # Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd --> 7 DOF arm , not compatible with ABR controller - prim_path=self.prim_path, - ) - robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - - - ## LOAD Jaco2 robot - assets_root_path = get_assets_root_path() - stage_utils.add_reference_to_stage( - usd_path=assets_root_path + "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd", - # Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd --> 7 DOF arm , not compatible with ABR controller - prim_path=self.prim_path, - ) - robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - - - # Load H1 robot - self.h1 = H1FlatTerrainPolicy( - prim_path=self.prim_path, - name=self.name, - usd_path=assets_root_path + "/Isaac/Robots/Unitree/H1/h1.usd", - position=np.array([0, 0, 1.05]), - ) - stage_utils.add_reference_to_stage( - usd_path=assets_root_path + "/Isaac/Robots/Unitree/H1/h1.usd", - prim_path=self.prim_path, - ) - ''' + # Resetting the world needs to be called before querying anything related to an articulation specifically. # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly self.world.reset() @@ -176,6 +146,13 @@ def connect(self, joint_names=None): self.joint_vel_addrs = [] self.joint_dyn_addrs = [] + if joint_names is None: + # get the joint names from the articulation view + joint_nr= robot_joint_nr + else: + joint_nr = len(joint_names) + + print("Connecting to robot config...") self.robot_config._connect( @@ -187,7 +164,7 @@ def connect(self, joint_names=None): self.joint_vel_addrs, self.prim_path, self.ee_name, - joint_names + joint_nr ) @@ -221,7 +198,7 @@ def get_joint_vel_addrs(self, joint_name): index = dof_name_to_index[joint_name] return index - ''' + def send_forces(self, u): """Applies the set of torques u to the arm.""" @@ -231,9 +208,11 @@ def send_forces(self, u): # Apply control torques to the controlled joints full_torques[:len(u)] = u + print("U: ", u) # Apply the control signal self.articulation_view.set_joint_efforts(full_torques) + #self.articulation_view.set_joint_efforts(np.ones(total_dofs) * -10000) # Set the joint efforts (torques) # Move simulation ahead one time step self.world.step(render=True) @@ -251,7 +230,7 @@ def send_forces(self, u): # move simulation ahead one time step self.world.step(render=True) # execute one physics step and one rendering step - + ''' def send_target_angles(self, q): """Moves the arm to the specified joint angles @@ -281,7 +260,8 @@ def send_target_angles(self, q): self.articulation_view.set_joint_positions(q_new) else: self.articulation_view.set_joint_positions(q) - + + self.world.step(render=True) ''' if len(q) > self.robot_config.N_JOINTS: q_new = q[:self.robot_config.N_JOINTS] diff --git a/abr_control/interfaces/nv_isaacsim_usd_jaco.py b/abr_control/interfaces/nv_isaacsim_usd_jaco.py deleted file mode 100644 index f886fbe9..00000000 --- a/abr_control/interfaces/nv_isaacsim_usd_jaco.py +++ /dev/null @@ -1,182 +0,0 @@ -import numpy as np -from isaacsim import SimulationApp -from .interface import Interface - -class IsaacSim(Interface): - """An interface for IsaacSim. - - Parameters - ---------- - robot_config : class instance - contains all relevant information about the arm - such as: number of joints, number of links, mass information etc. - dt : float, optional (Default: 0.001) - simulation time step in seconds - - """ - def __init__(self, robot_config, dt=0.001, force_download=False): - - super().__init__(robot_config) - self.dt = dt # time step - self.count = 0 # keep track of how many times send forces is called - - self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles - self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities - - self.prim_path = "/World/robot" - self.name = "robot" - - #self.misc_handles = {} # for tracking miscellaneous object handles - - - def connect(self, load_scene=True): - if load_scene: - """All initial setup.""" - self.simulation_app = SimulationApp({"headless": False}) - from isaacsim.core.api import World # type: ignore - import omni.isaac.core.utils.stage as stage_utils # type: ignore - import omni.kit.commands # type: ignore - import omni - from isaacsim.storage.native import get_assets_root_path - from isaacsim.core.api.robots import Robot - - # Create a world - self.world = World(physics_dt=self.dt,rendering_dt=self.dt) - self.world.scene.add_default_ground_plane() - - assets_root_path = get_assets_root_path() - - stage_utils.add_reference_to_stage( - usd_path=assets_root_path + "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd", - # Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd --> 7 DOF arm , not compatible with ABR controller - prim_path=self.prim_path, - ) - - robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - - - else: - self.world = SimulationApp.getWorld() - - # Get the articulation - from omni.isaac.core.articulations import Articulation# type: ignore - - # Resetting the world needs to be called before querying anything related to an articulation specifically. - # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly - self.world.reset() - # Load robot - self.articulation = Articulation(prim_path=self.prim_path, name="H1") - self.articulation.initialize() - print("DOF names:", self.articulation.dof_names) - - # necessary so self.q and self.dq are accessible - self.world.initialize_physics() - - - - def disconnect(self): - """Any socket closing etc that must be done to properly shut down""" - self.simulation_app.close() # close Isaac Sim - print("IsaacSim connection closed...") - - - def send_forces(self, u): - """Applies the set of torques u to the arm. If interfacing to - a simulation, also moves dynamics forward one time step. - - u : np.array - An array of joint torques [Nm] - """ - print("robot joint pos: ", self.robot.get_joint_positions()) - print("u: ", u) - # Apply the control signal - #self.articulation.set_joint_efforts(u) - #self.robot.set_joint_efforts(u) - self.robot.set_joint_efforts(u) - - # move simulation ahead one time step - self.world.step(render=True) # execute one physics step and one rendering step - - - - def send_target_angles(self, q): - """Moves the arm to the specified joint angles - - q : numpy.array - the target joint angles [radians] - """ - if np.size(q) == 12: - self.articulation.set_joint_positions(q) - if np.size(q) == 6: - fb = self.get_feedback(with_hand=True) - fb_q = fb["q"] - fb_q[:6] = q - self.articulation.set_joint_positions(fb_q) - else: - print("The method send_target_angles of the isaacsim interface does not support the number of joint angles that are attempted to be set.") - - # move simulation ahead one time step - self.world.step(render=True) # execute one physics step and one rendering step - - - - def get_feedback(self, with_hand=False): - """Return a dictionary of information needed by the controller. - - Returns the joint angles and joint velocities in [rad] and [rad/sec], - respectively - """ - q = self.articulation.get_joint_positions() - dq = self.articulation.get_joint_velocities() - if with_hand: - # Get the joint angles and velocities - self.q = q - self.dq = dq - else: - self.q = q[:6] - self.dq = dq[:6] - return {"q": self.q, "dq": self.dq} - - - - def get_xyz(self, name): - """Returns the xyz position of the specified object - - name : string - name of the object you want the xyz position of - """ - print("NAME: ", name) - #TODO check if we need misc handles for this - obj = self.world.scene.get_object(name) - object_position, object_orientation = obj.get_world_pose() - - return object_position - - - def get_orientation(self, name): - """Returns the orientation of an object in CoppeliaSim - - the Euler angles [radians] are returned in the relative xyz frame. - http://www.coppeliarobotics.com/helpFiles/en/eulerAngles.htm - - Parameters - ---------- - name : string - the name of the object of interest - """ - - obj = self.world.scene.get_object(name) - object_position, object_orientation = obj.get_world_pose() - return object_orientation - - - def set_xyz(self, name, xyz): - """Set the position of an object in the environment. - - name : string - the name of the object - xyz : np.array - the [x,y,z] location of the target [meters] - """ - _cube = self.world.scene.get_object(name) - _cube.set_world_pose(xyz, np.array([0., 0., 0., 1.])) # set the position and orientation of the object diff --git a/examples/IsaacSim/force_floating_control.py b/examples/IsaacSim/force_floating_control.py index 41d1124a..18f4b3a8 100644 --- a/examples/IsaacSim/force_floating_control.py +++ b/examples/IsaacSim/force_floating_control.py @@ -15,8 +15,8 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - #arm_model = "ur5" - arm_model = "jaco2" + arm_model = "ur5" + #arm_model = "jaco2" # initialize our robot config robot_config = arm(arm_model) # create the IsaacSim interface and connect up @@ -24,22 +24,23 @@ interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) 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_name = "end_effector" # EE + ee_track = [] q_track = [] try: # get the end-effector's initial position - feedback = interface.get_feedback() - start = robot_config.Tx(ee_name, q=feedback["q"]) + feedback = interface.get_feedback(all_joints=True) + start = robot_config.Tx(interface.ee_name, q=feedback["q"]) print("\nSimulation starting...\n") while 1: # get joint angle and velocity feedback - feedback = interface.get_feedback() + feedback = interface.get_feedback(all_joints=True) # calculate the control signal u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"]) @@ -48,7 +49,7 @@ interface.send_forces(u) # calculate the position of the hand - hand_xyz = robot_config.Tx(ee_name, q=feedback["q"]) + hand_xyz = robot_config.Tx(interface.ee_name, q=feedback["q"]) # track end effector position ee_track.append(np.copy(hand_xyz)) q_track.append(np.copy(feedback["q"])) From 361b35c6997792bd6cb5d622d25b1f6e2f9eaa3a Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Wed, 25 Jun 2025 14:57:44 -0400 Subject: [PATCH 20/42] new implementation of the method connect, following the structure of mujoco interface --- abr_control/interfaces/nv_isaacsim.py | 318 ++++++++++++++------------ 1 file changed, 176 insertions(+), 142 deletions(-) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 3dbe32df..be8ae874 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -21,9 +21,7 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called - # ee_name = "ft_frame" for ur2 - # #"end_effector" for jaco2 - + #self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles #self.initial_q = np.zeros(self.robot_config.N_JOINTS) # joint angles #self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities @@ -37,7 +35,9 @@ def __init__(self, robot_config, dt=0.001, force_download=False): #self.misc_handles = {} # for tracking miscellaneous object handles - def connect(self, joint_names=None): + + + def connect(self, joint_names=None, camera_id=-1): """ joint_names: list, optional (Default: None) list of joint names to send control signal to and get feedback from @@ -48,112 +48,123 @@ def connect(self, joint_names=None): """All initial setup.""" self.simulation_app = SimulationApp({"headless": False}) - from isaacsim.core.api import World # type: ignore - import omni.isaac.core.utils.stage as stage_utils # type: ignore - import omni.kit.commands # type: ignore import omni - from isaacsim.robot.policy.examples.robots.h1 import H1FlatTerrainPolicy - from isaacsim.storage.native import get_assets_root_path + import omni.isaac.core.utils.stage as stage_utils # type: ignore + from omni.isaac.core import World + #from omni.isaac.core.utils.stage import get_current_stage + from omni.isaac.core.utils.prims import get_prim_at_path from omni.isaac.core.articulations import Articulation, ArticulationView # type: ignore - import omni.isaac.core.utils.stage as stage_utils # type: ignore + from omni.isaac.core.utils.nucleus import get_assets_root_path + #TODO change import + #TODO is "Robot" even necessary from isaacsim.core.api.robots import Robot - from pxr import UsdLux, Sdf, Gf, UsdPhysics + from pxr import UsdPhysics, UsdGeom - # Create a world - self.world = World(physics_dt=self.dt,rendering_dt=self.dt) + # Initialize the simulation world + self.world = World(stage_units_in_meters=1.0) + self.world.scene.add_default_ground_plane() self.context = omni.usd.get_context() self.stage = self.context.get_stage() - #TODO necessary for H1 robot + #TODO necessary for H1 robot #self.world.add_physics_callback("send_actions", self.send_actions) - self.world.scene.add_default_ground_plane() - - # enable physics - scene = UsdPhysics.Scene.Define(self.stage, Sdf.Path("/physicsScene")) - - # set gravity - scene.CreateGravityDirectionAttr().Set(Gf.Vec3f(0.0, 0.0, -1.0)) - scene.CreateGravityMagnitudeAttr().Set(981.0) - - # add lighting - distantLight = UsdLux.DistantLight.Define(self.stage, Sdf.Path("/DistantLight")) - distantLight.CreateIntensityAttr(500) - ''' - # setting up import configuration: - status, import_config = omni.kit.commands.execute("MJCFCreateImportConfig") - import_config.set_fix_base(True) # fix the base of the robot - import_config.set_make_default_prim(False) - - # Get path to extension data: - ext_manager = omni.kit.app.get_app().get_extension_manager() - ext_id = ext_manager.get_enabled_extension_id("isaacsim.asset.importer.mjcf") - extension_path = ext_manager.get_extension_path(ext_id) - # import MJCF - omni.kit.commands.execute( - "MJCFCreateAsset", - #mjcf_path=extension_path + "/data/mjcf/nv_ant.xml", - #mjcf_path=extension_path + "/data/mjcf/nv_humanoid.xml", - mjcf_path=self.robot_config.xml_file, - import_config=import_config, - prim_path=self.prim_path - ) - self.ee_name = "flange" #"ft_frame" # end-effector name for UR5 - ''' - + robot_path = None + # Load the robot from USD file if self.robot_config.robot == "ur5": robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd" - self.ee_name = "flange" #"ft_frame" # end-effector name for UR5 - robot_joint_nr = 6 # UR5 has 6 joints + self.ee_link_name = "flange" #"ft_frame" # end-effector name for UR5 elif self.robot_config.robot == "jaco2": robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" - self.ee_name = "end_effector" # end-effector name for Jaco2 - robot_joint_nr = 6 # Jaco2 has 6 joints + self.ee_link_name = "end_effector" # end-effector name for Jaco2 elif self.robot_config.robot == "h1": robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" #TODO check this - self.ee_name = "EE" # end-effector name for H1 - robot_joint_nr = 6 # H1 has 6 joints + self.ee_link_name = "EE" # end-effector name for H1 assets_root_path = get_assets_root_path() + robot_usd_path = f"{assets_root_path}{robot_path}" + print(f"Loading robot from USD path: {robot_usd_path}") + + + stage_utils.add_reference_to_stage( usd_path=assets_root_path + robot_path, prim_path=self.prim_path, ) robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - - # Resetting the world needs to be called before querying anything related to an articulation specifically. - # Its recommended to always do a reset after adding your assets, for physics handles to be propagated properly + ''' + self.robot = self.world.scene.add( + Articulation( + prim_path="/World/Robot", + name="robot", + usd_path=robot_usd_path, + position=[0, 0, 0] + ) + ) + ''' self.world.reset() - - # Load robot self.articulation = Articulation(prim_path=self.prim_path, name=self.name + "_articulation") self.articulation.initialize() self.world.scene.add(self.articulation) # Add to scene if not already added by higher-level env + #TODO remove and replace with articulation self.articulation_view = ArticulationView(prim_paths_expr=self.prim_path, name=self.name + "_view") self.world.scene.add(self.articulation_view) self.articulation_view.initialize() - + + # Set simulation time step + self.world.get_physics_context().set_physics_dt(self.dt) + + # Reset the world to initialize physics self.world.reset() - # necessary so self.q and self.dq are accessible - self.world.initialize_physics() - - self.world.step(render=False) - + + # Get joint information self.joint_pos_addrs = [] self.joint_vel_addrs = [] self.joint_dyn_addrs = [] + + if joint_names is None: - # get the joint names from the articulation view - joint_nr= robot_joint_nr + print("No joint names provided, using all controllable joints in the articulation.") + # Get all controllable joints in the articulation + joint_names = self.articulation.dof_names + # If we need to filter to kinematic chain from EE to base + #TODO folowing, never jumped into, maybe remove + if hasattr(self.robot_config, 'ee_link_name'): + print("Using end-effector link name from robot config:", self.robot_config.ee_link_name) + ee_link_name = self.robot_config.ee_link_name + # Get kinematic chain from end-effector to base + joint_names = self._get_kinematic_chain_joints(ee_link_name) else: - joint_nr = len(joint_names) - + # Handle joint name mapping + joint_names = self._map_joint_names(joint_names) + + # Validate joint names and get indices + all_joint_names = self.articulation.dof_names + self.controlled_joint_indices = [] + + for name in joint_names: + if name not in all_joint_names: + raise Exception(f"Joint name {name} does not exist in robot model") + joint_idx = all_joint_names.index(name) + self.controlled_joint_indices.append(joint_idx) + self.joint_pos_addrs.append(joint_idx) + self.joint_vel_addrs.append(joint_idx) + self.joint_dyn_addrs.append(joint_idx) + + # Store joint names for later use + self.controlled_joint_names = joint_names + + # Initialize joint position and velocity arrays + self.num_dof = len(self.controlled_joint_indices) + + + # Connect robot config with simulation data print("Connecting to robot config...") self.robot_config._connect( self.world, @@ -163,41 +174,103 @@ def connect(self, joint_names=None): self.joint_pos_addrs, self.joint_vel_addrs, self.prim_path, - self.ee_name, - joint_nr + self.ee_link_name, ) - def disconnect(self): - """Any socket closing etc that must be done to properly shut down""" - self.simulation_app.close() # close Isaac Sim - print("IsaacSim connection closed...") + print(f"Connected to robot with {self.num_dof} controlled joints: {self.controlled_joint_names}") + - #TODO adapt to IsaacSim - def get_joint_pos_addrs(self, jntadr): - # store the data.qpos indices associated with this joint - first_pos = self.model.jnt_qposadr[jntadr] - posvec_length = self.robot_config.JNT_POS_LENGTH[self.model.jnt_type[jntadr]] - joint_pos_addr = list(range(first_pos, first_pos + posvec_length))[::-1] - return joint_pos_addr - def get_joint_vel_addrs(self, joint_name): - if self.articulation_view is None: - raise RuntimeError("Robot ArticulationView not set up.") + ''' + def _get_kinematic_chain_joints(self, ee_link_name): + """ + Get joints in kinematic chain from end-effector to base + """ - #dof_indices = self.articulation_view.get_dof_indices(joint_name) - #print('dof_indices:', dof_indices) - # Get all DOF names in the articulation - dof_names = self.articulation_view.dof_names - dof_name_to_index = {name: i for i, name in enumerate(dof_names)} + # For most robot arms, we can use all revolute joints + # This would need to be customized based on your specific robot + all_joints = self.articulation.dof_names + # Filter for revolute joints (exclude fixed joints) + kinematic_joints = [] + for joint_name in all_joints: + #joint_prim_path = f"/World/Robot/{joint_name}" + joint_prim_path = f"{self.prim_path}{joint_name}" + + joint_prim = get_prim_at_path(joint_prim_path) + if joint_prim and joint_prim.HasAPI(UsdPhysics.RevoluteJointAPI): + kinematic_joints.append(joint_name) - if joint_name.endswith(self.ee_name): - index = None - else: - index = dof_name_to_index[joint_name] + return kinematic_joints + ''' + + + + def _map_joint_names(self, joint_names): + """ + Map joint names from MuJoCo format to IsaacSim format + """ + # Get actual joint names from the robot + actual_joint_names = self.articulation.dof_names + + # If input names are in MuJoCo format (joint0, joint1, etc.) + if all(name.startswith('joint') and name[5:].isdigit() for name in joint_names): + # Map by index: joint0 -> first joint, joint1 -> second joint, etc. + mapped_names = [] + for name in joint_names: + joint_idx = int(name[5:]) # Extract number from "jointX" + if joint_idx < len(actual_joint_names): + mapped_names.append(actual_joint_names[joint_idx]) + else: + raise Exception(f"Joint index {joint_idx} out of range. Robot has {len(actual_joint_names)} joints.") + return mapped_names + + # If names are already in correct format, return as-is + return joint_names + + def get_joint_positions(self): + """Get current joint positions""" + if hasattr(self, 'articulation'): + all_positions = self.articulation.get_joint_positions() + return all_positions[self.controlled_joint_indices] + return None + + def get_joint_velocities(self): + """Get current joint velocities""" + if hasattr(self, 'articulation'): + all_velocities = self.articulation.get_joint_velocities() + return all_velocities[self.controlled_joint_indices] + return None + + def set_joint_positions(self, positions): + """Set joint positions""" + if hasattr(self, 'articulation'): + full_positions = self.articulation.get_joint_positions() + for i, idx in enumerate(self.controlled_joint_indices): + full_positions[idx] = positions[i] + self.articulation.set_joint_positions(full_positions) + + def set_joint_velocities(self, velocities): + """Set joint velocities""" + if hasattr(self, 'articulation'): + full_velocities = self.articulation.get_joint_velocities() + for i, idx in enumerate(self.controlled_joint_indices): + full_velocities[idx] = velocities[i] + self.articulation.set_joint_velocities(full_velocities) + + + + + + + + def disconnect(self): + """Any socket closing etc that must be done to properly shut down""" + self.simulation_app.close() # close Isaac Sim + print("IsaacSim connection closed...") + - return index def send_forces(self, u): """Applies the set of torques u to the arm.""" @@ -208,7 +281,7 @@ def send_forces(self, u): # Apply control torques to the controlled joints full_torques[:len(u)] = u - print("U: ", u) + #print("U: ", u) # Apply the control signal self.articulation_view.set_joint_efforts(full_torques) @@ -217,20 +290,8 @@ def send_forces(self, u): # Move simulation ahead one time step self.world.step(render=True) - ''' - def send_forces(self, u): - """Applies the set of torques u to the arm. If interfacing to - a simulation, also moves dynamics forward one time step. - - u : np.array - An array of joint torques [Nm] - """ - # Apply the control signal - self.articulation_view.set_joint_efforts(u) + - # move simulation ahead one time step - self.world.step(render=True) # execute one physics step and one rendering step - ''' def send_target_angles(self, q): """Moves the arm to the specified joint angles @@ -238,18 +299,6 @@ def send_target_angles(self, q): q : numpy.array the target joint angles [radians] - print("q: ", q) - print("robot joint pos: ", self.articulation.get_joint_positions()) - q_all = self.get_feedback()["q"] - print("q_all: ", q_all) - #TODO change that to variable number of joints - #q_all = q_all[:self.robot_config.N_JOINTS] # - q_all[:6] = q - print("result: ", q_all) - self.articulation_view.set_joint_positions(q_all) - """ - print("len(q) : ", len(q)) - print("robot_config.N_JOINTS: ", self.robot_config.N_JOINTS) # Check if the length of q is greater than the number of joints if len(q) > self.robot_config.N_JOINTS: q_new = q[:self.robot_config.N_JOINTS] @@ -260,19 +309,10 @@ def send_target_angles(self, q): self.articulation_view.set_joint_positions(q_new) else: self.articulation_view.set_joint_positions(q) - + """ + self.set_joint_positions(q) self.world.step(render=True) - ''' - if len(q) > self.robot_config.N_JOINTS: - q_new = q[:self.robot_config.N_JOINTS] - self.articulation_view.set_joint_positions(q_new) - elif self.robot_config.N_ALL_JOINTS > self.robot_config.N_JOINTS: - q_new = self.articulation_view.get_joint_positions() # Shape: (1, 12) - q_new[0, :self.robot_config.N_JOINTS] = q # Update first N_JOINTS for environment 0 - self.articulation_view.set_joint_positions(q_new) - else: - self.articulation_view.set_joint_positions(q) - ''' + @@ -283,15 +323,9 @@ def get_feedback(self, all_joints=False): Returns the joint angles and joint velocities in [rad] and [rad/sec], respectively """ - if not all_joints: - # Get the joint angles and velocities - self.q = self.articulation.get_joint_positions()[:self.robot_config.N_JOINTS] # only take the first N_JOINTS - self.dq = self.articulation.get_joint_velocities()[:self.robot_config.N_JOINTS] - else: - # Get the joint angles and velocities for all joints - self.q = self.articulation.get_joint_positions() - self.dq = self.articulation.get_joint_velocities() - + self.q = self.get_joint_positions() + self.dq = self.get_joint_velocities() + return {"q": self.q, "dq": self.dq} From 00471ef8123401f341efcb6c8273d6a5b75334e1 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Fri, 27 Jun 2025 11:19:24 -0400 Subject: [PATCH 21/42] Create a visual-only cube (no collision) --- abr_control/arms/isaacsim_config.py | 285 +++++++++++------- abr_control/interfaces/nv_isaacsim.py | 278 +++++++++++------ examples/IsaacSim/flying_cube.py | 64 ---- examples/IsaacSim/force_floating_control.py | 8 +- ...sition_joint_control_inverse_kinematics.py | 122 +++----- 5 files changed, 405 insertions(+), 352 deletions(-) delete mode 100644 examples/IsaacSim/flying_cube.py diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 296a9e0c..62c8f33b 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -127,7 +127,7 @@ def __init__(self, xml_file, folder=None, use_sim_state=True, force_download=Fal self.robot = xml_file self.use_sim_state = use_sim_state - def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path, ee_link_name): + def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -153,39 +153,38 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.joint_pos_addrs = np.copy(joint_pos_addrs) self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path - self.ee_link_name = ee_link_name self.N_JOINTS = len(self.joint_vel_addrs) print (f"Number of controllable joints: {self.N_JOINTS}") # number of joints in the IsaacSim simulation - self.N_ALL_JOINTS = self.articulation_view.num_dof - print (f"Number of ALL joints in simulation: {self.N_ALL_JOINTS}") + N_ALL_JOINTS = self.articulation_view.num_dof + print (f"Number of ALL joints in simulation: {N_ALL_JOINTS}") # need to calculate the joint_vel_addrs indices in flat vectors returned # for the Jacobian self.jac_indices = np.hstack( # 6 because position and rotation Jacobians are 3 x N_JOINTS - [self.joint_vel_addrs + (ii * self.N_ALL_JOINTS) for ii in range(3)] + [self.joint_vel_addrs + (ii * N_ALL_JOINTS) for ii in range(3)] ) # for the inertia matrix self.M_indices = [ - ii * self.N_ALL_JOINTS + jj + ii * N_ALL_JOINTS + jj for jj in self.joint_vel_addrs for ii in self.joint_vel_addrs ] # a place to store data returned from Mujoco self._g = np.zeros(self.N_JOINTS) - self._J3NP = np.zeros((3, self.N_ALL_JOINTS)) - self._J3NR = np.zeros((3, self.N_ALL_JOINTS)) + self._J3NP = np.zeros((3, N_ALL_JOINTS)) + self._J3NR = np.zeros((3, N_ALL_JOINTS)) self._J6N = np.zeros((6, self.N_JOINTS)) - self._MNN = np.zeros((self.N_ALL_JOINTS, self.N_ALL_JOINTS)) + self._MNN = np.zeros((N_ALL_JOINTS, N_ALL_JOINTS)) self._R9 = np.zeros(9) self._R = np.zeros((3, 3)) self._x = np.ones(4) - self.N_ALL_JOINTS = self.N_ALL_JOINTS + self.N_ALL_JOINTS = N_ALL_JOINTS @@ -269,9 +268,6 @@ def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): Last 3 rows: angular velocity Jacobian (Jω) """ - # Handle special case mappings - if name == "EE": - name = self.ee_link_name # Check for unsupported features if x is not None and not np.allclose(x, 0): @@ -283,17 +279,7 @@ def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): elif not hasattr(self, 'controlled_dof_indices'): self.controlled_dof_indices = list(range(6)) # Default: first 6 DOFs - ''' - # Handle joint state setting if provided - if q is not None: - # Store current state if we need to restore it - old_positions = self.articulation_view.get_joint_positions(clone=True) - # Set new joint positions - self.articulation_view.set_joint_positions(q.reshape(1, -1)) - # Forward the simulation to update Jacobians - # You might need to call a simulation step here depending on your setup - ''' - + if object_type == "body": # Get Jacobians from articulation view jacobians = self.articulation_view.get_jacobians(clone=True) @@ -303,8 +289,14 @@ def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") # Get link index - link_index = self._get_link_index(name) - + # General + link_index = self.articulation_view.get_link_index("j2n6s300_end_effector") + # UR + #link_index = self.articulation_view.get_link_index("wrist_3_link") + #link_index = self.articulation_view.get_link_index("j2n6s300_end_effector") + print("name: ", name) + print("link index: ", link_index) + # Extract Jacobian for specific link env_idx = 0 # Assuming single environment @@ -346,79 +338,121 @@ def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): - - def _get_link_index(self, name): - """Helper function to get link index from name.""" - try: - # Method 1: Direct lookup if body_names includes simple names - if hasattr(self.articulation_view, 'body_names'): - link_list = self.articulation_view.body_names - #print("link_list: ", link_list) - - # Try direct name first - if name in link_list: - return link_list.index(name) - - # Try with prefix (your original approach) - if link_list and '_' in link_list[0]: - prefix = link_list[0].split('_')[0] - full_name = f"{prefix}_{name}" - if full_name in link_list: - return link_list.index(full_name) - - # Method 2: Use articulation API if available - if hasattr(self.articulation, 'get_body_index'): - return self.articulation.get_body_index(name) - - # Method 3: Manual search through body names - for i, body_name in enumerate(link_list): - if body_name.endswith(name) or name in body_name: - return i - - raise ValueError(f"Link '{name}' not found in articulation.") + + def M(self, q=None, indices=None): + """ + Returns the inertia matrix using Isaac Sim Core API + + Args: + q (np.ndarray, optional): Joint positions + indices (optional): Specific articulation indices - except Exception as e: - raise RuntimeError(f"Error finding link '{name}': {str(e)}") - + Returns: + np.ndarray: Inertia matrix + """ + M = self._compute_mass_matrix_numerical() + + # Restore original state if q was provided + #if q is not None: + # self.articulation.set_joint_positions(current_positions) + # self.world.step(render=False) + + return np.copy(M) - def M(self, q=None): + #TODO: CHECK AND IF NECEAARY remove this, use M instead + def _compute_mass_matrix_numerical(self): """ - Return the joint-space inertia matrix M(q) using Isaac Sim. + Compute mass matrix numerically using finite differences + This is a fallback method when direct mass matrix access is not available + """ + import numpy as np + + M = np.zeros((self.N_JOINTS, self.N_JOINTS)) + + # Small perturbation for finite differences + epsilon = 1e-6 + + # Get current state + current_pos = self.get_joint_positions() + current_vel = self.get_joint_velocities() - Parameters - ---------- - q : np.ndarray, optional - Joint positions. If None, use current sim state. + # Zero velocities for clean computation + zero_vel = np.zeros_like(current_vel) + self.set_joint_velocities(zero_vel) + + # Compute each column of mass matrix + for i in range(self.N_JOINTS): + # Create unit acceleration in joint i + unit_accel = np.zeros(self.N_JOINTS) + unit_accel[i] = 1.0 + + # Compute required torques for this acceleration + # Using inverse dynamics: tau = M*qdd + C + G + tau = self._compute_inverse_dynamics(current_pos, zero_vel, unit_accel) + + # The torques give us the i-th column of the mass matrix + M[:, i] = tau + + return M + - Returns - ------- - np.ndarray - Dense inertia matrix (DoF x DoF) + + def _compute_inverse_dynamics(self, q, qd, qdd): + """ + Compute inverse dynamics: tau = M*qdd + C + G + This is an approximation using IsaacSim's physics engine """ ''' - - if not self.use_sim_state and q is not None: - # Save current joint state - old_q = self.articulation_view.get_joint_positions() - self.articulation_view.set_joint_positions(q) - ### self.world.step(render=False) # required to update PhysX buffers + # Method 1: Use PhysX articulation dynamics (if available) + try: + # Set desired accelerations and compute required forces + full_accelerations = np.zeros(self.robot.num_dof) + for i, idx in enumerate(self.joint_vel_addr): + full_accelerations[idx] = qdd[i] + + # Use articulation's compute_efforts method if available + if hasattr(self.robot, 'compute_efforts'): + efforts = self.robot.compute_efforts( + positions=self.robot.get_joint_positions(), + velocities=self.robot.get_joint_velocities(), + accelerations=full_accelerations + ) + return efforts[self.joint_vel_addr] + except: + pass ''' - - # Get mass matrix - M = self.articulation_view.get_mass_matrices() - if q is not None: - M = M[0, :len(q), :len(q)] # Ensure M is square and matches q size - + # Method 2: Numerical approximation + # Apply accelerations and measure required torques + dt = self.world.get_physics_dt() + + # Store current state + original_pos = self.get_joint_positions() + original_vel = self.get_joint_velocities() + + # Set desired state + self.set_joint_positions(q) + self.set_joint_velocities(qd) + + # Compute target velocities after acceleration + target_vel = qd + qdd * dt + + # Use PD control to estimate required torques + kp = 1000.0 # High proportional gain + kd = 100.0 # Damping + + pos_error = np.zeros_like(q) # No position error + vel_error = target_vel - qd + + tau = kp * pos_error + kd * vel_error + + # Restore original state + self.set_joint_positions(original_pos) + self.set_joint_velocities(original_vel) + + return tau - ''' - if not self.use_sim_state and q is not None: - # Restore previous state - self.articulation_view.set_joint_positions(old_q) - ### self.world.step(render=False) - ''' - return M def R(self, name, q=None, object_type="body"): """ @@ -465,11 +499,6 @@ def R(self, name, q=None, object_type="body"): [matrix[2][0], matrix[2][1], matrix[2][2]] ]) - ''' - if not self.use_sim_state and q is not None: - self.articulation_view.set_joint_positions(old_q) - #self._world.step(render=False) - ''' return R @@ -487,7 +516,6 @@ def quaternion(self, name, q=None): """ #TODO outsource this to a common function and check is can be qued for EE and # end_effector at the same time, or checked which is used for the current robot - if name == "EE": name = self.ee_link_name prim_path = self._get_prim_path(name) prim = self.stage.GetPrimAtPath(prim_path) @@ -531,12 +559,9 @@ def T(self, name, q=None, x=None): # TODO if ever required raise NotImplementedError - def Tx(self, name, q=None, object_type="body"): + def Tx(self, name, q=None, x=None, object_type="body"): """Simplified version that only gets current position without state changes.""" - #TODO handle q - if name == "EE": - name = self.ee_link_name - + # Get prim path if object_type in ["body", "link"]: prim_path = self._get_prim_path(name) @@ -575,7 +600,6 @@ def Tx_old(self, name, q=None, x=None, object_type="body"): np.ndarray World position [x, y, z] of the object. """ - if name == "EE": name = self.ee_link_name ''' if x is not None and not np.allclose(x, 0): raise Exception("x offset currently not supported: ", x) @@ -641,23 +665,7 @@ def T_inv(self, name, q=None, x=None): raise NotImplementedError - #TODO remove or see if useful - ''' - def save_current_state(self): - old_q = np.copy(self.articulation.get_joint_positions()) - old_dq = np.copy(self.articulation.get_joint_velocities()) - old_u = np.copy(self.articulation.get_applied_joint_efforts()) - - return old_q, old_dq, old_u - ''' - - #TODO remove or use - def restore_state(self, state): - old_q, old_dq, old_u = state - self.articulation.set_joint_positions(old_q) - self.articulation.set_joint_velocities(old_dq) - self.articulation.set_applied_joint_efforts(old_u) - + # HELPER FUNCTIONS def _get_prim_path(self, name): for prim in self.stage.Traverse(): # Check if the prim path ends with the desired name @@ -666,3 +674,46 @@ def _get_prim_path(self, name): return prim.GetPath() return None + + def get_joint_positions(self): + """Get current joint positions""" + if hasattr(self, 'articulation'): + all_positions = self.articulation.get_joint_positions() + return all_positions[self.joint_vel_addrs] + return None + + def get_joint_velocities(self): + """Get current joint velocities""" + if hasattr(self, 'articulation'): + all_velocities = self.articulation.get_joint_velocities() + return all_velocities[self.joint_vel_addrs] + return None + ''' + def set_joint_positions(self, positions): + """Set joint positions""" + if hasattr(self, 'articulation'): + full_positions = self.articulation.get_joint_positions() + for i, idx in enumerate(range(len(positions))): + full_positions[idx] = positions[i] + self.articulation.set_joint_positions(full_positions) + + ''' + def set_joint_positions(self, positions): + """Set joint positions""" + if hasattr(self, 'articulation'): + full_positions = self.articulation.get_joint_positions() + for i, idx in enumerate(self.joint_vel_addrs): + full_positions[idx] = positions[i] + self.articulation.set_joint_positions(full_positions) + + + def set_joint_velocities(self, velocities): + """Set joint velocities""" + if hasattr(self, 'articulation'): + full_velocities = self.articulation.get_joint_velocities() + for i, idx in enumerate(self.joint_vel_addrs): + full_velocities[idx] = velocities[i] + self.articulation.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 index be8ae874..eb03b847 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -2,6 +2,7 @@ from isaacsim import SimulationApp from .interface import Interface + class IsaacSim(Interface): """An interface for IsaacSim. @@ -21,17 +22,9 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called - - #self.q = np.zeros(self.robot_config.N_JOINTS) # joint angles - #self.initial_q = np.zeros(self.robot_config.N_JOINTS) # joint angles - #self.dq = np.zeros(self.robot_config.N_JOINTS) # joint_velocities - self.prim_path = "/World/robot" self.name = self.robot_config.xml_dir.rsplit('/', 1)[-1] - #self.name = "robot" - #robot_name = self.robot_config.xml_dir.rsplit('/', 1)[-1] - #self.misc_handles = {} # for tracking miscellaneous object handles @@ -49,6 +42,7 @@ def connect(self, joint_names=None, camera_id=-1): """All initial setup.""" self.simulation_app = SimulationApp({"headless": False}) import omni + import omni.kit.commands # type: ignore import omni.isaac.core.utils.stage as stage_utils # type: ignore from omni.isaac.core import World #from omni.isaac.core.utils.stage import get_current_stage @@ -72,14 +66,22 @@ def connect(self, joint_names=None, camera_id=-1): # Load the robot from USD file if self.robot_config.robot == "ur5": robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd" + #ee_path = self.create_ee_xform("wrist_3_link", np.array([-0.04, 0, 0])) + EE_parent_link = "wrist_3_link" # end-effector link for UR5 + has_EE = False # UR5 has no end-effector self.ee_link_name = "flange" #"ft_frame" # end-effector name for UR5 elif self.robot_config.robot == "jaco2": robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" + EE_link = "j2n6s300_end_effector" + EE_parent_link = "j2n6s300_link_6" # end-effector link for Jaco2 + has_EE = True # Jaco2 has an end-effector + #omni.kit.commands.execute("MovePrim", path_from="/World/robot/j2n6s300_end_effector", path_to="/World/robot/EE") + #ee_path = self.create_ee_xform("j2n6s300_link_6", np.array([-0.04, 0, 0])) self.ee_link_name = "end_effector" # end-effector name for Jaco2 elif self.robot_config.robot == "h1": robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" #TODO check this - self.ee_link_name = "EE" # end-effector name for H1 + EE_parent_link = "link6" # end-effector link for H1 assets_root_path = get_assets_root_path() @@ -130,14 +132,7 @@ def connect(self, joint_names=None, camera_id=-1): if joint_names is None: print("No joint names provided, using all controllable joints in the articulation.") # Get all controllable joints in the articulation - joint_names = self.articulation.dof_names - # If we need to filter to kinematic chain from EE to base - #TODO folowing, never jumped into, maybe remove - if hasattr(self.robot_config, 'ee_link_name'): - print("Using end-effector link name from robot config:", self.robot_config.ee_link_name) - ee_link_name = self.robot_config.ee_link_name - # Get kinematic chain from end-effector to base - joint_names = self._get_kinematic_chain_joints(ee_link_name) + joint_names = self.articulation.dof_names else: # Handle joint name mapping joint_names = self._map_joint_names(joint_names) @@ -146,26 +141,31 @@ def connect(self, joint_names=None, camera_id=-1): # Validate joint names and get indices all_joint_names = self.articulation.dof_names - self.controlled_joint_indices = [] for name in joint_names: if name not in all_joint_names: raise Exception(f"Joint name {name} does not exist in robot model") joint_idx = all_joint_names.index(name) - self.controlled_joint_indices.append(joint_idx) self.joint_pos_addrs.append(joint_idx) self.joint_vel_addrs.append(joint_idx) self.joint_dyn_addrs.append(joint_idx) - # Store joint names for later use - self.controlled_joint_names = joint_names + # is boolean flag necessary? + if has_EE: + print("End Effector ...") + # 'EE_link = "j2n6s300_end_effector"' + else: + print("No End Effector, creating EE Xform...") + #ee_info = self.initialize_robot_end_effector(self.prim_path, EE_parent_link, np.array([-0.04, 0, 0])) + #ee_path = self.create_ee_xform(EE_parent_link, np.array([-0.04, 0, 0])) + #print("End Effector Info: ", ee_info) - # Initialize joint position and velocity arrays - self.num_dof = len(self.controlled_joint_indices) - + # Connect robot config with simulation data print("Connecting to robot config...") + print("Joint Position Addresses: ", self.joint_pos_addrs) + print("Joint Velocity Addresses: ", self.joint_vel_addrs) self.robot_config._connect( self.world, self.stage, @@ -174,11 +174,8 @@ def connect(self, joint_names=None, camera_id=-1): self.joint_pos_addrs, self.joint_vel_addrs, self.prim_path, - self.ee_link_name, ) - - print(f"Connected to robot with {self.num_dof} controlled joints: {self.controlled_joint_names}") @@ -229,35 +226,7 @@ def _map_joint_names(self, joint_names): # If names are already in correct format, return as-is return joint_names - def get_joint_positions(self): - """Get current joint positions""" - if hasattr(self, 'articulation'): - all_positions = self.articulation.get_joint_positions() - return all_positions[self.controlled_joint_indices] - return None - - def get_joint_velocities(self): - """Get current joint velocities""" - if hasattr(self, 'articulation'): - all_velocities = self.articulation.get_joint_velocities() - return all_velocities[self.controlled_joint_indices] - return None - def set_joint_positions(self, positions): - """Set joint positions""" - if hasattr(self, 'articulation'): - full_positions = self.articulation.get_joint_positions() - for i, idx in enumerate(self.controlled_joint_indices): - full_positions[idx] = positions[i] - self.articulation.set_joint_positions(full_positions) - - def set_joint_velocities(self, velocities): - """Set joint velocities""" - if hasattr(self, 'articulation'): - full_velocities = self.articulation.get_joint_velocities() - for i, idx in enumerate(self.controlled_joint_indices): - full_velocities[idx] = velocities[i] - self.articulation.set_joint_velocities(full_velocities) @@ -286,7 +255,11 @@ def send_forces(self, u): # Apply the control signal self.articulation_view.set_joint_efforts(full_torques) #self.articulation_view.set_joint_efforts(np.ones(total_dofs) * -10000) # Set the joint efforts (torques) - + + #full_torques[:len(u)] = np.ones(len(u)) * -1000 + #full_torques[1] = -1000 + #self.articulation_view.set_joint_efforts(full_torques) # Set the joint efforts (torques) + # Move simulation ahead one time step self.world.step(render=True) @@ -310,56 +283,63 @@ def send_target_angles(self, q): else: self.articulation_view.set_joint_positions(q) """ - self.set_joint_positions(q) - self.world.step(render=True) - - + self.robot_config.set_joint_positions(q) + self.world.step(render=True) - def get_feedback(self, all_joints=False): + def get_feedback(self): """Return a dictionary of information needed by the controller. Returns the joint angles and joint velocities in [rad] and [rad/sec], respectively """ - self.q = self.get_joint_positions() - self.dq = self.get_joint_velocities() - + self.q = self.robot_config.get_joint_positions() + self.dq = self.robot_config.get_joint_velocities() return {"q": self.q, "dq": self.dq} - def get_xyz(self, name): + def get_xyz(self, prim_path): """Returns the xyz position of the specified object - name : string - name of the object you want the xyz position of + prim_path : string + path of the object you want the xyz position of """ - #TODO check if we need misc handles for this - obj = self.world.scene.get_object(name) - object_position, object_orientation = obj.get_world_pose() + transform_matrix = self.get_transform(prim_path) + translation = transform_matrix.ExtractTranslation() + return np.array([translation[0], translation[1], translation[2]], dtype=np.float64) - return object_position - - #TODO check if overlap to def quaternion - def get_orientation(self, name): - """Returns the orientation of an object in CoppeliaSim - - the Euler angles [radians] are returned in the relative xyz frame. - http://www.coppeliarobotics.com/helpFiles/en/eulerAngles.htm + #TODO check if overlap to def quaternion + def get_orientation(self, prim_path): + """Returns the orientation of an object in IsaacSim Parameters ---------- name : string the name of the object of interest """ - - obj = self.world.scene.get_object(name) - object_position, object_orientation = obj.get_world_pose() - return object_orientation + transform_matrix = self.get_transform(prim_path) + quat = transform_matrix.ExtractRotationQuat() + quat_np = np.array([quat.GetReal(), *quat.GetImaginary()]) + return quat_np + def get_transform(self, prim_path): + from pxr import Usd, UsdGeom, Gf + _cube = self.stage.GetPrimAtPath(prim_path) + # Check if it's an Xformable + if not _cube.IsValid() or not UsdGeom.Xformable(_cube): + print(f"Prim at {_cube.GetPath()} is not a valid Xformable.") + else: + xformable = UsdGeom.Xformable(_cube) + # Get the local transformation matrix + transform_matrix = xformable.GetLocalTransformation() + + return transform_matrix + + + #TODO change method name in 'set_named_prim' or something as mocap is mujoco thing def set_mocap_xyz(self, name, xyz): """ @@ -389,7 +369,7 @@ def set_mocap_xyz(self, name, xyz): #TODO remove as is the same as above - def set_xyz(self, name, xyz): + def set_xyz(self, prim_path, xyz, orientation=np.array([0., 0., 0., 1.])): """Set the position of an object in the environment. name : string @@ -397,9 +377,14 @@ def set_xyz(self, name, xyz): xyz : np.array the [x,y,z] location of the target [meters] """ - _cube = self.world.scene.get_object(name) - _cube.set_world_pose(xyz, np.array([0., 0., 0., 1.])) # set the position and orientation of the object + from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics # type: ignore + _cube = self.stage.GetPrimAtPath(prim_path) + #_cube.set_world_pose(xyz, orientation) # set the position and orientation of the object + + xformable = UsdGeom.Xformable(_cube) + transform_matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(xyz[0], xyz[1], xyz[2])) + xformable.MakeMatrixXform().Set(transform_matrix) # method for keep_standing @@ -424,4 +409,125 @@ def get_prim_ends_with_name(self, name): prim_path = prim.GetPath() break return prim_path - \ No newline at end of file + + + + + + + + + # Create a visual-only cube (no collision) + def create_target_prim(self, prim_path="/World/target_cube", position=np.array([0, 0, 1.0]), size = .1, color=np.array([0, 0, 1.0])): + from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics # type: ignore + + # Create cube geometry + cube_prim = UsdGeom.Cube.Define(self.stage, prim_path) + cube_prim.CreateSizeAttr(size) # Unit cube + + # Set transform (position and scale) + xformable = UsdGeom.Xformable(cube_prim) + transform_matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(position[0], position[1], position[2])) + xformable.MakeMatrixXform().Set(transform_matrix) + + # 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 + material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface") + + # Bind material to cube + 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 + + + #TODO remove + def find_EE(self, EE_name="end_effector"): + """Find the end-effector""" + + link_list = self.articulation_view.body_names + + + + # Check if the articulation has an end-effector link + ee_link_name = None + for link_name in self.articulation_view.body_names: + if "end_effector" in link_name.lower() or "ee" in link_name.lower(): + ee_link_name = link_name + break + + if ee_link_name is None: + raise Exception("End Effector link not found in the articulation.") + + # Return the name of the end-effector link + return ee_link_name + + + # Create an invisible Xform for EE (wrist_3_link for UR5) + def create_ee_xform(self, parent_link="link6", ee_offset=np.array([-0.04, 0, 0])): + """Create an invisible Xform node for End Effector queries + + Parameters + ---------- + parent_link : str + Name of the final link + ee_offset : np.array + Offset from parent_link to EE point [x, y, z] + """ + from pxr import UsdGeom, Gf, UsdPhysics, Sdf + + # Create an Xform (transform node) for the EE + ee_path = f"{self.prim_path}/{parent_link}/EE" + print("ee path: ", ee_path) + ee_xform = UsdGeom.Xform.Define(self.stage, ee_path) + + # Set position relative to parent_link + ee_xform.AddTranslateOp().Set(Gf.Vec3d(ee_offset[0], ee_offset[1], ee_offset[2])) + + # Add minimal physics properties without creating a separate rigid body + # This makes it part of the parent link's rigid body + ee_xform.GetPrim().CreateAttribute("physics:collisionEnabled", Sdf.ValueTypeNames.Bool).Set(False) + + # Add a custom attribute to mark it as an end effector for easier identification + ee_xform.GetPrim().CreateAttribute("custom:isEndEffector", Sdf.ValueTypeNames.Bool).Set(True) + + return ee_path + + + def get_ee_link_info(self, parent_link="link6"): + """Get information about all links including the EE + + Returns + ------- + dict + Dictionary with link names as keys and indices as values + """ + if hasattr(self, 'articulation_view'): + # Get all existing link names + link_names = self.articulation_view.body_names + + # Check if EE exists as a child of link6 + ee_path = f"{self.prim_path}/{parent_link}/EE" + ee_prim = self.stage.GetPrimAtPath(ee_path) + + if ee_prim.IsValid(): + # Add EE to the link names list + link_names.append("EE") + + # Create a mapping of names to indices + link_info = {name: idx for idx, name in enumerate(link_names)} + return link_info + + return {} diff --git a/examples/IsaacSim/flying_cube.py b/examples/IsaacSim/flying_cube.py deleted file mode 100644 index 2c484008..00000000 --- a/examples/IsaacSim/flying_cube.py +++ /dev/null @@ -1,64 +0,0 @@ -import numpy as np - -class FlyingCube(): - - def __init__(self, dt, world, has_collision = True, prim_path="/World/random_cube", name="fancy_cube", position=np.array([0, 0, 1.0]), scale=np.array([0.1015, 0.1015, 0.1015]), color=np.array([0, 0, 1.0])): - # IsaacSim imports must be done after instantiating the SimulationApp, which is done by the setup in the isaacsim interface - from omni.isaac.core.objects import DynamicCuboid # type: ignore - from omni.isaac.dynamic_control import _dynamic_control # type: ignore - - self.world = world - if has_collision: - fancy_cube = self.world.scene.add( - DynamicCuboid( - prim_path=prim_path, - name=name, - position=position, - scale=scale, - color=color, - ) - ) - # Set the cube to be dynamic using dynamic_control - self._dc = _dynamic_control.acquire_dynamic_control_interface() - self.cube_handle = self._dc.get_rigid_body(prim_path) - self._cube = self.world.scene.get_object(name) - - # Add velocity as an attribute of the world so that it is available at callback time - self.input_velocity = np.zeros(3) - - self.world.add_physics_callback(name + "_send_actions", self.send_actions) - self.world.add_physics_callback(name + "_make_state", self.make_state) - else: - prim_path = "/World/VisualOnlyCube" - prim_type = "Cube" - # Create a basic cube prim - - - - - - def send_actions(self, dt): - self._dc.set_rigid_body_linear_velocity(self.cube_handle, self.input_velocity) - - def make_state(self, dt): - position, orientation = self._cube.get_world_pose() - linear_velocity = self._cube.get_linear_velocity() - return { - 'position': np.array([[0., 0., 1.]]), - 'velocity': np.array([[0., 0., 0.]]), - 'orientation': np.array([[0., 0., 0., 0.]]) - } - - def step(self, velocity): - # Update the input velocity - self.input_velocity = velocity - self.world.step() - - # Get the cube's current state - position, orientation = self._cube.get_world_pose() - linear_velocity = self._cube.get_linear_velocity() - return np.concatenate([position, linear_velocity, orientation], axis=0) - - def _get_obj_pos(self): - position, orientation = self._cube.get_world_pose() - return position \ No newline at end of file diff --git a/examples/IsaacSim/force_floating_control.py b/examples/IsaacSim/force_floating_control.py index 18f4b3a8..c0c13565 100644 --- a/examples/IsaacSim/force_floating_control.py +++ b/examples/IsaacSim/force_floating_control.py @@ -35,12 +35,12 @@ try: # get the end-effector's initial position - feedback = interface.get_feedback(all_joints=True) - start = robot_config.Tx(interface.ee_name, q=feedback["q"]) + feedback = interface.get_feedback() + start = robot_config.Tx(interface.ee_link_name, q=feedback["q"]) print("\nSimulation starting...\n") while 1: # get joint angle and velocity feedback - feedback = interface.get_feedback(all_joints=True) + feedback = interface.get_feedback() # calculate the control signal u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"]) @@ -49,7 +49,7 @@ interface.send_forces(u) # calculate the position of the hand - hand_xyz = robot_config.Tx(interface.ee_name, q=feedback["q"]) + hand_xyz = robot_config.Tx(interface.ee_link_name, q=feedback["q"]) # track end effector position ee_track.append(np.copy(hand_xyz)) q_track.append(np.copy(feedback["q"])) diff --git a/examples/IsaacSim/position_joint_control_inverse_kinematics.py b/examples/IsaacSim/position_joint_control_inverse_kinematics.py index f7422cac..381c58e0 100644 --- a/examples/IsaacSim/position_joint_control_inverse_kinematics.py +++ b/examples/IsaacSim/position_joint_control_inverse_kinematics.py @@ -16,7 +16,7 @@ # initialize our robot config for the jaco2 -robot_config = arm("ur5", use_sim_state=False) +robot_config = arm("jaco2", use_sim_state=False) # create our path planner n_timesteps = 2000 @@ -25,64 +25,18 @@ # create our interface dt = 0.001 interface = IsaacSim(robot_config, dt=dt) -interface.connect() -interface.send_target_angles(robot_config.START_ANGLES) -feedback = interface.get_feedback() -isaac_target = FlyingCube(interface.dt, interface.world, name="target", prim_path="/World/target") -ee_name = "end_effector" # EE - - - - - - -print("\nSimulation starting...") -print("Click to move the target.\n") - -count = 0 -while 1: - - if count % n_timesteps == 0: - feedback = interface.get_feedback() - target_xyz = np.array( - [ - np.random.random() * 0.5 - 0.25, - np.random.random() * 0.5 - 0.25, - np.random.random() * 0.5 + 0.5, - ] - ) - R = robot_config.R(ee_name, q=feedback["q"]) - target_orientation = transformations.euler_from_matrix(R, "sxyz") - # update the position of the target - #interface.set_mocap_xyz("target", target_xyz) - interface.set_xyz("target", target_xyz) - - # can use 3 different methods to calculate inverse kinematics - # see inverse_kinematics.py file for details - print('target pos: ', target_xyz) - print('target_orientation: ', target_orientation) - path_planner.generate_path( - position=feedback["q"], - target_position=np.hstack([target_xyz, target_orientation]), - method=3, - dt=0.005, - n_timesteps=n_timesteps, - plot=False, - ) - - # returns desired [position, velocity] - target = path_planner.next()[0] - - # use position control - interface.send_target_angles(target[: robot_config.N_JOINTS]) - interface.viewer.render() - - count += 1 - +# interface.connect() +# interface.send_target_angles(robot_config.START_ANGLES) +n_dof_to_control = 6 +interface.connect(joint_names=[f"joint{ii}" for ii in range(n_dof_to_control)]) +interface.send_target_angles(robot_config.START_ANGLES[:n_dof_to_control]) +feedback = interface.get_feedback() +target_prim_path="/World/target" +isaac_target = interface.create_target_prim(prim_path=target_prim_path) @@ -100,36 +54,42 @@ while 1: if count % n_timesteps == 0: - feedback = interface.get_feedback() - target_xyz = np.array( - [ - np.random.random() * 0.5 - 0.25, - np.random.random() * 0.5 - 0.25, - np.random.random() * 0.5 + 0.5, - ] - ) - R = robot_config.R(ee_name, q=feedback["q"]) - target_orientation = transformations.euler_from_matrix(R, "sxyz") - # update the position of the target - interface.set_mocap_xyz("target", target_xyz) - - # can use 3 different methods to calculate inverse kinematics - # see inverse_kinematics.py file for details - path_planner.generate_path( - position=feedback["q"], - target_position=np.hstack([target_xyz, target_orientation]), - method=3, - dt=0.005, - n_timesteps=n_timesteps, - plot=False, - ) - - # returns desired [position, velocity] + feedback = interface.get_feedback() + target_xyz = np.array( + [ + np.random.random() * 0.5 - 0.25, + np.random.random() * 0.5 - 0.25, + np.random.random() * 0.5 + 0.5, + ] + ) + R = robot_config.R(interface.ee_link_name, q=feedback["q"]) + target_orientation = transformations.euler_from_matrix(R, "sxyz") + # update the position of the target + interface.set_xyz(target_prim_path, target_xyz) + + # can use 3 different methods to calculate inverse kinematics + # see inverse_kinematics.py file for details + print("feedback[q]: ", feedback["q"]) + print("target_xyz: ", target_xyz) + print("target_orientation: ", target_orientation) + path_planner.generate_path( + position=feedback["q"], + target_position=np.hstack([target_xyz, target_orientation]), + method=3, + dt=0.005, + n_timesteps=n_timesteps, + plot=False, + ) + print("after generate") + + # returns desired [position, velocity] target = path_planner.next()[0] # use position control interface.send_target_angles(target[: robot_config.N_JOINTS]) - interface.viewer.render() + interface.world.step(render=True) # execute one physics step and one rendering step + + #interface.viewer.render() count += 1 From 96b215106c51559106c62000e6ffaab846ea4e13 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Mon, 7 Jul 2025 09:53:21 -0400 Subject: [PATCH 22/42] fixed gains of arm joints, which were to stiff for force control --- abr_control/arms/isaacsim_config.py | 125 ++++----------- abr_control/interfaces/nv_isaacsim.py | 222 ++++++++++++++------------ 2 files changed, 156 insertions(+), 191 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 62c8f33b..06ec6698 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -127,7 +127,7 @@ def __init__(self, xml_file, folder=None, use_sim_state=True, force_download=Fal self.robot = xml_file self.use_sim_state = use_sim_state - def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path): + def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path, ee_link_name): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -153,6 +153,7 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.joint_pos_addrs = np.copy(joint_pos_addrs) self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path + self.ee_link_name = ee_link_name self.N_JOINTS = len(self.joint_vel_addrs) @@ -241,7 +242,7 @@ def dJ(self, name, q=None, dq=None, x=None): # general case, check differences.cpp' raise NotImplementedError - def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): + def J(self, name, q=None, x=None, object_type="body"): """Returns the Jacobian for the specified link, computed at the origin of the link's rigid body frame, which in Isaac Sim coincides with the link's center of mass. @@ -267,19 +268,13 @@ def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): First 3 rows: linear velocity Jacobian (Jv) Last 3 rows: angular velocity Jacobian (Jω) """ - + if name == "EE": name = self.ee_link_name # Check for unsupported features if x is not None and not np.allclose(x, 0): raise Exception("x offset currently not supported, set to None") - # Set controlled DOFs (default to first 6 for arm control) - if controlled_dofs is not None: - self.controlled_dof_indices = controlled_dofs - elif not hasattr(self, 'controlled_dof_indices'): - self.controlled_dof_indices = list(range(6)) # Default: first 6 DOFs - if object_type == "body": # Get Jacobians from articulation view jacobians = self.articulation_view.get_jacobians(clone=True) @@ -288,14 +283,11 @@ def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") + + #TODO remove of fix method used for UR5 + #link_index = self._get_link_index(name) # Get link index - # General - link_index = self.articulation_view.get_link_index("j2n6s300_end_effector") - # UR - #link_index = self.articulation_view.get_link_index("wrist_3_link") - #link_index = self.articulation_view.get_link_index("j2n6s300_end_effector") - print("name: ", name) - print("link index: ", link_index) + link_index = self.articulation_view.get_link_index(name) # Extract Jacobian for specific link env_idx = 0 # Assuming single environment @@ -313,13 +305,13 @@ def J(self, name, q=None, x=None, object_type="body", controlled_dofs=None): raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") # Extract only the columns for controllable DOFs - J = J_full[:, self.controlled_dof_indices] - + J = J_full[:, self.joint_pos_addrs] + # Verify dimensions if J.shape[0] != 6: raise RuntimeError(f"Expected Jacobian to have 6 rows, got {J.shape[0]}") - if J.shape[1] != len(self.controlled_dof_indices): - raise RuntimeError(f"Expected Jacobian to have {len(self.controlled_dof_indices)} columns, got {J.shape[1]}") + if J.shape[1] != len(self.joint_pos_addrs): + raise RuntimeError(f"Expected Jacobian to have {len(self.joint_pos_addrs)} columns, got {J.shape[1]}") # Assign to internal storage # Linear velocity Jacobian (first 3 rows) @@ -477,7 +469,6 @@ def R(self, name, q=None, object_type="body"): ''' if object_type == "body": - # Look for a link prim ending in the given name prim_path = self._get_prim_path(name) elif object_type in ["site", "geom"]: # Assume full path is given or fixed base path @@ -514,8 +505,7 @@ def quaternion(self, name, q=None): The joint angles of the robot. If None the current state is retrieved from the Mujoco simulator """ - #TODO outsource this to a common function and check is can be qued for EE and - # end_effector at the same time, or checked which is used for the current robot + if name == "EE": name = self.ee_link_name prim_path = self._get_prim_path(name) prim = self.stage.GetPrimAtPath(prim_path) @@ -561,7 +551,8 @@ def T(self, name, q=None, x=None): def Tx(self, name, q=None, x=None, object_type="body"): """Simplified version that only gets current position without state changes.""" - + if name == "EE": name = self.ee_link_name + #print(f"Tx: name={name}, q={q}, x={x}, object_type={object_type}") # Get prim path if object_type in ["body", "link"]: prim_path = self._get_prim_path(name) @@ -578,76 +569,11 @@ def Tx(self, name, q=None, x=None, object_type="body"): 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 Tx_old(self, name, q=None, x=None, object_type="body"): - """ Returns the world-frame Cartesian position of a named link, joint, or site. - - Parameters - ---------- - name : str - Name of the link, joint, or site (e.g., "j2n6s300_link_6"). - q : np.ndarray, optional - Joint positions to temporarily set before computing position. - object_type : str - "body" (link), "joint", or custom prim. - - Returns - ------- - np.ndarray - World position [x, y, z] of the object. - """ - ''' - if x is not None and not np.allclose(x, 0): - raise Exception("x offset currently not supported: ", x) - # Optionally set joint state - if not self.use_sim_state and q is not None: - old_q = self.articulation_view.get_joint_positions() - self.articulation_view.set_joint_positions(q) - ### self.world.step(render=False) - ''' - prim_path = None - #print(f"Tx: name={name}, q={q}, object_type={object_type}") - - #TODO similar to others like R, ousource - if object_type == "body": - prim_path = self._get_prim_path(name) - + return np.array([position[0], position[1], position[2]], dtype=np.float64) - elif object_type == "joint": - # Use joint index to find parent link's prim path - try: - joint_index = self.articulation_view.joint_names.index(name) - link_index = self.articulation_view.joint_parent_indices[joint_index] - prim_path = self.articulation_view._prim_paths[link_index] - except ValueError: - raise RuntimeError(f"Joint name '{name}' not found in articulation.") - else: - raise ValueError(f"Unsupported object_type: {object_type}") - if prim_path is None: - raise RuntimeError(f"Could not find prim for name '{name}' with type '{object_type}'.") - - # Get world transform matrix - prim = self.stage.GetPrimAtPath(prim_path) - if not prim.IsValid(): - raise RuntimeError(f"Invalid prim at path: {prim_path}") - - matrix = omni.usd.utils.get_world_transform_matrix(prim) - position = matrix.ExtractTranslation() - ''' - # Restore state if needed - if not self.use_sim_state and q is not None: - self.articulation_view.set_joint_positions(old_q) - ### self.world.step(render=False) - ''' - Tx =np.array([position[0], position[1], position[2]]) - return Tx def T_inv(self, name, q=None, x=None): """Get the inverse transform matrix of the specified body. @@ -666,10 +592,10 @@ def T_inv(self, name, q=None, x=None): # HELPER FUNCTIONS + # get the prim path for the name of the link, joint, or site def _get_prim_path(self, name): for prim in self.stage.Traverse(): - # Check if the prim path ends with the desired name - #print(f"Checking prim: {prim.GetPath()}") + #TODO could be more general to inlcude TCP etc if str(prim.GetPath()).endswith(name): return prim.GetPath() return None @@ -715,5 +641,18 @@ def set_joint_velocities(self, velocities): full_velocities[idx] = velocities[i] self.articulation.set_joint_velocities(full_velocities) + #TODO remove of fix method used for UR5 + def _get_link_index(self, name): + """Get the index of a link by its name""" + + import omni.isaac.core.utils.prims as prims_utils + #import omni.isaac.core.prims as prims_utils - \ No newline at end of file + prim_path = self._get_prim_path(name) + prim = prims_utils.get_prim_at_path(prim_path) + parent_prim = prims_utils.get_prim_parent(prim) + parent_name = parent_prim.GetName() + link_index = self.articulation_view.get_link_index(parent_name) + print(f"Link name: {name}, Prim path: {prim_path}, Parent name: {parent_name}, Link index: {link_index}") + + return link_index + 1 \ No newline at end of file diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index eb03b847..ac97668a 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -63,26 +63,23 @@ def connect(self, joint_names=None, camera_id=-1): #self.world.add_physics_callback("send_actions", self.send_actions) robot_path = None + # Load the robot from USD file if self.robot_config.robot == "ur5": robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd" #ee_path = self.create_ee_xform("wrist_3_link", np.array([-0.04, 0, 0])) - EE_parent_link = "wrist_3_link" # end-effector link for UR5 has_EE = False # UR5 has no end-effector - self.ee_link_name = "flange" #"ft_frame" # end-effector name for UR5 + EE_parent_link = "wrist_3_link" # end-effector link for UR5 + self.ee_link_name = "flange" # end-effector name for UR5 elif self.robot_config.robot == "jaco2": robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" - EE_link = "j2n6s300_end_effector" - EE_parent_link = "j2n6s300_link_6" # end-effector link for Jaco2 has_EE = True # Jaco2 has an end-effector - #omni.kit.commands.execute("MovePrim", path_from="/World/robot/j2n6s300_end_effector", path_to="/World/robot/EE") - #ee_path = self.create_ee_xform("j2n6s300_link_6", np.array([-0.04, 0, 0])) - self.ee_link_name = "end_effector" # end-effector name for Jaco2 + self.ee_link_name = "j2n6s300_end_effector" # end-effector name for Jaco2 elif self.robot_config.robot == "h1": robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" #TODO check this EE_parent_link = "link6" # end-effector link for H1 - + print(f"End Effector with name '{self.ee_link_name}' specified in UDS, using it ...") assets_root_path = get_assets_root_path() robot_usd_path = f"{assets_root_path}{robot_path}" @@ -127,8 +124,6 @@ def connect(self, joint_names=None, camera_id=-1): self.joint_vel_addrs = [] self.joint_dyn_addrs = [] - - if joint_names is None: print("No joint names provided, using all controllable joints in the articulation.") # Get all controllable joints in the articulation @@ -137,29 +132,28 @@ def connect(self, joint_names=None, camera_id=-1): # Handle joint name mapping joint_names = self._map_joint_names(joint_names) - - # Validate joint names and get indices all_joint_names = self.articulation.dof_names for name in joint_names: if name not in all_joint_names: raise Exception(f"Joint name {name} does not exist in robot model") + #TODO check if joint_idx is necessary joint_idx = all_joint_names.index(name) self.joint_pos_addrs.append(joint_idx) self.joint_vel_addrs.append(joint_idx) self.joint_dyn_addrs.append(joint_idx) + + ''' # is boolean flag necessary? if has_EE: - print("End Effector ...") - # 'EE_link = "j2n6s300_end_effector"' + print(f"End Effector with name '{self.ee_link_name}' specified in UDS, using it ...") else: print("No End Effector, creating EE Xform...") - #ee_info = self.initialize_robot_end_effector(self.prim_path, EE_parent_link, np.array([-0.04, 0, 0])) - #ee_path = self.create_ee_xform(EE_parent_link, np.array([-0.04, 0, 0])) + ee_path = self.create_ee_xform(EE_parent_link, np.array([-0.04, 0, 0])) #print("End Effector Info: ", ee_info) - + ''' # Connect robot config with simulation data @@ -174,6 +168,7 @@ def connect(self, joint_names=None, camera_id=-1): self.joint_pos_addrs, self.joint_vel_addrs, self.prim_path, + self.ee_link_name ) @@ -226,13 +221,7 @@ def _map_joint_names(self, joint_names): # If names are already in correct format, return as-is return joint_names - - - - - - - + def disconnect(self): """Any socket closing etc that must be done to properly shut down""" @@ -240,29 +229,46 @@ def disconnect(self): print("IsaacSim connection closed...") - + def send_forces(self, u): - """Applies the set of torques u to the arm.""" - + """Applies the set of torques u to the arm - now working correctly!""" # Create full torque vector for all DOFs - total_dofs = self.articulation_view.num_dof - full_torques = np.zeros(total_dofs) - + full_torques = np.zeros(self.robot_config.N_ALL_JOINTS) # Apply control torques to the controlled joints full_torques[:len(u)] = u - #print("U: ", 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_forces(self, u): + """Applies the set of torques u to the arm.""" + # Create full torque vector for all DOFs + full_torques = np.zeros(self.robot_config.N_ALL_JOINTS) + # Apply control torques to the controlled joints + full_torques[:len(u)] = u + #full_torques[len(u):] = u + #print("full_torques: ", full_torques ) + + + + + #self.articulation_view.set_joint_efforts(np.ones(total_dofs) * -10000) # Set the joint efforts (torques) #full_torques[:len(u)] = np.ones(len(u)) * -1000 #full_torques[1] = -1000 #self.articulation_view.set_joint_efforts(full_torques) # Set the joint efforts (torques) + # Apply the control signal + #self.articulation_view.set_effort_modes("force") + self.articulation_view.set_joint_efforts(full_torques) + # Move simulation ahead one time step self.world.step(render=True) - + ''' @@ -398,7 +404,7 @@ def send_actions(self, dt): - #TODO use for R and Tx in isaacsim_config + #TODO remove def get_prim_ends_with_name(self, name): prim_path = None for prim in self.stage.Traverse(): @@ -453,81 +459,101 @@ def create_target_prim(self, prim_path="/World/target_cube", position=np.array([ return cube_prim - #TODO remove - def find_EE(self, EE_name="end_effector"): - """Find the end-effector""" - - link_list = self.articulation_view.body_names + #TODO not sure why this is necessary + def fix_arm_joint_gains(self): + """Properly set gains for arm joints (DOFs 0-5)""" + n_dofs = self.robot_config.N_ALL_JOINTS - # Check if the articulation has an end-effector link - ee_link_name = None - for link_name in self.articulation_view.body_names: - if "end_effector" in link_name.lower() or "ee" in link_name.lower(): - ee_link_name = link_name - break + # Get current gains or set defaults + stiffness = np.ones(n_dofs) * 100.0 # Default high stiffness + damping = np.ones(n_dofs) * 10.0 # Default damping - if ee_link_name is None: - raise Exception("End Effector link not found in the articulation.") + # Set ARM joints (0-5) to zero stiffness for force control + arm_joint_indices = [0, 1, 2, 3, 4, 5] # These are your arm joints - # Return the name of the end-effector link - return ee_link_name - - - # Create an invisible Xform for EE (wrist_3_link for UR5) - def create_ee_xform(self, parent_link="link6", ee_offset=np.array([-0.04, 0, 0])): - """Create an invisible Xform node for End Effector queries + for idx in arm_joint_indices: + stiffness[idx] = 0.0 # Zero stiffness = force control + damping[idx] = 0.1 # Low damping for responsiveness - Parameters - ---------- - parent_link : str - Name of the final link - ee_offset : np.array - Offset from parent_link to EE point [x, y, z] - """ - from pxr import UsdGeom, Gf, UsdPhysics, Sdf + # Keep finger joints with some stiffness if you want them stable + finger_joint_indices = [6, 7, 8, 9, 10, 11] + for idx in finger_joint_indices: + stiffness[idx] = 50.0 # Moderate stiffness for fingers + damping[idx] = 5.0 # Moderate damping for fingers - # Create an Xform (transform node) for the EE - ee_path = f"{self.prim_path}/{parent_link}/EE" - print("ee path: ", ee_path) - ee_xform = UsdGeom.Xform.Define(self.stage, ee_path) + self.articulation_view.set_gains(stiffness, damping) + print("Fixed gains for arm joints (0-5)") - # Set position relative to parent_link - ee_xform.AddTranslateOp().Set(Gf.Vec3d(ee_offset[0], ee_offset[1], ee_offset[2])) - # Add minimal physics properties without creating a separate rigid body - # This makes it part of the parent link's rigid body - ee_xform.GetPrim().CreateAttribute("physics:collisionEnabled", Sdf.ValueTypeNames.Bool).Set(False) - - # Add a custom attribute to mark it as an end effector for easier identification - ee_xform.GetPrim().CreateAttribute("custom:isEndEffector", Sdf.ValueTypeNames.Bool).Set(True) - - return ee_path - def get_ee_link_info(self, parent_link="link6"): - """Get information about all links including the EE + def test_arm_joints_after_fix(self): + """Test arm joints after fixing gains""" + # First fix the gains + #self.fix_arm_joint_gains() - Returns - ------- - dict - Dictionary with link names as keys and indices as values - """ - if hasattr(self, 'articulation_view'): - # Get all existing link names - link_names = self.articulation_view.body_names + # Wait a moment for gains to take effect + for _ in range(10): + self.world.step(render=True) + + # Test each arm joint individually + for joint_idx in range(6): + print(f"\n--- Testing ARM Joint {joint_idx} ---") + + # Apply torque to this joint only + test_torques = np.zeros(self.robot_config.N_ALL_JOINTS) + test_torques[joint_idx] = 3.0 # 3 Nm torque + + initial_pos = self.articulation_view.get_joint_positions()[0] + + for _ in range(100): + self.articulation_view.set_joint_efforts(test_torques) + self.world.step(render=True) - # Check if EE exists as a child of link6 - ee_path = f"{self.prim_path}/{parent_link}/EE" - ee_prim = self.stage.GetPrimAtPath(ee_path) + final_pos = self.articulation_view.get_joint_positions()[0] + change = final_pos[joint_idx] - initial_pos[joint_idx] - if ee_prim.IsValid(): - # Add EE to the link names list - link_names.append("EE") - - # Create a mapping of names to indices - link_info = {name: idx for idx, name in enumerate(link_names)} - return link_info - - return {} + print(f" Joint {joint_idx} moved: {change:8.6f} rad ({np.degrees(change):6.2f}°)") + + # Reset position + self.articulation_view.set_joint_positions(initial_pos.reshape(1, -1)) + for _ in range(10): + self.world.step(render=True) + + + + def test_expected_coupling(self): + """Test that demonstrates expected dynamic coupling""" + + # Test 1: Joint 0 (base) should affect all joints + print("=== Joint 0 (Base) Coupling Test ===") + self.apply_torque_and_measure(joint_idx=0, torque=1.0) + + # Test 2: Joint 5 (wrist) should have minimal effect on others + print("\n=== Joint 5 (Wrist) Isolation Test ===") + self.apply_torque_and_measure(joint_idx=5, torque=1.0) + + # Test 3: Joint 2 (elbow) should affect downstream joints + print("\n=== Joint 2 (Elbow) Coupling Test ===") + self.apply_torque_and_measure(joint_idx=2, torque=1.0) + + + + def apply_torque_and_measure(self, joint_idx, torque): + initial_pos = self.articulation_view.get_joint_positions()[0][:6] + + for _ in range(10): + test_torques = np.zeros(self.robot_config.N_ALL_JOINTS) + test_torques[joint_idx] = torque + self.articulation_view.set_joint_efforts(test_torques.reshape(1, -1)) + self.world.step(render=True) + + final_pos = self.articulation_view.get_joint_positions()[0][:6] + change = final_pos - initial_pos + + print(f"Applied {torque} Nm to joint {joint_idx}") + print(f"Target joint moved: {change[joint_idx]:.6f} rad") + print(f"Max other joint movement: {np.max(np.abs(np.delete(change, joint_idx))):.6f} rad") + print(f"All changes: {change}") \ No newline at end of file From 04d54a8c9253797682f6b1b52503dffe5be2001c Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Mon, 7 Jul 2025 11:56:17 -0400 Subject: [PATCH 23/42] fixed def M --- abr_control/arms/isaacsim_config.py | 114 +++------------------------- 1 file changed, 11 insertions(+), 103 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 06ec6698..dd84de04 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -242,6 +242,8 @@ def dJ(self, name, q=None, dq=None, x=None): # general case, check differences.cpp' raise NotImplementedError + + def J(self, name, q=None, x=None, object_type="body"): """Returns the Jacobian for the specified link, computed at the origin of the link's rigid body frame, @@ -331,10 +333,9 @@ def J(self, name, q=None, x=None, object_type="body"): - def M(self, q=None, indices=None): + def M(self, q=None): """ - Returns the inertia matrix using Isaac Sim Core API - + Returns the inertia matrix for the controlled arm_joints. Args: q (np.ndarray, optional): Joint positions indices (optional): Specific articulation indices @@ -342,108 +343,15 @@ def M(self, q=None, indices=None): Returns: np.ndarray: Inertia matrix """ - M = self._compute_mass_matrix_numerical() - - # Restore original state if q was provided - #if q is not None: - # self.articulation.set_joint_positions(current_positions) - # self.world.step(render=False) - - return np.copy(M) - - - #TODO: CHECK AND IF NECEAARY remove this, use M instead - def _compute_mass_matrix_numerical(self): - """ - Compute mass matrix numerically using finite differences - This is a fallback method when direct mass matrix access is not available - """ - import numpy as np - - M = np.zeros((self.N_JOINTS, self.N_JOINTS)) - - # Small perturbation for finite differences - epsilon = 1e-6 - - # Get current state - current_pos = self.get_joint_positions() - current_vel = self.get_joint_velocities() - - # Zero velocities for clean computation - zero_vel = np.zeros_like(current_vel) - self.set_joint_velocities(zero_vel) - - # Compute each column of mass matrix - for i in range(self.N_JOINTS): - # Create unit acceleration in joint i - unit_accel = np.zeros(self.N_JOINTS) - unit_accel[i] = 1.0 - - # Compute required torques for this acceleration - # Using inverse dynamics: tau = M*qdd + C + G - tau = self._compute_inverse_dynamics(current_pos, zero_vel, unit_accel) - - # The torques give us the i-th column of the mass matrix - M[:, i] = tau - - return M - + # get_mass_matrices returns (num_envs, dof_count, dof_count) + M = self.articulation_view.get_mass_matrices() + # If you have only one robot in ArticulationView + M_full = M[0] + # extract only the controlled DOF + M_arm = M_full[:self.N_JOINTS, :self.N_JOINTS] + return np.copy(M_arm) - def _compute_inverse_dynamics(self, q, qd, qdd): - """ - Compute inverse dynamics: tau = M*qdd + C + G - This is an approximation using IsaacSim's physics engine - """ - ''' - # Method 1: Use PhysX articulation dynamics (if available) - try: - # Set desired accelerations and compute required forces - full_accelerations = np.zeros(self.robot.num_dof) - for i, idx in enumerate(self.joint_vel_addr): - full_accelerations[idx] = qdd[i] - - # Use articulation's compute_efforts method if available - if hasattr(self.robot, 'compute_efforts'): - efforts = self.robot.compute_efforts( - positions=self.robot.get_joint_positions(), - velocities=self.robot.get_joint_velocities(), - accelerations=full_accelerations - ) - return efforts[self.joint_vel_addr] - except: - pass - ''' - # Method 2: Numerical approximation - # Apply accelerations and measure required torques - dt = self.world.get_physics_dt() - - # Store current state - original_pos = self.get_joint_positions() - original_vel = self.get_joint_velocities() - - # Set desired state - self.set_joint_positions(q) - self.set_joint_velocities(qd) - - # Compute target velocities after acceleration - target_vel = qd + qdd * dt - - # Use PD control to estimate required torques - kp = 1000.0 # High proportional gain - kd = 100.0 # Damping - - pos_error = np.zeros_like(q) # No position error - vel_error = target_vel - qd - - tau = kp * pos_error + kd * vel_error - - # Restore original state - self.set_joint_positions(original_pos) - self.set_joint_velocities(original_vel) - - return tau - def R(self, name, q=None, object_type="body"): From 823aa6fb451592392c1130c44703277cd9463c40 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 8 Jul 2025 09:46:20 -0400 Subject: [PATCH 24/42] Example with force_osc working --- abr_control/arms/isaacsim_config.py | 94 ++++----- abr_control/interfaces/nv_isaacsim.py | 196 +----------------- examples/IsaacSim/force_osc_xyz.py | 166 +++++++++++++++ ...sition_joint_control_inverse_kinematics.py | 100 --------- 4 files changed, 213 insertions(+), 343 deletions(-) create mode 100644 examples/IsaacSim/force_osc_xyz.py delete mode 100644 examples/IsaacSim/position_joint_control_inverse_kinematics.py diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index dd84de04..c82a1b96 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -244,59 +244,41 @@ def dJ(self, name, q=None, dq=None, x=None): + def J(self, name, q=None, x=None, object_type="body"): - """Returns the Jacobian for the specified link, - computed at the origin of the link's rigid body frame, - which in Isaac Sim coincides with the link's center of mass. - - Parameters - ---------- - name: string - The name of the link/body to retrieve the Jacobian for - q: float numpy.array, optional (Default: None) - The joint angles of the robot. If None the current state is - retrieved from the Isaac Sim simulator - x: float numpy.array, optional (Default: None) - Offset from link origin (currently not supported) - object_type: string, optional (Default: "body") - The object type - options: body, geom, site - controlled_dofs: list, optional (Default: None) - List of DOF indices to include in Jacobian. If None, uses first 6 DOFs - - Returns - ------- - numpy.array - 6xN Jacobian matrix where N is number of controlled DOFs - First 3 rows: linear velocity Jacobian (Jv) - Last 3 rows: angular velocity Jacobian (Jω) - """ - if name == "EE": name = self.ee_link_name + if name == "EE": + name = self.ee_link_name # Check for unsupported features if x is not None and not np.allclose(x, 0): raise Exception("x offset currently not supported, set to None") - if object_type == "body": - # Get Jacobians from articulation view + # Get Jacobians from articulation view - ensure proper tensor handling jacobians = self.articulation_view.get_jacobians(clone=True) - + + # Convert to numpy if it's a tensor + if hasattr(jacobians, 'cpu'): + jacobians = jacobians.cpu().numpy() + elif hasattr(jacobians, 'numpy'): + jacobians = jacobians.numpy() + # Check if ArticulationView has any environments if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") - - #TODO remove of fix method used for UR5 - #link_index = self._get_link_index(name) # Get link index link_index = self.articulation_view.get_link_index(name) - + # Extract Jacobian for specific link env_idx = 0 # Assuming single environment - + # Handle different Jacobian tensor shapes + # Based on your diagnostic: shape is (1, 13, 6, 12) + # This means: (num_envs, num_bodies, 6_dof_per_body, total_dofs) if len(jacobians.shape) == 4: # Shape: (num_envs, num_bodies, 6, num_dofs) + # Get Jacobian for the end-effector link (link_index 7) J_full = jacobians[env_idx, link_index, :, :] elif len(jacobians.shape) == 3: # Shape: (num_envs, num_bodies * 6, num_dofs) @@ -305,22 +287,38 @@ def J(self, name, q=None, x=None, object_type="body"): J_full = jacobians[env_idx, start_row:end_row, :] else: raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") - - # Extract only the columns for controllable DOFs + + # For Jaco 2, we want only the first 6 DOFs (arm joints, not fingers) + # J_full is 6x12 (6 DOF end-effector w.r.t. 12 total DOFs) + # We only want columns 0-5 (the arm joints) + if len(self.joint_pos_addrs) != 6: + raise RuntimeError(f"Expected 6 joint addresses for robot arm, got {len(self.joint_pos_addrs)}") + + # Extract only the columns for controllable DOFs + # This gives us the end-effector Jacobian w.r.t. only the arm joints J = J_full[:, self.joint_pos_addrs] - + # Verify dimensions if J.shape[0] != 6: raise RuntimeError(f"Expected Jacobian to have 6 rows, got {J.shape[0]}") - if J.shape[1] != len(self.joint_pos_addrs): - raise RuntimeError(f"Expected Jacobian to have {len(self.joint_pos_addrs)} columns, got {J.shape[1]}") - + if J.shape[1] != 6: + raise RuntimeError(f"Expected Jacobian to have 6 columns for arm DOFs, got {J.shape[1]}") + + # Check for NaN or inf values + if np.any(np.isnan(J)) or np.any(np.isinf(J)): + raise RuntimeError("Jacobian contains NaN or infinite values") + # Assign to internal storage # Linear velocity Jacobian (first 3 rows) self._J6N[:3, :] = J[:3, :] - # Angular velocity Jacobian (last 3 rows) + # Angular velocity Jacobian (last 3 rows) self._J6N[3:, :] = J[3:, :] - + + # Additional validation - check if Jacobian makes sense + # For a 6-DOF arm, we should have non-zero values + if np.allclose(J, 0): + raise RuntimeError("Jacobian is all zeros - check robot configuration and joint addresses") + elif object_type == "geom": raise NotImplementedError("Calculating the Jacobian for 'geom' is not yet implemented.") elif object_type == "site": @@ -332,7 +330,6 @@ def J(self, name, q=None, x=None, object_type="body"): - def M(self, q=None): """ Returns the inertia matrix for the controlled arm_joints. @@ -352,8 +349,6 @@ def M(self, q=None): return np.copy(M_arm) - - def R(self, name, q=None, object_type="body"): """ Returns the rotation matrix of the specified object in Isaac Sim. @@ -367,14 +362,6 @@ def R(self, name, q=None, object_type="body"): object_type : str One of "body", "geom", or "site". """ - ''' - - print(f"R: {name}, q: {q}, object_type: {object_type}") - if not self.use_sim_state and q is not None: - old_q = self.articulation_view.get_joint_positions() - self.articulation_view.set_joint_positions(q) - #self._world.step(render=False) - ''' if object_type == "body": prim_path = self._get_prim_path(name) @@ -460,7 +447,6 @@ def T(self, name, q=None, x=None): def Tx(self, name, q=None, x=None, object_type="body"): """Simplified version that only gets current position without state changes.""" if name == "EE": name = self.ee_link_name - #print(f"Tx: name={name}, q={q}, x={x}, object_type={object_type}") # Get prim path if object_type in ["body", "link"]: prim_path = self._get_prim_path(name) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index ac97668a..16fb378d 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -21,15 +21,10 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.robot_config = robot_config self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called - self.prim_path = "/World/robot" self.name = self.robot_config.xml_dir.rsplit('/', 1)[-1] - - #self.misc_handles = {} # for tracking miscellaneous object handles - - def connect(self, joint_names=None, camera_id=-1): """ joint_names: list, optional (Default: None) @@ -38,21 +33,17 @@ def connect(self, joint_names=None, camera_id=-1): to the world are used """ - """All initial setup.""" self.simulation_app = SimulationApp({"headless": False}) import omni - import omni.kit.commands # type: ignore - import omni.isaac.core.utils.stage as stage_utils # type: ignore - from omni.isaac.core import World - #from omni.isaac.core.utils.stage import get_current_stage - from omni.isaac.core.utils.prims import get_prim_at_path + import omni.kit.commands # type: ignore + import omni.isaac.core.utils.stage as stage_utils # type: ignore + from omni.isaac.core import World # type: ignore from omni.isaac.core.articulations import Articulation, ArticulationView # type: ignore - from omni.isaac.core.utils.nucleus import get_assets_root_path + from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore #TODO change import #TODO is "Robot" even necessary - from isaacsim.core.api.robots import Robot - from pxr import UsdPhysics, UsdGeom + from isaacsim.core.api.robots import Robot # type: ignore # Initialize the simulation world self.world = World(stage_units_in_meters=1.0) @@ -79,29 +70,20 @@ def connect(self, joint_names=None, camera_id=-1): robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" #TODO check this EE_parent_link = "link6" # end-effector link for H1 + print(f"Robot '{self.name}' is loaded from UDS ...") print(f"End Effector with name '{self.ee_link_name}' specified in UDS, using it ...") assets_root_path = get_assets_root_path() robot_usd_path = f"{assets_root_path}{robot_path}" print(f"Loading robot from USD path: {robot_usd_path}") - stage_utils.add_reference_to_stage( usd_path=assets_root_path + robot_path, prim_path=self.prim_path, ) robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - ''' - self.robot = self.world.scene.add( - Articulation( - prim_path="/World/Robot", - name="robot", - usd_path=robot_usd_path, - position=[0, 0, 0] - ) - ) - ''' + self.world.reset() self.articulation = Articulation(prim_path=self.prim_path, name=self.name + "_articulation") self.articulation.initialize() @@ -173,32 +155,6 @@ def connect(self, joint_names=None, camera_id=-1): - - - ''' - def _get_kinematic_chain_joints(self, ee_link_name): - """ - Get joints in kinematic chain from end-effector to base - """ - - # For most robot arms, we can use all revolute joints - # This would need to be customized based on your specific robot - all_joints = self.articulation.dof_names - # Filter for revolute joints (exclude fixed joints) - kinematic_joints = [] - for joint_name in all_joints: - #joint_prim_path = f"/World/Robot/{joint_name}" - joint_prim_path = f"{self.prim_path}{joint_name}" - - joint_prim = get_prim_at_path(joint_prim_path) - if joint_prim and joint_prim.HasAPI(UsdPhysics.RevoluteJointAPI): - kinematic_joints.append(joint_name) - - return kinematic_joints - ''' - - - def _map_joint_names(self, joint_names): """ Map joint names from MuJoCo format to IsaacSim format @@ -221,7 +177,6 @@ def _map_joint_names(self, joint_names): # If names are already in correct format, return as-is return joint_names - def disconnect(self): """Any socket closing etc that must be done to properly shut down""" @@ -229,7 +184,6 @@ def disconnect(self): print("IsaacSim connection closed...") - def send_forces(self, u): """Applies the set of torques u to the arm - now working correctly!""" # Create full torque vector for all DOFs @@ -242,58 +196,16 @@ def send_forces(self, u): self.world.step(render=True) - ''' - def send_forces(self, u): - """Applies the set of torques u to the arm.""" - # Create full torque vector for all DOFs - full_torques = np.zeros(self.robot_config.N_ALL_JOINTS) - # Apply control torques to the controlled joints - full_torques[:len(u)] = u - #full_torques[len(u):] = u - #print("full_torques: ", full_torques ) - - - - - - #self.articulation_view.set_joint_efforts(np.ones(total_dofs) * -10000) # Set the joint efforts (torques) - - #full_torques[:len(u)] = np.ones(len(u)) * -1000 - #full_torques[1] = -1000 - #self.articulation_view.set_joint_efforts(full_torques) # Set the joint efforts (torques) - - # Apply the control signal - #self.articulation_view.set_effort_modes("force") - 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): """Moves the arm to the specified joint angles q : numpy.array the target joint angles [radians] - - # Check if the length of q is greater than the number of joints - if len(q) > self.robot_config.N_JOINTS: - q_new = q[:self.robot_config.N_JOINTS] - self.articulation_view.set_joint_positions(q_new) - elif len(q) < self.robot_config.N_ALL_JOINTS : - q_new = self.articulation_view.get_joint_positions() # Shape: (1, 12) - q_new[0, :len(q)] = q # Update first N_JOINTS for environment 0 - self.articulation_view.set_joint_positions(q_new) - else: - self.articulation_view.set_joint_positions(q) """ self.robot_config.set_joint_positions(q) self.world.step(render=True) - def get_feedback(self): """Return a dictionary of information needed by the controller. @@ -305,7 +217,6 @@ def get_feedback(self): return {"q": self.q, "dq": self.dq} - def get_xyz(self, prim_path): """Returns the xyz position of the specified object @@ -404,25 +315,6 @@ def send_actions(self, dt): - #TODO remove - def get_prim_ends_with_name(self, name): - prim_path = None - for prim in self.stage.Traverse(): - print("prim: ", prim) - print("prim.GetPath(): ", prim.GetPath()) - print("name: ", name) - if str(prim.GetPath()).endswith(name): - prim_path = prim.GetPath() - break - return prim_path - - - - - - - - # Create a visual-only cube (no collision) def create_target_prim(self, prim_path="/World/target_cube", position=np.array([0, 0, 1.0]), size = .1, color=np.array([0, 0, 1.0])): from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics # type: ignore @@ -459,8 +351,6 @@ def create_target_prim(self, prim_path="/World/target_cube", position=np.array([ return cube_prim - - #TODO not sure why this is necessary def fix_arm_joint_gains(self): """Properly set gains for arm joints (DOFs 0-5)""" @@ -485,75 +375,3 @@ def fix_arm_joint_gains(self): self.articulation_view.set_gains(stiffness, damping) print("Fixed gains for arm joints (0-5)") - - - - - def test_arm_joints_after_fix(self): - """Test arm joints after fixing gains""" - # First fix the gains - #self.fix_arm_joint_gains() - - # Wait a moment for gains to take effect - for _ in range(10): - self.world.step(render=True) - - # Test each arm joint individually - for joint_idx in range(6): - print(f"\n--- Testing ARM Joint {joint_idx} ---") - - # Apply torque to this joint only - test_torques = np.zeros(self.robot_config.N_ALL_JOINTS) - test_torques[joint_idx] = 3.0 # 3 Nm torque - - initial_pos = self.articulation_view.get_joint_positions()[0] - - for _ in range(100): - self.articulation_view.set_joint_efforts(test_torques) - self.world.step(render=True) - - final_pos = self.articulation_view.get_joint_positions()[0] - change = final_pos[joint_idx] - initial_pos[joint_idx] - - print(f" Joint {joint_idx} moved: {change:8.6f} rad ({np.degrees(change):6.2f}°)") - - # Reset position - self.articulation_view.set_joint_positions(initial_pos.reshape(1, -1)) - for _ in range(10): - self.world.step(render=True) - - - - def test_expected_coupling(self): - """Test that demonstrates expected dynamic coupling""" - - # Test 1: Joint 0 (base) should affect all joints - print("=== Joint 0 (Base) Coupling Test ===") - self.apply_torque_and_measure(joint_idx=0, torque=1.0) - - # Test 2: Joint 5 (wrist) should have minimal effect on others - print("\n=== Joint 5 (Wrist) Isolation Test ===") - self.apply_torque_and_measure(joint_idx=5, torque=1.0) - - # Test 3: Joint 2 (elbow) should affect downstream joints - print("\n=== Joint 2 (Elbow) Coupling Test ===") - self.apply_torque_and_measure(joint_idx=2, torque=1.0) - - - - def apply_torque_and_measure(self, joint_idx, torque): - initial_pos = self.articulation_view.get_joint_positions()[0][:6] - - for _ in range(10): - test_torques = np.zeros(self.robot_config.N_ALL_JOINTS) - test_torques[joint_idx] = torque - self.articulation_view.set_joint_efforts(test_torques.reshape(1, -1)) - self.world.step(render=True) - - final_pos = self.articulation_view.get_joint_positions()[0][:6] - change = final_pos - initial_pos - - print(f"Applied {torque} Nm to joint {joint_idx}") - print(f"Target joint moved: {change[joint_idx]:.6f} rad") - print(f"Max other joint movement: {np.max(np.abs(np.delete(change, joint_idx))):.6f} rad") - print(f"All changes: {change}") \ No newline at end of file diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py new file mode 100644 index 00000000..d5213ae1 --- /dev/null +++ b/examples/IsaacSim/force_osc_xyz.py @@ -0,0 +1,166 @@ +""" +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 +import time +last_time = time.time() + + +if len(sys.argv) > 1: + arm_model = sys.argv[1] +else: + arm_model = "jaco2" +robot_config = arm(arm_model) + +# create our IsaacSim interface +dt = 0.007 # 143 Hz +#dt = 0.001 # 1000 Hz + +target_prim_path="/World/target" +interface = IsaacSim(robot_config, dt=dt) +interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) + +interface.send_target_angles(robot_config.START_ANGLES) +isaac_target = interface.create_target_prim(prim_path=target_prim_path) +interface.fix_arm_joint_gains() + +print("joint_pos_addrs: ", interface.robot_config.joint_pos_addrs) + + +# 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] + # control (x, y, z) out of [x, y, z, alpha, beta, gamma] + ctrlr_dof=[True, True, True, False, False, False], +) + +# set up lists for tracking data +ee_track = [] +target_track = [] + +#target_geom = "target" +green = [0, 0.9, 0, 0.5] +red = [0.9, 0, 0, 0.5] + +np.random.seed(0) +def gen_target(interface): + target_xyz = (np.random.rand(3) + np.array([-0.5, -0.5, 0.5])) * np.array( + [1, 1, 0.5] + ) + interface.set_xyz(target_prim_path, target_xyz) + +try: + # get the end-effector's initial position + feedback = interface.get_feedback() + start = robot_config.Tx(interface.ee_link_name, feedback["q"]) + + # make the target offset from that start position + gen_target(interface) + + count = 0 + debug = 0 + print("\nSimulation starting...\n") + while 1: + current_time = time.time() + dt = current_time - last_time + print(f"Control dt: {dt:.6f}s, Freq: {1/dt:.1f}Hz") + last_time = current_time + # get joint angle and velocity feedback + feedback = interface.get_feedback() + + target = np.hstack( + [ + interface.get_xyz(target_prim_path), + transformations.euler_from_quaternion( + interface.get_orientation(target_prim_path), "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(interface.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.02: + # interface.model.geom(target_geom).rgba = green + count += 1 + else: + count = 0 + # interface.model.geom(target_geom).rgba = red + + if count >= 50: + print("Generating a new target") + gen_target(interface) + 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() diff --git a/examples/IsaacSim/position_joint_control_inverse_kinematics.py b/examples/IsaacSim/position_joint_control_inverse_kinematics.py deleted file mode 100644 index 381c58e0..00000000 --- a/examples/IsaacSim/position_joint_control_inverse_kinematics.py +++ /dev/null @@ -1,100 +0,0 @@ -""" -Running the joint controller with an inverse kinematics path planner -for a Mujoco simulation. The path planning system will generate -a trajectory in joint space that moves the end effector in a straight line -to the target, which changes every n time steps. -""" -import numpy as np - -#from abr_control.arms.mujoco_config import MujocoConfig as arm -from abr_control.arms.isaacsim_config import IsaacsimConfig as arm -from abr_control.controllers import path_planners -#from abr_control.interfaces.mujoco import Mujoco -from abr_control.interfaces.nv_isaacsim import IsaacSim -from abr_control.utils import transformations -from flying_cube import FlyingCube - - -# initialize our robot config for the jaco2 -robot_config = arm("jaco2", use_sim_state=False) - -# create our path planner -n_timesteps = 2000 -path_planner = path_planners.InverseKinematics(robot_config) - -# create our interface -dt = 0.001 -interface = IsaacSim(robot_config, dt=dt) -# interface.connect() -# interface.send_target_angles(robot_config.START_ANGLES) - - -n_dof_to_control = 6 -interface.connect(joint_names=[f"joint{ii}" for ii in range(n_dof_to_control)]) -interface.send_target_angles(robot_config.START_ANGLES[:n_dof_to_control]) - - -feedback = interface.get_feedback() -target_prim_path="/World/target" -isaac_target = interface.create_target_prim(prim_path=target_prim_path) - - - - - - - - - -try: - print("\nSimulation starting...") - print("Click to move the target.\n") - - count = 0 - while 1: - - if count % n_timesteps == 0: - feedback = interface.get_feedback() - target_xyz = np.array( - [ - np.random.random() * 0.5 - 0.25, - np.random.random() * 0.5 - 0.25, - np.random.random() * 0.5 + 0.5, - ] - ) - R = robot_config.R(interface.ee_link_name, q=feedback["q"]) - target_orientation = transformations.euler_from_matrix(R, "sxyz") - # update the position of the target - interface.set_xyz(target_prim_path, target_xyz) - - # can use 3 different methods to calculate inverse kinematics - # see inverse_kinematics.py file for details - print("feedback[q]: ", feedback["q"]) - print("target_xyz: ", target_xyz) - print("target_orientation: ", target_orientation) - path_planner.generate_path( - position=feedback["q"], - target_position=np.hstack([target_xyz, target_orientation]), - method=3, - dt=0.005, - n_timesteps=n_timesteps, - plot=False, - ) - print("after generate") - - # returns desired [position, velocity] - target = path_planner.next()[0] - - # use position control - interface.send_target_angles(target[: robot_config.N_JOINTS]) - interface.world.step(render=True) # execute one physics step and one rendering step - - #interface.viewer.render() - - count += 1 - -finally: - # stop and reset the simulation - interface.disconnect() - - print("Simulation terminated...") \ No newline at end of file From c2593a2d61215cd53bf8901f154b9effb1f86436 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 8 Jul 2025 13:41:53 -0400 Subject: [PATCH 25/42] added example linear path gaussian velocity --- abr_control/arms/isaacsim_config.py | 159 +++++------- abr_control/interfaces/nv_isaacsim.py | 182 +++++++------- examples/IsaacSim/force_osc_xyz.py | 21 +- ...c_xyz_linear_path_gaussian_velocity_WIP.py | 232 ++++++++++++++++++ 4 files changed, 381 insertions(+), 213 deletions(-) create mode 100644 examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity_WIP.py diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index c82a1b96..31d88b12 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -1,10 +1,13 @@ -import os from xml.etree import ElementTree import numpy as np from abr_control.utils import download_meshes import omni + + + + class IsaacsimConfig: """A wrapper on the Mujoco simulator to generate all the kinematics and dynamics calculations necessary for controllers. @@ -37,7 +40,7 @@ class IsaacsimConfig: - def __init__(self, xml_file, folder=None, use_sim_state=True, force_download=False): + def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=False): """Loads the Isaacsim model from the specified xml file Parameters @@ -71,63 +74,35 @@ def __init__(self, xml_file, folder=None, use_sim_state=True, force_download=Fal False: if the meshes folder is missing it will ask the user whether they want to download them """ - if folder is None: - arm_dir = xml_file.split("_")[0] - current_dir = os.path.dirname(__file__) - self.xml_file = os.path.join(current_dir, arm_dir, f"{xml_file}.xml") - self.xml_dir = f"{current_dir}/{arm_dir}" - else: - self.xml_dir = f"{folder}" - self.xml_file = os.path.join(self.xml_dir, xml_file) - - self.N_GRIPPER_JOINTS = 0 - - # get access to some of our custom arm parameters from the xml definition - tree = ElementTree.parse(self.xml_file) - root = tree.getroot() - for custom in root.findall("custom/numeric"): - name = custom.get("name") - if name == "START_ANGLES": - START_ANGLES = custom.get("data").split(" ") - self.START_ANGLES = np.array([float(angle) for angle in START_ANGLES]) - elif name == "N_GRIPPER_JOINTS": - self.N_GRIPPER_JOINTS = int(custom.get("data")) - - # check for google_id specifying download location of robot mesh files - self.google_id = None - for custom in root.findall("custom/text"): - name = custom.get("name") - if name == "google_id": - self.google_id = custom.get("data") - - actuators = root.find(f'actuator') - self.joint_names = [actuator.get("joint") for actuator in actuators] - - # check if the user has downloaded the required mesh files - # if not prompt them to do so - if self.google_id is not None: - # get list of expected files to check if all have been downloaded - files = [] - for asset in root.findall("asset/mesh"): - files.append(asset.get("file")) - - for asset in root.findall("asset/texture"): - # assuming that texture are placed in the meshes folder - files.append(asset.get("file").split("/")[1]) - - # check if our mesh folder exists, then check we have all the files - download_meshes.check_and_download( - name=self.xml_dir + "/meshes", - google_id=self.google_id, - force_download=force_download, - files=files, - ) - - #TODO fix above to get away from xml - self.robot = xml_file + self.robot_type = robot_type + #TODO obsolete, from mujoco self.use_sim_state = use_sim_state + + self.robot_path = None + + # Load the robot from USD file + if self.robot_type == "ur5": + self.robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd" + has_EE = False # UR5 has no end-effector + EE_parent_link = "wrist_3_link" # end-effector link for UR5 + self.ee_link_name = "flange" # end-effector name for UR5 + START_ANGLES = "0 -.67 -.67 0 0 0" + + elif self.robot_type == "jaco2": + self.robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" + self.ee_link_name = "j2n6s300_end_effector" # end-effector name for Jaco2 + START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" + + elif self.robot_type == "h1": + self.robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" + self.ee_link_name = "EE" + START_ANGLES = "0 0 0 0" - def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path, ee_link_name): + self.START_ANGLES = np.array(START_ANGLES.split(), dtype=float) + + + + def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -153,14 +128,9 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.joint_pos_addrs = np.copy(joint_pos_addrs) self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path - self.ee_link_name = ee_link_name - - self.N_JOINTS = len(self.joint_vel_addrs) - print (f"Number of controllable joints: {self.N_JOINTS}") # number of joints in the IsaacSim simulation N_ALL_JOINTS = self.articulation_view.num_dof - print (f"Number of ALL joints in simulation: {N_ALL_JOINTS}") # need to calculate the joint_vel_addrs indices in flat vectors returned # for the Jacobian @@ -216,7 +186,7 @@ def g(self, q=None): # If q is provided, ensure g matches the size of q if len(g_full) != len(q): g_full = g_full[:len(q)] - + return -g_full @@ -267,8 +237,8 @@ def J(self, name, q=None, x=None, object_type="body"): if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") - # Get link index - link_index = self.articulation_view.get_link_index(name) + #link_index = self.articulation_view.get_link_index(name) + link_index = self.safe_get_link(name, default=6) # Extract Jacobian for specific link env_idx = 0 # Assuming single environment @@ -279,6 +249,7 @@ def J(self, name, q=None, x=None, object_type="body"): if len(jacobians.shape) == 4: # Shape: (num_envs, num_bodies, 6, num_dofs) # Get Jacobian for the end-effector link (link_index 7) + #print("link_index: ", link_index) J_full = jacobians[env_idx, link_index, :, :] elif len(jacobians.shape) == 3: # Shape: (num_envs, num_bodies * 6, num_dofs) @@ -288,22 +259,10 @@ def J(self, name, q=None, x=None, object_type="body"): else: raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") - # For Jaco 2, we want only the first 6 DOFs (arm joints, not fingers) - # J_full is 6x12 (6 DOF end-effector w.r.t. 12 total DOFs) - # We only want columns 0-5 (the arm joints) - if len(self.joint_pos_addrs) != 6: - raise RuntimeError(f"Expected 6 joint addresses for robot arm, got {len(self.joint_pos_addrs)}") - # Extract only the columns for controllable DOFs # This gives us the end-effector Jacobian w.r.t. only the arm joints J = J_full[:, self.joint_pos_addrs] - - # Verify dimensions - if J.shape[0] != 6: - raise RuntimeError(f"Expected Jacobian to have 6 rows, got {J.shape[0]}") - if J.shape[1] != 6: - raise RuntimeError(f"Expected Jacobian to have 6 columns for arm DOFs, got {J.shape[1]}") - + # Check for NaN or inf values if np.any(np.isnan(J)) or np.any(np.isinf(J)): raise RuntimeError("Jacobian contains NaN or infinite values") @@ -314,8 +273,7 @@ def J(self, name, q=None, x=None, object_type="body"): # Angular velocity Jacobian (last 3 rows) self._J6N[3:, :] = J[3:, :] - # Additional validation - check if Jacobian makes sense - # For a 6-DOF arm, we should have non-zero values + # Additional validation - check if Jacobian makes sense (non-zero values) if np.allclose(J, 0): raise RuntimeError("Jacobian is all zeros - check robot configuration and joint addresses") @@ -329,7 +287,6 @@ def J(self, name, q=None, x=None, object_type="body"): return np.copy(self._J6N) - def M(self, q=None): """ Returns the inertia matrix for the controlled arm_joints. @@ -408,9 +365,7 @@ def quaternion(self, name, q=None): # Get 4x4 world transform matrix matrix = omni.usd.get_world_transform_matrix(prim) - from pxr import Gf - # Extract quaternion (as Gf.Quatf or Gf.Quatd) - quat = matrix.ExtractRotationQuat() # returns Gf.Quatd or Gf.Quatf + quat = matrix.ExtractRotationQuat() # Convert to [w, x, y, z] NumPy array quat_np = np.array([quat.GetReal(), *quat.GetImaginary()]) @@ -444,6 +399,7 @@ def T(self, name, q=None, x=None): # TODO if ever required raise NotImplementedError + def Tx(self, name, q=None, x=None, object_type="body"): """Simplified version that only gets current position without state changes.""" if name == "EE": name = self.ee_link_name @@ -467,8 +423,6 @@ def Tx(self, name, q=None, x=None, object_type="body"): return np.array([position[0], position[1], position[2]], dtype=np.float64) - - def T_inv(self, name, q=None, x=None): """Get the inverse transform matrix of the specified body. @@ -508,31 +462,23 @@ def get_joint_velocities(self): all_velocities = self.articulation.get_joint_velocities() return all_velocities[self.joint_vel_addrs] return None - ''' - def set_joint_positions(self, positions): - """Set joint positions""" - if hasattr(self, 'articulation'): - full_positions = self.articulation.get_joint_positions() - for i, idx in enumerate(range(len(positions))): - full_positions[idx] = positions[i] - self.articulation.set_joint_positions(full_positions) + - ''' - def set_joint_positions(self, positions): + def set_joint_positions(self, q): """Set joint positions""" if hasattr(self, 'articulation'): full_positions = self.articulation.get_joint_positions() - for i, idx in enumerate(self.joint_vel_addrs): - full_positions[idx] = positions[i] + for i, idx in enumerate(range(len(q))): + full_positions[idx] = q[i] self.articulation.set_joint_positions(full_positions) - def set_joint_velocities(self, velocities): + def set_joint_velocities(self, dq): """Set joint velocities""" if hasattr(self, 'articulation'): full_velocities = self.articulation.get_joint_velocities() - for i, idx in enumerate(self.joint_vel_addrs): - full_velocities[idx] = velocities[i] + for i, idx in enumerate(range(len(dq))): + full_velocities[idx] = dq[i] self.articulation.set_joint_velocities(full_velocities) #TODO remove of fix method used for UR5 @@ -540,7 +486,6 @@ def _get_link_index(self, name): """Get the index of a link by its name""" import omni.isaac.core.utils.prims as prims_utils - #import omni.isaac.core.prims as prims_utils prim_path = self._get_prim_path(name) prim = prims_utils.get_prim_at_path(prim_path) @@ -549,4 +494,12 @@ def _get_link_index(self, name): link_index = self.articulation_view.get_link_index(parent_name) print(f"Link name: {name}, Prim path: {prim_path}, Parent name: {parent_name}, Link index: {link_index}") - return link_index + 1 \ No newline at end of file + return link_index + 1 + + + def safe_get_link(self, name, default=None): + try: + return self.articulation_view.get_link_index(name) + except KeyError: + print(f"Link '{name}' not found, using default: {default}") + return default diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 16fb378d..4bdcdb69 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -1,6 +1,19 @@ import numpy as np from isaacsim import SimulationApp from .interface import Interface +simulation_app = SimulationApp({"headless": False}) +import omni +import omni.kit.commands # type: ignore +import omni.isaac.core.utils.stage as stage_utils # type: ignore +from omni.isaac.core import World # type: ignore +from omni.isaac.core.articulations import Articulation, ArticulationView # type: ignore +#TODO change import +#TODO is "Robot" even necessary +from isaacsim.core.api.robots import Robot # type: ignore +from pxr import UsdGeom, Gf, UsdShade, Sdf # type: ignore +import omni.isaac.core.utils.stage as stage_utils # type: ignore +from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore + class IsaacSim(Interface): @@ -22,7 +35,7 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.dt = dt # time step self.count = 0 # keep track of how many times send forces is called self.prim_path = "/World/robot" - self.name = self.robot_config.xml_dir.rsplit('/', 1)[-1] + self.name = self.robot_config.robot_type def connect(self, joint_names=None, camera_id=-1): @@ -34,17 +47,6 @@ def connect(self, joint_names=None, camera_id=-1): """ """All initial setup.""" - self.simulation_app = SimulationApp({"headless": False}) - import omni - import omni.kit.commands # type: ignore - import omni.isaac.core.utils.stage as stage_utils # type: ignore - from omni.isaac.core import World # type: ignore - from omni.isaac.core.articulations import Articulation, ArticulationView # type: ignore - from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore - #TODO change import - #TODO is "Robot" even necessary - from isaacsim.core.api.robots import Robot # type: ignore - # Initialize the simulation world self.world = World(stage_units_in_meters=1.0) self.world.scene.add_default_ground_plane() @@ -52,37 +54,22 @@ def connect(self, joint_names=None, camera_id=-1): self.stage = self.context.get_stage() #TODO necessary for H1 robot #self.world.add_physics_callback("send_actions", self.send_actions) - - robot_path = None - - # Load the robot from USD file - if self.robot_config.robot == "ur5": - robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd" - #ee_path = self.create_ee_xform("wrist_3_link", np.array([-0.04, 0, 0])) - has_EE = False # UR5 has no end-effector - EE_parent_link = "wrist_3_link" # end-effector link for UR5 - self.ee_link_name = "flange" # end-effector name for UR5 - elif self.robot_config.robot == "jaco2": - robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" - has_EE = True # Jaco2 has an end-effector - self.ee_link_name = "j2n6s300_end_effector" # end-effector name for Jaco2 - elif self.robot_config.robot == "h1": - robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" - #TODO check this - EE_parent_link = "link6" # end-effector link for H1 - print(f"Robot '{self.name}' is loaded from UDS ...") - print(f"End Effector with name '{self.ee_link_name}' specified in UDS, using it ...") + - assets_root_path = get_assets_root_path() - robot_usd_path = f"{assets_root_path}{robot_path}" - print(f"Loading robot from USD path: {robot_usd_path}") - + 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}") + print(f"End Effector with name '{self.robot_config.ee_link_name}' specified in UDS, using it ...") + + + stage_utils.add_reference_to_stage( - usd_path=assets_root_path + robot_path, - prim_path=self.prim_path, - ) + usd_path=robot_usd_path, + prim_path=self.prim_path, + ) robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) + self.world.reset() self.articulation = Articulation(prim_path=self.prim_path, name=self.name + "_articulation") @@ -137,11 +124,8 @@ def connect(self, joint_names=None, camera_id=-1): #print("End Effector Info: ", ee_info) ''' - # Connect robot config with simulation data print("Connecting to robot config...") - print("Joint Position Addresses: ", self.joint_pos_addrs) - print("Joint Velocity Addresses: ", self.joint_vel_addrs) self.robot_config._connect( self.world, self.stage, @@ -150,11 +134,9 @@ def connect(self, joint_names=None, camera_id=-1): self.joint_pos_addrs, self.joint_vel_addrs, self.prim_path, - self.ee_link_name ) - def _map_joint_names(self, joint_names): """ Map joint names from MuJoCo format to IsaacSim format @@ -243,7 +225,6 @@ def get_orientation(self, prim_path): def get_transform(self, prim_path): - from pxr import Usd, UsdGeom, Gf _cube = self.stage.GetPrimAtPath(prim_path) # Check if it's an Xformable if not _cube.IsValid() or not UsdGeom.Xformable(_cube): @@ -256,49 +237,15 @@ def get_transform(self, prim_path): return transform_matrix - - #TODO change method name in 'set_named_prim' or something as mocap is mujoco thing - def set_mocap_xyz(self, name, xyz): - """ - Set the world position of a named prim (used like a mocap target). - - Parameters - ---------- - name : str - Name of the prim (e.g. site or target object) - xyz : np.ndarray - Target world position [x, y, z] in meters - """ - - world_path = "/World" - prim_path = f"{world_path}/{name}" - print("prim_path:", prim_path) - prim = self.stage.GetPrimAtPath(prim_path) - - prim.set_world_pose(xyz, np.array([0., 0., 0., 1.])) # set the position and orientation of the object - - - print("prim_path:", prim_path) - #from omni.isaac.core.utils.prims import set_prim_world_position - #set_prim_world_position(prim_path, xyz) - - - - - #TODO remove as is the same as above def set_xyz(self, prim_path, xyz, orientation=np.array([0., 0., 0., 1.])): """Set the position of an object in the environment. - name : string - the name of the object + prim_path : string + the prim_path of the object xyz : np.array the [x,y,z] location of the target [meters] - """ - from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics # type: ignore - + """ _cube = self.stage.GetPrimAtPath(prim_path) - #_cube.set_world_pose(xyz, orientation) # set the position and orientation of the object - xformable = UsdGeom.Xformable(_cube) transform_matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(xyz[0], xyz[1], xyz[2])) xformable.MakeMatrixXform().Set(transform_matrix) @@ -306,8 +253,7 @@ def set_xyz(self, prim_path, xyz, orientation=np.array([0., 0., 0., 1.])): # method for keep_standing def send_actions(self, dt): - pelvis_prim_path = '/World/robot/pelvis' - from pxr import Gf # type: ignore + pelvis_prim_path = '/World/robot/pelvis' prim=self.stage.GetPrimAtPath(pelvis_prim_path) 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 ,0.02)) @@ -316,9 +262,7 @@ def send_actions(self, dt): # Create a visual-only cube (no collision) - def create_target_prim(self, prim_path="/World/target_cube", position=np.array([0, 0, 1.0]), size = .1, color=np.array([0, 0, 1.0])): - from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics # type: ignore - + def create_target_prim(self, prim_path="/World/target_cube", position=np.array([0, 0, 1.0]), size = .1, color=np.array([0, 0, 1.0])): # Create cube geometry cube_prim = UsdGeom.Cube.Define(self.stage, prim_path) cube_prim.CreateSizeAttr(size) # Unit cube @@ -352,26 +296,64 @@ def create_target_prim(self, prim_path="/World/target_cube", position=np.array([ #TODO not sure why this is necessary - def fix_arm_joint_gains(self): - """Properly set gains for arm joints (DOFs 0-5)""" - n_dofs = self.robot_config.N_ALL_JOINTS + def set_gains_force_control(self): + """Properly set gains for arm joints (DOFs 0-5) and finger joints if present""" # Get current gains or set defaults - stiffness = np.ones(n_dofs) * 100.0 # Default high stiffness - damping = np.ones(n_dofs) * 10.0 # Default damping - - # Set ARM joints (0-5) to zero stiffness for force control - arm_joint_indices = [0, 1, 2, 3, 4, 5] # These are your arm joints + stiffness = np.ones(self.robot_config.N_ALL_JOINTS) * 100.0 # Default high stiffness + damping = np.ones(self.robot_config.N_ALL_JOINTS) * 10.0 # Default damping + # Set controlled arm joints to zero stiffness for force control + arm_joint_indices = list(range(self.robot_config.N_JOINTS)) + for idx in arm_joint_indices: stiffness[idx] = 0.0 # Zero stiffness = force control damping[idx] = 0.1 # Low damping for responsiveness # Keep finger joints with some stiffness if you want them stable - finger_joint_indices = [6, 7, 8, 9, 10, 11] - for idx in finger_joint_indices: - stiffness[idx] = 50.0 # Moderate stiffness for fingers - damping[idx] = 5.0 # Moderate damping for fingers + if self.robot_config.N_ALL_JOINTS > self.robot_config.N_JOINTS: + dof_names = self.articulation_view.dof_names + finger_joint_indices = list(range(self.robot_config.N_JOINTS, self.robot_config.N_ALL_JOINTS)) + for idx in finger_joint_indices: + if "finger" in dof_names[idx].lower(): + stiffness[idx] = 50.0 # Moderate stiffness for fingers + damping[idx] = 5.0 # Moderate damping for fingers + + self.articulation_view.set_gains(stiffness, damping) + print(f"Set gains for force control for arm joints {arm_joint_indices }") + + + + def set_gains_position_control_min(self): + """Set up position control with high stiffness""" + n_dofs = self.robot_config.N_JOINTS + stiffness = np.ones(n_dofs) * 1000.0 # High stiffness for position control + damping = np.ones(n_dofs) * 100.0 # High damping for stability + + self.articulation_view.set_gains(stiffness, damping) + + + def set_gains_position_control(self): + """Properly set gains for arm joints for position control and finger joints if present""" + # Get current gains or set defaults + stiffness = np.ones(self.robot_config.N_ALL_JOINTS) * 100.0 # Default high stiffness + damping = np.ones(self.robot_config.N_ALL_JOINTS) * 10.0 # Default damping + + # Set controlled arm joints to HIGH stiffness for position control + arm_joint_indices = list(range(self.robot_config.N_JOINTS)) + for idx in arm_joint_indices: + stiffness[idx] = 1000.0 # High stiffness = position control + damping[idx] = 100.0 # High damping for stability + + # Keep finger joints with some stiffness if you want them stable + if self.robot_config.N_ALL_JOINTS > self.robot_config.N_JOINTS: + dof_names = self.articulation_view.dof_names + finger_joint_indices = list(range(self.robot_config.N_JOINTS, self.robot_config.N_ALL_JOINTS)) + for idx in finger_joint_indices: + if "finger" in dof_names[idx].lower(): + stiffness[idx] = 50.0 # Moderate stiffness for fingers + damping[idx] = 5.0 # Moderate damping for fingers self.articulation_view.set_gains(stiffness, damping) - print("Fixed gains for arm joints (0-5)") + print(f"Set gains for position control for arm joints {arm_joint_indices }") + diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index d5213ae1..8d291cc0 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -20,19 +20,22 @@ arm_model = "jaco2" robot_config = arm(arm_model) -# create our IsaacSim interface dt = 0.007 # 143 Hz #dt = 0.001 # 1000 Hz - target_prim_path="/World/target" + +# create our IsaacSim interface interface = IsaacSim(robot_config, dt=dt) + interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) +#dof_names = interface.articulation.dof_names +#print("dof_names: " + dof_names) interface.send_target_angles(robot_config.START_ANGLES) isaac_target = interface.create_target_prim(prim_path=target_prim_path) -interface.fix_arm_joint_gains() -print("joint_pos_addrs: ", interface.robot_config.joint_pos_addrs) + +interface.set_gains_force_control() # damp the movements of the arm @@ -65,18 +68,17 @@ def gen_target(interface): try: # get the end-effector's initial position feedback = interface.get_feedback() - start = robot_config.Tx(interface.ee_link_name, feedback["q"]) + start = robot_config.Tx(robot_config.ee_link_name, feedback["q"]) # make the target offset from that start position gen_target(interface) count = 0 - debug = 0 print("\nSimulation starting...\n") while 1: current_time = time.time() dt = current_time - last_time - print(f"Control dt: {dt:.6f}s, Freq: {1/dt:.1f}Hz") + #print(f"Control dt: {dt:.6f}s, Freq: {1/dt:.1f}Hz") last_time = current_time # get joint angle and velocity feedback feedback = interface.get_feedback() @@ -96,12 +98,11 @@ def gen_target(interface): dq=feedback["dq"], target=target, ) - interface.send_forces(u) # calculate end-effector position - ee_xyz = robot_config.Tx(interface.ee_link_name, q=feedback["q"]) + 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])) @@ -124,7 +125,7 @@ def gen_target(interface): finally: # stop and reset the IsaacSim simulation - interface.disconnect() + #interface.disconnect() print("Simulation terminated...") diff --git a/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity_WIP.py b/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity_WIP.py new file mode 100644 index 00000000..95afc6df --- /dev/null +++ b/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity_WIP.py @@ -0,0 +1,232 @@ +""" +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" +# initialize our robot config for the jaco2 +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.007 # 143 Hz +#dt = 0.001 # 1000 Hz +target_prim_path="/World/target" + +# create our interface +interface = IsaacSim(robot_config, dt=dt) +interface.connect() +interface.send_target_angles(robot_config.START_ANGLES) +isaac_target = interface.create_target_prim(prim_path=target_prim_path) +interface.fix_arm_joint_gains() + +# 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("EE", 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("EE", 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("EE", 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("EE", 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() From 1da42b93b96de8654f73e55ae4233fda08e711ca Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 10 Jul 2025 13:57:02 -0400 Subject: [PATCH 26/42] adaptations for no EE --- abr_control/arms/isaacsim_config.py | 71 ++++++++--- abr_control/interfaces/nv_isaacsim.py | 162 +++++++++++++++++++++++--- examples/IsaacSim/force_osc_xyz.py | 5 +- 3 files changed, 205 insertions(+), 33 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 31d88b12..dc51a00d 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -79,24 +79,28 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.use_sim_state = use_sim_state self.robot_path = None + self.ee_link_name = "EE" # name used for virtual end-effector, overwritten if one exists already - # Load the robot from USD file if self.robot_type == "ur5": self.robot_path = "/Isaac/Robots/UniversalRobots/ur5/ur5.usd" - has_EE = False # UR5 has no end-effector - EE_parent_link = "wrist_3_link" # end-effector link for UR5 - self.ee_link_name = "flange" # end-effector name for UR5 + self.has_EE = False # UR5 has no end-effector + self.EE_parent_link = "wrist_3_link" START_ANGLES = "0 -.67 -.67 0 0 0" + print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "jaco2": self.robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" - self.ee_link_name = "j2n6s300_end_effector" # end-effector name for Jaco2 + self.has_EE = True # jaco2 has an end-effector + self.ee_link_name = "j2n6s300_end_effector" START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" + print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") elif self.robot_type == "h1": self.robot_path = "/Isaac/Robots/Unitree/H1/h1.usd" - self.ee_link_name = "EE" + self.has_EE = False # H1 has no end-effector + self.EE_parent_link = "right_elbow_link" START_ANGLES = "0 0 0 0" + print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") self.START_ANGLES = np.array(START_ANGLES.split(), dtype=float) @@ -237,19 +241,20 @@ def J(self, name, q=None, x=None, object_type="body"): if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") - #link_index = self.articulation_view.get_link_index(name) - link_index = self.safe_get_link(name, default=6) + # jaco2 version + link_index = self.articulation_view.get_link_index(name) + + # parent version for ur5 and h1 + #link_index = self.articulation_view.get_link_index(self.EE_parent_link) + + #link_index = self.safe_get_link(name, default=6) # Extract Jacobian for specific link env_idx = 0 # Assuming single environment - # Handle different Jacobian tensor shapes - # Based on your diagnostic: shape is (1, 13, 6, 12) - # This means: (num_envs, num_bodies, 6_dof_per_body, total_dofs) + # shape is (1, 13, 6, 12) if len(jacobians.shape) == 4: # Shape: (num_envs, num_bodies, 6, num_dofs) - # Get Jacobian for the end-effector link (link_index 7) - #print("link_index: ", link_index) J_full = jacobians[env_idx, link_index, :, :] elif len(jacobians.shape) == 3: # Shape: (num_envs, num_bodies * 6, num_dofs) @@ -328,6 +333,7 @@ def R(self, name, q=None, object_type="body"): else: raise ValueError(f"Unsupported object type: {object_type}") + prim = self.stage.GetPrimAtPath(prim_path) if not prim.IsValid(): raise RuntimeError(f"Prim '{prim_path}' not found") @@ -364,7 +370,6 @@ def quaternion(self, name, q=None): # Get 4x4 world transform matrix matrix = omni.usd.get_world_transform_matrix(prim) - quat = matrix.ExtractRotationQuat() # Convert to [w, x, y, z] NumPy array @@ -412,6 +417,10 @@ def Tx(self, name, q=None, x=None, object_type="body"): else: raise ValueError(f"Unsupported object_type: {object_type}") + + #print("dof_names: ", self.articulation.dof_names) + #print("name :", name) + #print("prim_path :", prim_path) # Get world position prim = self.stage.GetPrimAtPath(prim_path) if not prim.IsValid(): @@ -496,10 +505,36 @@ def _get_link_index(self, name): return link_index + 1 - + # remove + def safe_get_link(self, name, default=None): - try: + if self.has_EE is True: return self.articulation_view.get_link_index(name) - except KeyError: - print(f"Link '{name}' not found, using default: {default}") + else: + from pxr import UsdGeom + + # Get the parent prim + parent_prim = self.stage.GetPrimAtPath(self.prim_path) + + + for child in parent_prim.GetChildren(): + if child.GetName() == name: + print("Found EE prim:", child.GetPath()) + ee_xform = UsdGeom.Xformable(self.stage.GetPrimAtPath(child.GetPath())) + return ee_xform + return default + + + def adjoint_transform(offset=[0, 0, 0.05]): + r = np.asarray(offset) + S = np.array([ + [0, -r[2], r[1]], + [r[2], 0, -r[0]], + [-r[1], r[0], 0] + ]) + upper = np.hstack([np.eye(3), np.zeros((3,3))]) + lower = np.hstack([-S, np.eye(3)]) + adj = np.vstack([upper, lower]) + return adj + diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 4bdcdb69..7c8c5fa6 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -10,7 +10,7 @@ #TODO change import #TODO is "Robot" even necessary from isaacsim.core.api.robots import Robot # type: ignore -from pxr import UsdGeom, Gf, UsdShade, Sdf # type: ignore +from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics# type: ignore import omni.isaac.core.utils.stage as stage_utils # type: ignore from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore @@ -33,7 +33,7 @@ def __init__(self, robot_config, dt=0.001, force_download=False): super().__init__(robot_config) self.robot_config = robot_config self.dt = dt # time step - self.count = 0 # keep track of how many times send forces is called + #self.count = 0 # keep track of how many times send forces is called self.prim_path = "/World/robot" self.name = self.robot_config.robot_type @@ -52,18 +52,13 @@ def connect(self, joint_names=None, camera_id=-1): self.world.scene.add_default_ground_plane() self.context = omni.usd.get_context() self.stage = self.context.get_stage() - #TODO necessary for H1 robot - #self.world.add_physics_callback("send_actions", self.send_actions) - - + + # 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}") - print(f"End Effector with name '{self.robot_config.ee_link_name}' specified in UDS, using it ...") - - - + stage_utils.add_reference_to_stage( usd_path=robot_usd_path, prim_path=self.prim_path, @@ -80,8 +75,16 @@ def connect(self, joint_names=None, camera_id=-1): self.articulation_view = ArticulationView(prim_paths_expr=self.prim_path, name=self.name + "_view") self.world.scene.add(self.articulation_view) self.articulation_view.initialize() - + + + + # add virtual EE if none exists + if (self.robot_config.has_EE is False): + 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) + print("links 2 : ", self.articulation_view.body_names) + # Set simulation time step self.world.get_physics_context().set_physics_dt(self.dt) @@ -136,6 +139,10 @@ def connect(self, joint_names=None, camera_id=-1): self.prim_path, ) + #TODO necessary for H1 robot + #self.world.add_physics_callback("send_actions", self.send_actions) + + def _map_joint_names(self, joint_names): """ @@ -254,9 +261,9 @@ def set_xyz(self, prim_path, xyz, orientation=np.array([0., 0., 0., 1.])): # method for keep_standing def send_actions(self, dt): pelvis_prim_path = '/World/robot/pelvis' - prim=self.stage.GetPrimAtPath(pelvis_prim_path) - 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 ,0.02)) + prim = self.stage.GetPrimAtPath(pelvis_prim_path) + 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, 0.85)) #prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(0.70711 ,0.70711 ,0.0 ,0.0)) @@ -324,6 +331,86 @@ def set_gains_force_control(self): + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): + """Add virtual end effector link as an Xform under the specified parent link""" + # Full path to parent + 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)) + + print(f"Created virtual EE link at {ee_prim_path}") + + + + + + ''' + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): + """Add virtual end effector link""" + # Create new prim for EE + ee_prim_path = f"{self.prim_path}/{ee_name}" + ee_prim = self.stage.DefinePrim(ee_prim_path, "Xform") + # Set transform relative to parent + xform = UsdGeom.Xform(ee_prim) + xform.AddTranslateOp().Set(Gf.Vec3d(*offset)) + # Parent it to the hand link + parent_prim = self.stage.GetPrimAtPath(f"{self.prim_path}/{EE_parent_link}") + ee_prim.GetReferences().AddInternalReference(parent_prim.GetPath()) + + + + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): + """Add virtual end effector link - FIXED VERSION with proper mass""" + # Create new prim for EE + ee_prim_path = f"{self.prim_path}/{ee_name}" + ee_prim = self.stage.DefinePrim(ee_prim_path, "Xform") + + # Set transform relative to parent + xform = UsdGeom.Xform(ee_prim) + xform.AddTranslateOp().Set(Gf.Vec3d(*offset)) + + # Make it a physics body + rigid_body_api = UsdPhysics.RigidBodyAPI.Apply(ee_prim) + + # ADD PROPER MASS PROPERTIES: + mass_api = UsdPhysics.MassAPI.Apply(ee_prim) + mass_api.GetMassAttr().Set(0.001) # 1 gram + + # Set proper inertia (for a small 5mm radius sphere) + inertia_val = 0.001 * (0.005 ** 2) # ≈ 2.5e-8 + inertia_diagonal = Gf.Vec3f(inertia_val, inertia_val, inertia_val) + mass_api.GetDiagonalInertiaAttr().Set(inertia_diagonal) + + # Create fixed joint to parent + joint_prim_path = f"{self.prim_path}/{ee_name}_joint" + joint_prim = UsdPhysics.FixedJoint.Define(self.stage, joint_prim_path) + + # Connect to parent + parent_prim_path = f"{self.prim_path}/{EE_parent_link}" + joint_prim.GetBody0Rel().SetTargets([parent_prim_path]) + joint_prim.GetBody1Rel().SetTargets([ee_prim_path]) + + # Set joint offset + joint_prim.GetLocalPos0Attr().Set(Gf.Vec3d(*offset)) + joint_prim.GetLocalPos1Attr().Set(Gf.Vec3d(0, 0, 0)) + + return ee_prim +''' + + + + + + + + ''' def set_gains_position_control_min(self): """Set up position control with high stiffness""" n_dofs = self.robot_config.N_JOINTS @@ -356,4 +443,51 @@ def set_gains_position_control(self): self.articulation_view.set_gains(stiffness, damping) print(f"Set gains for position control for arm joints {arm_joint_indices }") + ''' + + + + + + + + + + + + ''' + + def add_virtual_ee_link_(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): + """Add virtual end effector link""" + # Create new prim for EE + ee_prim_path = f"{self.prim_path}/{ee_name}" + ee_prim = self.stage.DefinePrim(ee_prim_path, "Xform") + + # Set transform relative to parent + xform = UsdGeom.Xform(ee_prim) + xform.AddTranslateOp().Set(Gf.Vec3d(*offset)) + + # Get parent prim + parent_prim_path = f"{self.prim_path}/{EE_parent_link}" + parent_prim = self.stage.GetPrimAtPath(parent_prim_path) + + # Check if parent prim exists + if not parent_prim.IsValid(): + print(f"Error: Parent prim '{parent_prim_path}' not found") + return None + + # Method 1: Set parent-child relationship using stage hierarchy + # This is the most straightforward approach for creating hierarchy + ee_prim_under_parent_path = f"{parent_prim_path}/{ee_name}" + ee_prim_under_parent = self.stage.DefinePrim(ee_prim_under_parent_path, "Xform") + + # Set transform on the correctly parented prim + xform_parented = UsdGeom.Xform(ee_prim_under_parent) + xform_parented.AddTranslateOp().Set(Gf.Vec3d(*offset)) + + # Remove the original incorrectly placed prim + self.stage.RemovePrim(ee_prim_path) + + return ee_prim_under_parent + ''' \ No newline at end of file diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index 8d291cc0..baf4d151 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -17,7 +17,7 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "jaco2" + arm_model = "ur5" robot_config = arm(arm_model) dt = 0.007 # 143 Hz @@ -100,6 +100,9 @@ def gen_target(interface): ) interface.send_forces(u) + #interface.world.step(render=True) + + # calculate end-effector position ee_xyz = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"]) From fc6bd286edeca02f2de990e90c01f76c63d7f7ec Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 10 Jul 2025 14:19:07 -0400 Subject: [PATCH 27/42] force control working for UR5 and jaco2 --- abr_control/arms/isaacsim_config.py | 92 ++------ abr_control/arms/unitree_H1/__init__.py | 1 - abr_control/arms/unitree_H1/config.py | 287 ------------------------ abr_control/interfaces/nv_isaacsim.py | 159 ++----------- 4 files changed, 38 insertions(+), 501 deletions(-) delete mode 100644 abr_control/arms/unitree_H1/__init__.py delete mode 100644 abr_control/arms/unitree_H1/config.py diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index dc51a00d..5f34b080 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -163,37 +163,24 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr - + def g(self, q=None): """ - Returns the joint-space forces due to gravity, Coriolis, and centrifugal effects - in Isaac Sim (equivalent to MuJoCo's qfrc_bias). - - Parameters - ---------- - q: np.ndarray, optional (Default: None) - Joint positions to compute the bias forces at. If None, uses current sim state. + Returns the gravity and Coriolis/centrifugal forces for the controlled arm_joints. + Args: + q (np.ndarray, optional): Joint positions + Returns: + np.ndarray: Generalized bias forces for controlled DOF """ # Compute gravity and Coriolis/centrifugal separately - gravity = self.articulation_view.get_generalized_gravity_forces() - coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces() + gravity = self.articulation_view.get_generalized_gravity_forces(joint_indices=self.joint_pos_addrs)[0] + coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.joint_pos_addrs)[0] - # Total generalized bias forces - g_full = gravity + coriolis - - # Handle batch dimension if present - if g_full.ndim == 2: - if g_full.shape[0] == 1: - g_full = g_full[0] # Remove batch dimension - - if q is not None: - # If q is provided, ensure g matches the size of q - if len(g_full) != len(q): - g_full = g_full[:len(q)] + g = gravity + coriolis - return -g_full - + return -g + def dJ(self, name, q=None, dq=None, x=None): """Returns the derivative of the Jacobian wrt to time @@ -242,12 +229,8 @@ def J(self, name, q=None, x=None, object_type="body"): raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") # jaco2 version - link_index = self.articulation_view.get_link_index(name) - - # parent version for ur5 and h1 - #link_index = self.articulation_view.get_link_index(self.EE_parent_link) - - #link_index = self.safe_get_link(name, default=6) + #link_index = self.articulation_view.get_link_index(name) + link_index = self.N_JOINTS -1 # Extract Jacobian for specific link env_idx = 0 # Assuming single environment @@ -490,51 +473,4 @@ def set_joint_velocities(self, dq): full_velocities[idx] = dq[i] self.articulation.set_joint_velocities(full_velocities) - #TODO remove of fix method used for UR5 - def _get_link_index(self, name): - """Get the index of a link by its name""" - - import omni.isaac.core.utils.prims as prims_utils - - prim_path = self._get_prim_path(name) - prim = prims_utils.get_prim_at_path(prim_path) - parent_prim = prims_utils.get_prim_parent(prim) - parent_name = parent_prim.GetName() - link_index = self.articulation_view.get_link_index(parent_name) - print(f"Link name: {name}, Prim path: {prim_path}, Parent name: {parent_name}, Link index: {link_index}") - - return link_index + 1 - - # remove - - def safe_get_link(self, name, default=None): - if self.has_EE is True: - return self.articulation_view.get_link_index(name) - else: - from pxr import UsdGeom - - # Get the parent prim - parent_prim = self.stage.GetPrimAtPath(self.prim_path) - - - for child in parent_prim.GetChildren(): - if child.GetName() == name: - print("Found EE prim:", child.GetPath()) - ee_xform = UsdGeom.Xformable(self.stage.GetPrimAtPath(child.GetPath())) - return ee_xform - - return default - - - def adjoint_transform(offset=[0, 0, 0.05]): - r = np.asarray(offset) - S = np.array([ - [0, -r[2], r[1]], - [r[2], 0, -r[0]], - [-r[1], r[0], 0] - ]) - upper = np.hstack([np.eye(3), np.zeros((3,3))]) - lower = np.hstack([-S, np.eye(3)]) - adj = np.vstack([upper, lower]) - return adj - + \ No newline at end of file diff --git a/abr_control/arms/unitree_H1/__init__.py b/abr_control/arms/unitree_H1/__init__.py deleted file mode 100644 index cca5d9bd..00000000 --- a/abr_control/arms/unitree_H1/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .config import Config diff --git a/abr_control/arms/unitree_H1/config.py b/abr_control/arms/unitree_H1/config.py deleted file mode 100644 index 27c924c6..00000000 --- a/abr_control/arms/unitree_H1/config.py +++ /dev/null @@ -1,287 +0,0 @@ -import numpy as np -import sympy as sp - -from ..base_config import BaseConfig - - -class Config(BaseConfig): - """Robot config file for the right H1 arm for IsaacSim - - Attributes - ---------- - START_ANGLES : numpy.array - The joint angles for a safe home or rest position - _M_LINKS : sympy.diag - inertia matrix of the links - _M_JOINTS : sympy.diag - inertia matrix of the joints - L : numpy.array - segment lengths of arm [meters] - KZ : sympy.Matrix - z isolation vector in orientational part of Jacobian - - Transform Naming Convention: Tpoint1point2 - ex: Tj1l1 transforms from joint 1 to link 1 - - Transforms are broken up into two matrices for simplification - ex: Tj0l1a and Tj0l1b where the former transform accounts for - joint rotations and the latter accounts for static rotations - and translations - """ - - def __init__(self, **kwargs): - - super().__init__(N_JOINTS=4, N_LINKS=5, ROBOT_NAME="unitree_H1", **kwargs) - - self._T = {} # dictionary for storing calculated transforms - - self.JOINT_NAMES = [f"H1_right_arm_joint{ii}" for ii in range(self.N_JOINTS)] - - # for the null space controller, keep arm near these angles - #self.START_ANGLES = np.array( - # [np.pi / 4.0, np.pi / 4.0, np.pi / 4.0, np.pi / 4.0], dtype="float32" - #) - self.START_ANGLES = np.array( - [0.0, 0.0, 0.0, 0.0], dtype="float32" - ) - - # TODO adapt this - # create the inertia matrices for each link of the three joint - # TODO: identify the actual values for these links - self._M_LINKS.append(sp.zeros(6, 6)) # link0 - self._M_LINKS.append(sp.diag(0.001, 0.0, 0.0, 0.001, 0.0, 0.001)) # link1 - right_shoulder_pitch_link - self._M_LINKS.append(sp.diag(0.002, 0.0, 0.0, 0.002, 0.0, 0.001)) # link2 - right_shoulder_roll_link - self._M_LINKS.append(sp.diag(0.004, 0.0, 0.0, 0.004, 0.0, 0.0)) # link3 - right_shoulder_yaw_link - self._M_LINKS.append(sp.diag(0.0, 0.0, 0.0, 0.006, 0.0, 0.006)) # link4 - right_elbow_link - - # the joints don't weigh anything - self._M_JOINTS = [sp.zeros(6, 6) for ii in range(self.N_JOINTS)] - - # segment lengths associated with each joint - # [x, y, z], Ignoring lengths < 1e-04 - - - self.L = [ - [0.0, 0.0, 0.0], # base offset (optional) - [0.0055, -0.15535, 0.42999], # right_shoulder_pitch_joint - [0.005045, -0.053657, -0.015715], # right_shoulder_pitch_link - [-0.0055, -0.0565, -0.0165], # right_shoulder_roll_joint - [0.000679, -0.00115, -0.094076], # right_shoulder_roll_link - [0.0, 0.0, -0.1343], # right_shoulder_yaw_joint - [0.01365, -0.002767, -0.16266], # right_shoulder_yaw_link - [0.0185, 0.0, -0.198], # right_elbow_joint - [0.164862, -0.000118, -0.015734], # right_elbow_link - [0.0, 0.0, 0.09], # offset to end of hand/fingers (assumed) - ] - self.L = np.array(self.L) - - - - ''' - self.L = [ - [0.0, 0.0, 0.0], # base offset (optional, can be torso_link) - [0.0055, -0.15535, 0.42999], # right_shoulder_pitch_joint (from torso_link) - [0.005045, -0.053657, -0.015715], # right_shoulder_pitch_link (inertial origin) - [-0.0055, -0.0565, -0.0165], # right_shoulder_roll_joint (from shoulder_pitch_link) - [0.000679, -0.00115, -0.094076], # right_shoulder_roll_link (inertial origin) - [0.0, 0.0, -0.1343], # right_shoulder_yaw_joint (from shoulder_roll_link) - [0.01365, -0.002767, -0.16266], # right_shoulder_yaw_link (inertial origin) - [0.0185, 0.0, -0.198], # right_elbow_joint (from shoulder_yaw_link) - [0.164862, -0.000118, -0.015734], # right_elbow_link (inertial origin) - ] - ''' - - - - # Transform matrix : origin -> link 0 - # no change of axes, account for offsets - self.Torgl0 = sp.Matrix( - [ - [1, 0, 0, self.L[0, 0]], - [0, 1, 0, self.L[0, 1]], - [0, 0, 1, self.L[0, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : link 0 -> joint 0 - # no change of axes, account for offsets - self.Tl0j0 = sp.Matrix( - [ - [1, 0, 0, self.L[1, 0]], - [0, 1, 0, self.L[1, 1]], - [0, 0, 1, self.L[1, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : joint 0 -> link 1 - # account for rotation of q - self.Tj0l1a = sp.Matrix( - [ - [sp.cos(self.q[0]), -sp.sin(self.q[0]), 0, 0], - [sp.sin(self.q[0]), sp.cos(self.q[0]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - ) - # no change of axes, account for offsets - self.Tj0l1b = sp.Matrix( - [ - [1, 0, 0, self.L[2, 0]], - [0, 1, 0, self.L[2, 1]], - [0, 0, 1, self.L[2, 2]], - [0, 0, 0, 1], - ] - ) - self.Tj0l1 = self.Tj0l1a * self.Tj0l1b - - # Transform matrix : link 1 -> joint 1 - # no change of axes, account for offsets - self.Tl1j1 = sp.Matrix( - [ - [1, 0, 0, self.L[3, 0]], - [0, 1, 0, self.L[3, 1]], - [0, 0, 1, self.L[3, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : joint 1 -> link 2 - # account for rotation of q - self.Tj1l2a = sp.Matrix( - [ - [sp.cos(self.q[1]), -sp.sin(self.q[1]), 0, 0], - [sp.sin(self.q[1]), sp.cos(self.q[1]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - ) - # no change of axes, account for offsets - self.Tj1l2b = sp.Matrix( - [ - [1, 0, 0, self.L[4, 0]], - [0, 1, 0, self.L[4, 1]], - [0, 0, 1, self.L[4, 2]], - [0, 0, 0, 1], - ] - ) - self.Tj1l2 = self.Tj1l2a * self.Tj1l2b - - # Transform matrix : link 2 -> joint 2 - # no change of axes, account for offsets - self.Tl2j2 = sp.Matrix( - [ - [1, 0, 0, self.L[5, 0]], - [0, 1, 0, self.L[5, 1]], - [0, 0, 1, self.L[5, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : joint 2 -> link 3 - # account for rotation of q - self.Tj2l3a = sp.Matrix( - [ - [sp.cos(self.q[2]), -sp.sin(self.q[2]), 0, 0], - [sp.sin(self.q[2]), sp.cos(self.q[2]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - ) - # no change of axes, account for offsets - self.Tj2l3b = sp.Matrix( - [ - [1, 0, 0, self.L[6, 0]], - [0, 1, 0, self.L[6, 1]], - [0, 0, 1, self.L[6, 2]], - [0, 0, 0, 1], - ] - ) - self.Tj2l3 = self.Tj2l3a * self.Tj2l3b - - # Transform matrix : link 3 -> end-effector - # no change of axes, account for offsets - self.Tl3j3 = sp.Matrix( - [ - [1, 0, 0, self.L[7, 0]], - [0, 1, 0, self.L[7, 1]], - [0, 0, 1, self.L[7, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : joint 3 -> link 4 - # account for rotation of q - self.Tj3l4a = sp.Matrix( - [ - [sp.cos(self.q[3]), -sp.sin(self.q[3]), 0, 0], - [sp.sin(self.q[3]), sp.cos(self.q[3]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - ) - # no change of axes, account for offsets - self.Tj3l4b = sp.Matrix( - [ - [1, 0, 0, self.L[8, 0]], - [0, 1, 0, self.L[8, 1]], - [0, 0, 1, self.L[8, 2]], - [0, 0, 0, 1], - ] - ) - self.Tj3l4 = self.Tj3l4a * self.Tj3l4b - - # Transform matrix : link 4 -> end-effector - # no change of axes, account for offsets - self.Tl4ee = sp.Matrix( - [ - [1, 0, 0, self.L[9, 0]], - [0, 1, 0, self.L[9, 1]], - [0, 0, 1, self.L[9, 2]], - [0, 0, 0, 1], - ] - ) - - - # orientation part of the Jacobian (compensating for angular velocity) - self.J_orientation = [ - self._calc_T("joint0")[:3, :3] * self._KZ, # joint 0 orientation - self._calc_T("joint1")[:3, :3] * self._KZ, # joint 1 orientation - self._calc_T("joint2")[:3, :3] * self._KZ, # joint 2 orientation - self._calc_T("joint3")[:3, :3] * self._KZ, - ] # joint 3 orientation - - def _calc_T(self, name): - """Uses Sympy to generate the transform for a joint or link - - name : string - name of the joint, link, or end-effector - """ - - if self._T.get(name, None) is None: - if name == "link0": - self._T[name] = self.Torgl0 - elif name == "joint0": - self._T[name] = self._calc_T("link0") * self.Tl0j0 - elif name == "link1": - self._T[name] = self._calc_T("joint0") * self.Tj0l1 - elif name == "joint1": - self._T[name] = self._calc_T("link1") * self.Tl1j1 - elif name == "link2": - self._T[name] = self._calc_T("joint1") * self.Tj1l2 - elif name == "joint2": - self._T[name] = self._calc_T("link2") * self.Tl2j2 - elif name == "link3": - self._T[name] = self._calc_T("joint2") * self.Tj2l3 - elif name == "joint3": - self._T[name] = self._calc_T("link3") * self.Tl3j3 - elif name == "link4": - self._T[name] = self._calc_T("joint3") * self.Tj3l4 - elif name == "EE": - self._T[name] = self._calc_T("link4") * self.Tl4ee - - else: - raise Exception(f"Invalid transformation name: {name}") - - return self._T[name] diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 7c8c5fa6..bf943a4d 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -78,12 +78,13 @@ def connect(self, joint_names=None, camera_id=-1): - + # add virtual EE if none exists if (self.robot_config.has_EE is False): 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) print("links 2 : ", self.articulation_view.body_names) + # Set simulation time step self.world.get_physics_context().set_physics_dt(self.dt) @@ -110,22 +111,13 @@ def connect(self, joint_names=None, camera_id=-1): for name in joint_names: if name not in all_joint_names: raise Exception(f"Joint name {name} does not exist in robot model") - #TODO check if joint_idx is necessary + joint_idx = all_joint_names.index(name) self.joint_pos_addrs.append(joint_idx) self.joint_vel_addrs.append(joint_idx) + #TODO check if joint_dyn_addrs necessary self.joint_dyn_addrs.append(joint_idx) - - ''' - # is boolean flag necessary? - if has_EE: - print(f"End Effector with name '{self.ee_link_name}' specified in UDS, using it ...") - else: - print("No End Effector, creating EE Xform...") - ee_path = self.create_ee_xform(EE_parent_link, np.array([-0.04, 0, 0])) - #print("End Effector Info: ", ee_info) - ''' # Connect robot config with simulation data print("Connecting to robot config...") @@ -150,19 +142,23 @@ def _map_joint_names(self, joint_names): """ # Get actual joint names from the robot actual_joint_names = self.articulation.dof_names - - # If input names are in MuJoCo format (joint0, joint1, etc.) - if all(name.startswith('joint') and name[5:].isdigit() for name in joint_names): - # Map by index: joint0 -> first joint, joint1 -> second joint, etc. - mapped_names = [] - for name in joint_names: - joint_idx = int(name[5:]) # Extract number from "jointX" - if joint_idx < len(actual_joint_names): - mapped_names.append(actual_joint_names[joint_idx]) - else: - raise Exception(f"Joint index {joint_idx} out of range. Robot has {len(actual_joint_names)} joints.") - return mapped_names - + + if self.name is "h1": + joint_list = ['right_shoulder_pitch_joint', 'right_shoulder_roll_joint', 'right_shoulder_yaw_joint', 'right_elbow_joint'] + return np.array(joint_list) + else: + # If input names are in MuJoCo format (joint0, joint1, etc.) + if all(name.startswith('joint') and name[5:].isdigit() for name in joint_names): + # Map by index: joint0 -> first joint, joint1 -> second joint, etc. + mapped_names = [] + for name in joint_names: + joint_idx = int(name[5:]) # Extract number from "jointX" + if joint_idx < len(actual_joint_names): + mapped_names.append(actual_joint_names[joint_idx]) + else: + raise Exception(f"Joint index {joint_idx} out of range. Robot has {len(actual_joint_names)} joints.") + return mapped_names + # If names are already in correct format, return as-is return joint_names @@ -330,7 +326,7 @@ def set_gains_force_control(self): print(f"Set gains for force control for arm joints {arm_joint_indices }") - + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): """Add virtual end effector link as an Xform under the specified parent link""" # Full path to parent @@ -348,69 +344,9 @@ def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): print(f"Created virtual EE link at {ee_prim_path}") - - - ''' - def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): - """Add virtual end effector link""" - # Create new prim for EE - ee_prim_path = f"{self.prim_path}/{ee_name}" - ee_prim = self.stage.DefinePrim(ee_prim_path, "Xform") - # Set transform relative to parent - xform = UsdGeom.Xform(ee_prim) - xform.AddTranslateOp().Set(Gf.Vec3d(*offset)) - # Parent it to the hand link - parent_prim = self.stage.GetPrimAtPath(f"{self.prim_path}/{EE_parent_link}") - ee_prim.GetReferences().AddInternalReference(parent_prim.GetPath()) - - - - def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): - """Add virtual end effector link - FIXED VERSION with proper mass""" - # Create new prim for EE - ee_prim_path = f"{self.prim_path}/{ee_name}" - ee_prim = self.stage.DefinePrim(ee_prim_path, "Xform") - - # Set transform relative to parent - xform = UsdGeom.Xform(ee_prim) - xform.AddTranslateOp().Set(Gf.Vec3d(*offset)) - - # Make it a physics body - rigid_body_api = UsdPhysics.RigidBodyAPI.Apply(ee_prim) - - # ADD PROPER MASS PROPERTIES: - mass_api = UsdPhysics.MassAPI.Apply(ee_prim) - mass_api.GetMassAttr().Set(0.001) # 1 gram - - # Set proper inertia (for a small 5mm radius sphere) - inertia_val = 0.001 * (0.005 ** 2) # ≈ 2.5e-8 - inertia_diagonal = Gf.Vec3f(inertia_val, inertia_val, inertia_val) - mass_api.GetDiagonalInertiaAttr().Set(inertia_diagonal) - - # Create fixed joint to parent - joint_prim_path = f"{self.prim_path}/{ee_name}_joint" - joint_prim = UsdPhysics.FixedJoint.Define(self.stage, joint_prim_path) - - # Connect to parent - parent_prim_path = f"{self.prim_path}/{EE_parent_link}" - joint_prim.GetBody0Rel().SetTargets([parent_prim_path]) - joint_prim.GetBody1Rel().SetTargets([ee_prim_path]) - - # Set joint offset - joint_prim.GetLocalPos0Attr().Set(Gf.Vec3d(*offset)) - joint_prim.GetLocalPos1Attr().Set(Gf.Vec3d(0, 0, 0)) - - return ee_prim -''' - - - - - - - ''' + def set_gains_position_control_min(self): """Set up position control with high stiffness""" n_dofs = self.robot_config.N_JOINTS @@ -443,51 +379,4 @@ def set_gains_position_control(self): self.articulation_view.set_gains(stiffness, damping) print(f"Set gains for position control for arm joints {arm_joint_indices }") - ''' - - - - - - - - - - - - - ''' - - def add_virtual_ee_link_(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): - """Add virtual end effector link""" - # Create new prim for EE - ee_prim_path = f"{self.prim_path}/{ee_name}" - ee_prim = self.stage.DefinePrim(ee_prim_path, "Xform") - - # Set transform relative to parent - xform = UsdGeom.Xform(ee_prim) - xform.AddTranslateOp().Set(Gf.Vec3d(*offset)) - - # Get parent prim - parent_prim_path = f"{self.prim_path}/{EE_parent_link}" - parent_prim = self.stage.GetPrimAtPath(parent_prim_path) - - # Check if parent prim exists - if not parent_prim.IsValid(): - print(f"Error: Parent prim '{parent_prim_path}' not found") - return None - - # Method 1: Set parent-child relationship using stage hierarchy - # This is the most straightforward approach for creating hierarchy - ee_prim_under_parent_path = f"{parent_prim_path}/{ee_name}" - ee_prim_under_parent = self.stage.DefinePrim(ee_prim_under_parent_path, "Xform") - - # Set transform on the correctly parented prim - xform_parented = UsdGeom.Xform(ee_prim_under_parent) - xform_parented.AddTranslateOp().Set(Gf.Vec3d(*offset)) - - # Remove the original incorrectly placed prim - self.stage.RemovePrim(ee_prim_path) - - return ee_prim_under_parent - ''' \ No newline at end of file + ''' \ No newline at end of file From f7bbb1ec02a5e377cc8f4ab953463bf1ccc888e0 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Mon, 14 Jul 2025 12:42:33 -0400 Subject: [PATCH 28/42] adapted mass matrix to extract the correct, controlled joints --- abr_control/arms/isaacsim_config.py | 28 +++++++++++++++++---------- abr_control/interfaces/nv_isaacsim.py | 3 +-- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 5f34b080..746947eb 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -98,7 +98,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F elif self.robot_type == "h1": 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_parent_link = "right_elbow_link" START_ANGLES = "0 0 0 0" print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") @@ -204,8 +204,7 @@ def dJ(self, name, q=None, dq=None, x=None): raise NotImplementedError - - + def J(self, name, q=None, x=None, object_type="body"): if name == "EE": name = self.ee_link_name @@ -230,7 +229,17 @@ def J(self, name, q=None, x=None, object_type="body"): # jaco2 version #link_index = self.articulation_view.get_link_index(name) - link_index = self.N_JOINTS -1 + #link_index = self.N_JOINTS -1 + #print("link_index old: ", link_index) + #link_index = self.articulation_view.get_link_index(name) + #print("name: ", name, " link_index new: ", link_index) + + if self.robot_type is "ur5": + link_index = 5 + + elif self.robot_type is "jaco2": + link_index = 6 + # Extract Jacobian for specific link env_idx = 0 # Assuming single environment @@ -273,6 +282,9 @@ def J(self, name, q=None, x=None, object_type="body"): raise ValueError(f"Invalid object type specified: {object_type}") return np.copy(self._J6N) + + + def M(self, q=None): @@ -290,7 +302,7 @@ def M(self, q=None): # If you have only one robot in ArticulationView M_full = M[0] # extract only the controlled DOF - M_arm = M_full[:self.N_JOINTS, :self.N_JOINTS] + M_arm = M_full[np.ix_(self.joint_pos_addrs, self.joint_pos_addrs)] return np.copy(M_arm) @@ -399,11 +411,7 @@ def Tx(self, name, q=None, x=None, object_type="body"): prim_path = self._get_prim_path(name) else: raise ValueError(f"Unsupported object_type: {object_type}") - - - #print("dof_names: ", self.articulation.dof_names) - #print("name :", name) - #print("prim_path :", prim_path) + # Get world position prim = self.stage.GetPrimAtPath(prim_path) if not prim.IsValid(): diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index bf943a4d..c211ea2f 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -83,7 +83,6 @@ def connect(self, joint_names=None, camera_id=-1): if (self.robot_config.has_EE is False): 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) - print("links 2 : ", self.articulation_view.body_names) # Set simulation time step @@ -327,7 +326,7 @@ def set_gains_force_control(self): - def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[0, 0, 0.05]): + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): """Add virtual end effector link as an Xform under the specified parent link""" # Full path to parent parent_path = f"{self.prim_path}/{EE_parent_link}" From 4ec172c05d83e8424625c323865ebcc962c53e6f Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 15 Jul 2025 11:25:41 -0400 Subject: [PATCH 29/42] added torso joint and used standing method in setup for h1 --- abr_control/interfaces/nv_isaacsim.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index c211ea2f..2416e5b4 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -130,8 +130,8 @@ def connect(self, joint_names=None, camera_id=-1): self.prim_path, ) - #TODO necessary for H1 robot - #self.world.add_physics_callback("send_actions", self.send_actions) + if self.robot_config.robot_type == "h1": + self.world.add_physics_callback("send_actions", self.send_actions) @@ -143,7 +143,11 @@ def _map_joint_names(self, joint_names): actual_joint_names = self.articulation.dof_names if self.name is "h1": - joint_list = ['right_shoulder_pitch_joint', 'right_shoulder_roll_joint', 'right_shoulder_yaw_joint', 'right_elbow_joint'] + joint_list = ['torso_joint', # 1 + 'right_shoulder_pitch_joint', # 2 + 'right_shoulder_roll_joint', # 3 + 'right_shoulder_yaw_joint', # 4 + 'right_elbow_joint'] # 5 return np.array(joint_list) else: # If input names are in MuJoCo format (joint0, joint1, etc.) From 53067fbc69d0a9e7ace36700cad03a61ffaeff3f Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Wed, 16 Jul 2025 13:04:40 -0400 Subject: [PATCH 30/42] integrated robot H1 with hands --- abr_control/arms/isaacsim_config.py | 75 +++++++++++++++++++++++++++++ 1 file changed, 75 insertions(+) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 746947eb..3a028ed7 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -102,6 +102,16 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F START_ANGLES = "0 0 0 0" print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") + elif self.robot_type == "h1_hands": + self.robot_path = "/Isaac/Robots/Unitree/H1/h1_with_hand.usd" + self.has_EE = True # H1 has no end-effector + #self.EE_parent_link = "right_elbow_link" + self.ee_link_name = "right_hand_link" + START_ANGLES = "0 0 0 0" + print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") + + + self.START_ANGLES = np.array(START_ANGLES.split(), dtype=float) @@ -162,6 +172,10 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.N_ALL_JOINTS = N_ALL_JOINTS + + + + def g(self, q=None): @@ -205,6 +219,7 @@ def dJ(self, name, q=None, dq=None, x=None): + def J(self, name, q=None, x=None, object_type="body"): if name == "EE": name = self.ee_link_name @@ -239,7 +254,14 @@ def J(self, name, q=None, x=None, object_type="body"): elif self.robot_type is "jaco2": link_index = 6 + # [6, 10, 14, 18] + elif self.robot_type is "h1": + link_index = 18 + # [6, 10, 14, 18, 20] + elif self.robot_type is "h1_hands": + link_index = 20 + # Extract Jacobian for specific link env_idx = 0 # Assuming single environment @@ -269,6 +291,13 @@ def J(self, name, q=None, x=None, object_type="body"): self._J6N[:3, :] = J[:3, :] # Angular velocity Jacobian (last 3 rows) self._J6N[3:, :] = J[3:, :] + + #print(f"Robot type: {self.robot_type}") + #print(f"Link index: {link_index}") + #print(f"Arm joint indices: {self.joint_pos_addrs}") + #print(f"J_full shape: {J_full.shape}") + #print(f"J shape after filtering: {J.shape}") + #print(f"self.joint_pos_addrs: {self.joint_pos_addrs}") # Additional validation - check if Jacobian makes sense (non-zero values) if np.allclose(J, 0): @@ -284,6 +313,52 @@ def J(self, name, q=None, x=None, object_type="body"): return np.copy(self._J6N) + ''' + + def J(self, name, q=None, x=None, object_type="body"): + + """ + Returns the Jacobian for a specified end-effector link, projected onto controlled joints. + + Parameters + ---------- + ee_link_name : str + Name of the end-effector link (e.g., 'right_wrist_link') + controlled_joint_names : list of str + List of joint names to keep in the Jacobian columns. + + Returns + ------- + np.ndarray + (6, num_controlled_dofs) Jacobian [linear; angular] + """ + if name == "EE": + name = self.ee_link_name + + # Make sure physics is stepped + self.world.step(render=False) + + # Get full Jacobians + jacobians = self.articulation_view.get_jacobians(clone=True) # (1, num_links, 6, num_dofs) + + # Get link index + #link_index = self.articulation_view.body_names.index(name) + link_index = self.articulation_view.body_names.index(self.EE_parent_link) + + + # Full 6 x N Jacobian for this link + #J_full = jacobians[0, link_index, :, :] # shape (6, num_dofs) + J_full = jacobians[0, link_index, :, :] # shape (6, num_dofs) + + + # Select only columns corresponding to controlled joints + #print("selected indices: ", self.joint_pos_addrs) + J_projected = J_full[:, self.joint_pos_addrs] # shape (6, num_controlled_dofs) + + return J_projected + ''' + + From 62cdbeccebe5bd12f89f37b10fe965856a7c7bb1 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Wed, 16 Jul 2025 13:05:15 -0400 Subject: [PATCH 31/42] fixed method set_gains_force_control so it adresses the right joints --- abr_control/interfaces/nv_isaacsim.py | 97 +++++++++++++++++++++++---- 1 file changed, 83 insertions(+), 14 deletions(-) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 2416e5b4..0731932f 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -11,8 +11,9 @@ #TODO is "Robot" even necessary from isaacsim.core.api.robots import Robot # type: ignore from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics# type: ignore -import omni.isaac.core.utils.stage as stage_utils # type: ignore from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore +from omni.isaac.core.utils.prims import define_prim +from omni.isaac.core.utils.prims import create_prim @@ -56,6 +57,9 @@ def connect(self, joint_names=None, camera_id=-1): # Load the robot from USD file assets_root_path = get_assets_root_path() + #if self.robot_config.robot_type == "h1": + # robot_usd_path = "/home/steffen/Downloads/h1_wrist.usd" + #else: 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}") @@ -63,6 +67,15 @@ def connect(self, joint_names=None, camera_id=-1): usd_path=robot_usd_path, prim_path=self.prim_path, ) + + + + #if self.robot_config.robot_type == "h1": + # self.create_new_link_with_joint(parent_link = "/right_elbow_link", link_name = "/right_wrist_link", joint_name = "/right_wrist_joint") + + + + robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) @@ -90,6 +103,8 @@ def connect(self, joint_names=None, camera_id=-1): # Reset the world to initialize physics self.world.reset() + + # Get joint information self.joint_pos_addrs = [] @@ -106,7 +121,7 @@ def connect(self, joint_names=None, camera_id=-1): # Validate joint names and get indices all_joint_names = self.articulation.dof_names - + print(all_joint_names) for name in joint_names: if name not in all_joint_names: raise Exception(f"Joint name {name} does not exist in robot model") @@ -130,8 +145,14 @@ def connect(self, joint_names=None, camera_id=-1): self.prim_path, ) - if self.robot_config.robot_type == "h1": + if self.robot_config.robot_type == "h1_hands": self.world.add_physics_callback("send_actions", self.send_actions) + + + + + + @@ -143,11 +164,20 @@ def _map_joint_names(self, joint_names): actual_joint_names = self.articulation.dof_names if self.name is "h1": - joint_list = ['torso_joint', # 1 - 'right_shoulder_pitch_joint', # 2 - 'right_shoulder_roll_joint', # 3 - 'right_shoulder_yaw_joint', # 4 - 'right_elbow_joint'] # 5 + joint_list = [#'torso_joint', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint'] , + #'right_wrist_joint'] + return np.array(joint_list) + + elif self.name is "h1_hands": + joint_list = ['right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_hand_joint'] return np.array(joint_list) else: # If input names are in MuJoCo format (joint0, joint1, etc.) @@ -299,9 +329,9 @@ def create_target_prim(self, prim_path="/World/target_cube", position=np.array([ cube_prim.GetPrim().CreateAttribute("physics:collisionEnabled", Sdf.ValueTypeNames.Bool).Set(False) return cube_prim - + + - #TODO not sure why this is necessary def set_gains_force_control(self): """Properly set gains for arm joints (DOFs 0-5) and finger joints if present""" @@ -310,9 +340,7 @@ def set_gains_force_control(self): damping = np.ones(self.robot_config.N_ALL_JOINTS) * 10.0 # Default damping # Set controlled arm joints to zero stiffness for force control - arm_joint_indices = list(range(self.robot_config.N_JOINTS)) - - for idx in arm_joint_indices: + for idx in self.joint_pos_addrs: stiffness[idx] = 0.0 # Zero stiffness = force control damping[idx] = 0.1 # Low damping for responsiveness @@ -326,7 +354,7 @@ def set_gains_force_control(self): damping[idx] = 5.0 # Moderate damping for fingers self.articulation_view.set_gains(stiffness, damping) - print(f"Set gains for force control for arm joints {arm_joint_indices }") + print(f"Set gains for force control for arm joints {self.joint_pos_addrs}") @@ -347,6 +375,47 @@ def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): print(f"Created virtual EE link at {ee_prim_path}") + # TODO check if all necessary or what could be omitted + # TODO maybe add offest? + def create_new_link_with_joint(self, parent_link , link_name, joint_name): + print ("IN METHOD") + right_elbow_link = self.prim_path + parent_link + right_wrist_link = self.prim_path + link_name + define_prim(right_wrist_link, "Xform") + + create_prim( + prim_path= right_wrist_link + "/geometry", + prim_type="Sphere", + attributes={"radius": 0.02}, + ) + + # Apply physics APIs + UsdPhysics.RigidBodyAPI.Apply(self.stage.GetPrimAtPath(right_wrist_link)) + UsdPhysics.CollisionAPI.Apply(self.stage.GetPrimAtPath(right_wrist_link)) + mass_api = UsdPhysics.MassAPI.Apply(self.stage.GetPrimAtPath(right_wrist_link)) + mass_api.CreateMassAttr(0.1) + + # Create joint at ROOT level (not under elbow link) + #joint_prim_path = self.prim_path + joint_name + joint_prim_path = right_elbow_link + joint_name + joint = UsdPhysics.RevoluteJoint.Define(self.stage, joint_prim_path) + joint.CreateBody0Rel().SetTargets([right_elbow_link]) + joint.CreateBody1Rel().SetTargets([right_wrist_link]) + joint.CreateAxisAttr().Set("Z") + joint.CreateLowerLimitAttr().Set(-3.14) + joint.CreateUpperLimitAttr().Set(3.14) + + # Drive API makes the joint controllable and appear in dof_names + drive_api = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "angular") + drive_api.CreateTypeAttr("force") # or "position" + drive_api.CreateMaxForceAttr(1000.0) + + # Add joint name for articulation system + joint.GetPrim().CreateAttribute("physics:jointName", Sdf.ValueTypeNames.String).Set("right_wrist_joint") + + + self.robot_config.EE_parent_link = "right_wrist_link" + ''' From ff9299a08b002f8af193cd500136f23f0f45c8f2 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Wed, 16 Jul 2025 13:23:33 -0400 Subject: [PATCH 32/42] adapted method send_forces to work with abritary list of joint indices --- abr_control/interfaces/nv_isaacsim.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 0731932f..5fe5fd7c 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -203,11 +203,11 @@ def disconnect(self): def send_forces(self, u): - """Applies the set of torques u to the arm - now working correctly!""" + """Applies the torques u to the joints specified in indices.""" # Create full torque vector for all DOFs full_torques = np.zeros(self.robot_config.N_ALL_JOINTS) # Apply control torques to the controlled joints - full_torques[:len(u)] = u + full_torques[self.joint_vel_addrs] = u # Apply the control signal self.articulation_view.set_joint_efforts(full_torques) # Move simulation ahead one time step From b1cb9ea6b1b2f75d6539f77c18f4361138fc2f47 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 17 Jul 2025 11:25:33 -0400 Subject: [PATCH 33/42] fixed helper methods for arbitrary indices generalized target generation --- abr_control/arms/isaacsim_config.py | 74 +++----------- abr_control/interfaces/nv_isaacsim.py | 105 ++------------------ examples/IsaacSim/force_floating_control.py | 16 +-- examples/IsaacSim/force_osc_xyz.py | 24 +++-- 4 files changed, 52 insertions(+), 167 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 3a028ed7..7225f427 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -74,6 +74,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F False: if the meshes folder is missing it will ask the user whether they want to download them """ + self.robot_type = robot_type #TODO obsolete, from mujoco self.use_sim_state = use_sim_state @@ -86,6 +87,8 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.has_EE = False # UR5 has no end-effector self.EE_parent_link = "wrist_3_link" START_ANGLES = "0 -.67 -.67 0 0 0" + self.target_min = np.array([-0.5, -0.5, 0.5]) + self.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "jaco2": @@ -93,6 +96,8 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.has_EE = True # jaco2 has an end-effector self.ee_link_name = "j2n6s300_end_effector" START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" + self.target_min = np.array([-0.5, -0.5, 0.5]) + self.joint_names = ['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'] print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") elif self.robot_type == "h1": @@ -100,6 +105,9 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.has_EE = False # H1 has no end-effector self.EE_parent_link = "right_elbow_link" START_ANGLES = "0 0 0 0" + self.target_min = np.array([0.1, -0.55, 1.4]) + self.joint_names = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint'] + print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "h1_hands": @@ -107,7 +115,9 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.has_EE = True # H1 has no end-effector #self.EE_parent_link = "right_elbow_link" self.ee_link_name = "right_hand_link" - START_ANGLES = "0 0 0 0" + START_ANGLES = "0 0 0 0 0" + self.target_min = np.array([0.1, -0.55, 1.4]) + self.joint_names = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint','right_hand_joint'] print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") @@ -143,7 +153,6 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path self.N_JOINTS = len(self.joint_vel_addrs) - # number of joints in the IsaacSim simulation N_ALL_JOINTS = self.articulation_view.num_dof # need to calculate the joint_vel_addrs indices in flat vectors returned @@ -172,12 +181,6 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.N_ALL_JOINTS = N_ALL_JOINTS - - - - - - def g(self, q=None): """ Returns the gravity and Coriolis/centrifugal forces for the controlled arm_joints. @@ -193,6 +196,7 @@ def g(self, q=None): g = gravity + coriolis return -g + @@ -253,7 +257,7 @@ def J(self, name, q=None, x=None, object_type="body"): link_index = 5 elif self.robot_type is "jaco2": - link_index = 6 + link_index = 5 # [6, 10, 14, 18] elif self.robot_type is "h1": link_index = 18 @@ -311,52 +315,6 @@ def J(self, name, q=None, x=None, object_type="body"): raise ValueError(f"Invalid object type specified: {object_type}") return np.copy(self._J6N) - - - ''' - - def J(self, name, q=None, x=None, object_type="body"): - - """ - Returns the Jacobian for a specified end-effector link, projected onto controlled joints. - - Parameters - ---------- - ee_link_name : str - Name of the end-effector link (e.g., 'right_wrist_link') - controlled_joint_names : list of str - List of joint names to keep in the Jacobian columns. - - Returns - ------- - np.ndarray - (6, num_controlled_dofs) Jacobian [linear; angular] - """ - if name == "EE": - name = self.ee_link_name - - # Make sure physics is stepped - self.world.step(render=False) - - # Get full Jacobians - jacobians = self.articulation_view.get_jacobians(clone=True) # (1, num_links, 6, num_dofs) - - # Get link index - #link_index = self.articulation_view.body_names.index(name) - link_index = self.articulation_view.body_names.index(self.EE_parent_link) - - - # Full 6 x N Jacobian for this link - #J_full = jacobians[0, link_index, :, :] # shape (6, num_dofs) - J_full = jacobians[0, link_index, :, :] # shape (6, num_dofs) - - - # Select only columns corresponding to controlled joints - #print("selected indices: ", self.joint_pos_addrs) - J_projected = J_full[:, self.joint_pos_addrs] # shape (6, num_controlled_dofs) - - return J_projected - ''' @@ -543,8 +501,7 @@ def set_joint_positions(self, q): """Set joint positions""" if hasattr(self, 'articulation'): full_positions = self.articulation.get_joint_positions() - for i, idx in enumerate(range(len(q))): - full_positions[idx] = q[i] + full_positions[self.joint_vel_addrs] = q self.articulation.set_joint_positions(full_positions) @@ -552,8 +509,7 @@ def set_joint_velocities(self, dq): """Set joint velocities""" if hasattr(self, 'articulation'): full_velocities = self.articulation.get_joint_velocities() - for i, idx in enumerate(range(len(dq))): - full_velocities[idx] = dq[i] + full_velocities[self.joint_vel_addrs] = dq self.articulation.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 index 5fe5fd7c..b322aab2 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -12,9 +12,6 @@ from isaacsim.core.api.robots import Robot # type: ignore from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics# type: ignore from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore -from omni.isaac.core.utils.prims import define_prim -from omni.isaac.core.utils.prims import create_prim - class IsaacSim(Interface): @@ -69,12 +66,6 @@ def connect(self, joint_names=None, camera_id=-1): ) - - #if self.robot_config.robot_type == "h1": - # self.create_new_link_with_joint(parent_link = "/right_elbow_link", link_name = "/right_wrist_link", joint_name = "/right_wrist_joint") - - - robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) @@ -201,7 +192,6 @@ def disconnect(self): self.simulation_app.close() # close Isaac Sim print("IsaacSim connection closed...") - def send_forces(self, u): """Applies the torques u to the joints specified in indices.""" # Create full torque vector for all DOFs @@ -209,10 +199,11 @@ def send_forces(self, u): # Apply control torques to the controlled joints full_torques[self.joint_vel_addrs] = u # Apply the control signal + #TODO maybe outsource as done for position 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): """Moves the arm to the specified joint angles @@ -292,11 +283,13 @@ def send_actions(self, dt): pelvis_prim_path = '/World/robot/pelvis' prim = self.stage.GetPrimAtPath(pelvis_prim_path) 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, 0.85)) + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3f(0.0, 0.0, 1.4)) #prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(0.70711 ,0.70711 ,0.0 ,0.0)) + + - + #TODO check if position is even set here. maybe remove # Create a visual-only cube (no collision) def create_target_prim(self, prim_path="/World/target_cube", position=np.array([0, 0, 1.0]), size = .1, color=np.array([0, 0, 1.0])): # Create cube geometry @@ -307,6 +300,8 @@ def create_target_prim(self, prim_path="/World/target_cube", position=np.array([ xformable = UsdGeom.Xformable(cube_prim) transform_matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(position[0], position[1], position[2])) xformable.MakeMatrixXform().Set(transform_matrix) + # xformable.AddTranslateOp().Set(Gf.Vec3f(*position)) + # Create and apply material for color material_path = prim_path + "/Material" @@ -329,8 +324,8 @@ def create_target_prim(self, prim_path="/World/target_cube", position=np.array([ cube_prim.GetPrim().CreateAttribute("physics:collisionEnabled", Sdf.ValueTypeNames.Bool).Set(False) return cube_prim - + def set_gains_force_control(self): """Properly set gains for arm joints (DOFs 0-5) and finger joints if present""" @@ -355,9 +350,8 @@ def set_gains_force_control(self): self.articulation_view.set_gains(stiffness, damping) print(f"Set gains for force control for arm joints {self.joint_pos_addrs}") - - + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): """Add virtual end effector link as an Xform under the specified parent link""" # Full path to parent @@ -373,82 +367,3 @@ def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): ee_prim.AddTranslateOp().Set(Gf.Vec3d(*offset)) print(f"Created virtual EE link at {ee_prim_path}") - - - # TODO check if all necessary or what could be omitted - # TODO maybe add offest? - def create_new_link_with_joint(self, parent_link , link_name, joint_name): - print ("IN METHOD") - right_elbow_link = self.prim_path + parent_link - right_wrist_link = self.prim_path + link_name - define_prim(right_wrist_link, "Xform") - - create_prim( - prim_path= right_wrist_link + "/geometry", - prim_type="Sphere", - attributes={"radius": 0.02}, - ) - - # Apply physics APIs - UsdPhysics.RigidBodyAPI.Apply(self.stage.GetPrimAtPath(right_wrist_link)) - UsdPhysics.CollisionAPI.Apply(self.stage.GetPrimAtPath(right_wrist_link)) - mass_api = UsdPhysics.MassAPI.Apply(self.stage.GetPrimAtPath(right_wrist_link)) - mass_api.CreateMassAttr(0.1) - - # Create joint at ROOT level (not under elbow link) - #joint_prim_path = self.prim_path + joint_name - joint_prim_path = right_elbow_link + joint_name - joint = UsdPhysics.RevoluteJoint.Define(self.stage, joint_prim_path) - joint.CreateBody0Rel().SetTargets([right_elbow_link]) - joint.CreateBody1Rel().SetTargets([right_wrist_link]) - joint.CreateAxisAttr().Set("Z") - joint.CreateLowerLimitAttr().Set(-3.14) - joint.CreateUpperLimitAttr().Set(3.14) - - # Drive API makes the joint controllable and appear in dof_names - drive_api = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "angular") - drive_api.CreateTypeAttr("force") # or "position" - drive_api.CreateMaxForceAttr(1000.0) - - # Add joint name for articulation system - joint.GetPrim().CreateAttribute("physics:jointName", Sdf.ValueTypeNames.String).Set("right_wrist_joint") - - - self.robot_config.EE_parent_link = "right_wrist_link" - - ''' - - - def set_gains_position_control_min(self): - """Set up position control with high stiffness""" - n_dofs = self.robot_config.N_JOINTS - stiffness = np.ones(n_dofs) * 1000.0 # High stiffness for position control - damping = np.ones(n_dofs) * 100.0 # High damping for stability - - self.articulation_view.set_gains(stiffness, damping) - - - def set_gains_position_control(self): - """Properly set gains for arm joints for position control and finger joints if present""" - # Get current gains or set defaults - stiffness = np.ones(self.robot_config.N_ALL_JOINTS) * 100.0 # Default high stiffness - damping = np.ones(self.robot_config.N_ALL_JOINTS) * 10.0 # Default damping - - # Set controlled arm joints to HIGH stiffness for position control - arm_joint_indices = list(range(self.robot_config.N_JOINTS)) - for idx in arm_joint_indices: - stiffness[idx] = 1000.0 # High stiffness = position control - damping[idx] = 100.0 # High damping for stability - - # Keep finger joints with some stiffness if you want them stable - if self.robot_config.N_ALL_JOINTS > self.robot_config.N_JOINTS: - dof_names = self.articulation_view.dof_names - finger_joint_indices = list(range(self.robot_config.N_JOINTS, self.robot_config.N_ALL_JOINTS)) - for idx in finger_joint_indices: - if "finger" in dof_names[idx].lower(): - stiffness[idx] = 50.0 # Moderate stiffness for fingers - damping[idx] = 5.0 # Moderate damping for fingers - - self.articulation_view.set_gains(stiffness, damping) - print(f"Set gains for position control for arm joints {arm_joint_indices }") - ''' \ No newline at end of file diff --git a/examples/IsaacSim/force_floating_control.py b/examples/IsaacSim/force_floating_control.py index c0c13565..623d6ec5 100644 --- a/examples/IsaacSim/force_floating_control.py +++ b/examples/IsaacSim/force_floating_control.py @@ -15,14 +15,15 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "ur5" - #arm_model = "jaco2" + arm_model = "h1_hands" + #arm_model = "ur5" # initialize our robot config robot_config = arm(arm_model) # create the IsaacSim interface and connect up -interface = IsaacSim(robot_config, dt=0.001) +interface = IsaacSim(robot_config, dt = 0.007) interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) interface.send_target_angles(robot_config.START_ANGLES) +interface.set_gains_force_control() # instantiate the controller @@ -36,7 +37,7 @@ try: # get the end-effector's initial position feedback = interface.get_feedback() - start = robot_config.Tx(interface.ee_link_name, q=feedback["q"]) + start = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"]) print("\nSimulation starting...\n") while 1: # get joint angle and velocity feedback @@ -45,11 +46,12 @@ # calculate the control signal u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"]) - # send forces into Mujoco + # send forces into Isaacsim interface.send_forces(u) + #interface.world.step(render=True) # calculate the position of the hand - hand_xyz = robot_config.Tx(interface.ee_link_name, q=feedback["q"]) + 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"])) @@ -60,7 +62,7 @@ finally: # close the connection to the arm - interface.disconnect() + #interface.disconnect() print("Simulation terminated...") diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index baf4d151..1f81daa1 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -17,7 +17,7 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "ur5" + arm_model = "ur5" #"h1_hands" robot_config = arm(arm_model) dt = 0.007 # 143 Hz @@ -28,11 +28,11 @@ interface = IsaacSim(robot_config, dt=dt) interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) -#dof_names = interface.articulation.dof_names -#print("dof_names: " + dof_names) + interface.send_target_angles(robot_config.START_ANGLES) -isaac_target = interface.create_target_prim(prim_path=target_prim_path) +#isaac_target = interface.create_target_prim(prim_path=target_prim_path) +interface.create_target_prim(target_prim_path) interface.set_gains_force_control() @@ -59,12 +59,25 @@ red = [0.9, 0, 0, 0.5] np.random.seed(0) +''' def gen_target(interface): target_xyz = (np.random.rand(3) + np.array([-0.5, -0.5, 0.5])) * np.array( [1, 1, 0.5] ) interface.set_xyz(target_prim_path, target_xyz) + +def gen_target(interface): + target_xyz = (np.random.rand(3) + np.array([0.1, -0.5, 0.5])) * np.array([1, -0.1, 1.5]) + interface.set_xyz(target_prim_path, target_xyz) +''' + +def gen_target(interface): + target_min = robot_config.target_min + target_range = np.array([1, 1, 0.5]) + target_xyz = (target_min + np.random.rand(3)) * target_range + interface.set_xyz(target_prim_path, target_xyz) + try: # get the end-effector's initial position feedback = interface.get_feedback() @@ -103,7 +116,6 @@ def gen_target(interface): #interface.world.step(render=True) - # calculate end-effector position ee_xyz = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"]) # track data @@ -167,4 +179,4 @@ def gen_target(interface): c="g", ) ax2.legend() - plt.show() + plt.show() \ No newline at end of file From 15dca0a8528566fdfef58f02dbf2c0eea9efd9a1 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Wed, 30 Jul 2025 14:57:49 -0400 Subject: [PATCH 34/42] removed coriolis forces from function for gravity compensation --- abr_control/arms/isaacsim_config.py | 55 +++++--------- abr_control/interfaces/nv_isaacsim.py | 100 ++++++++++++++++++++------ 2 files changed, 97 insertions(+), 58 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 7225f427..315407c7 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -1,13 +1,7 @@ -from xml.etree import ElementTree import numpy as np -from abr_control.utils import download_meshes import omni - - - - class IsaacsimConfig: """A wrapper on the Mujoco simulator to generate all the kinematics and dynamics calculations necessary for controllers. @@ -83,41 +77,45 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.ee_link_name = "EE" # name used for virtual end-effector, overwritten if one exists already if self.robot_type == "ur5": + self.ctrlr_dof = [True, True, True, False, False, False] 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" START_ANGLES = "0 -.67 -.67 0 0 0" self.target_min = np.array([-0.5, -0.5, 0.5]) - self.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] + self.controlled_joints = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "jaco2": + self.ctrlr_dof = [True, True, True, False, False, False] self.robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" self.has_EE = True # jaco2 has an end-effector self.ee_link_name = "j2n6s300_end_effector" START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" self.target_min = np.array([-0.5, -0.5, 0.5]) - self.joint_names = ['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'] + self.controlled_joints = ['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'] print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") elif self.robot_type == "h1": + self.ctrlr_dof = [True, True, True, False] 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" - START_ANGLES = "0 0 0 0" + START_ANGLES = "0. 0. 0. 0." self.target_min = np.array([0.1, -0.55, 1.4]) - self.joint_names = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint'] + self.controlled_joints = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint'] print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "h1_hands": + self.ctrlr_dof = [True, True, True, False, False] self.robot_path = "/Isaac/Robots/Unitree/H1/h1_with_hand.usd" self.has_EE = True # H1 has no end-effector #self.EE_parent_link = "right_elbow_link" self.ee_link_name = "right_hand_link" - START_ANGLES = "0 0 0 0 0" + START_ANGLES = "0. 0. 0. 0. 0." self.target_min = np.array([0.1, -0.55, 1.4]) - self.joint_names = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint','right_hand_joint'] + self.controlled_joints = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint','right_hand_joint'] print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") @@ -191,12 +189,11 @@ def g(self, q=None): """ # Compute gravity and Coriolis/centrifugal separately gravity = self.articulation_view.get_generalized_gravity_forces(joint_indices=self.joint_pos_addrs)[0] - coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.joint_pos_addrs)[0] - - g = gravity + coriolis + #coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.joint_pos_addrs)[0] + #g = gravity + coriolis - return -g - + return -gravity + #return -g @@ -245,13 +242,6 @@ def J(self, name, q=None, x=None, object_type="body"): # Check if ArticulationView has any environments if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") - - # jaco2 version - #link_index = self.articulation_view.get_link_index(name) - #link_index = self.N_JOINTS -1 - #print("link_index old: ", link_index) - #link_index = self.articulation_view.get_link_index(name) - #print("name: ", name, " link_index new: ", link_index) if self.robot_type is "ur5": link_index = 5 @@ -264,10 +254,7 @@ def J(self, name, q=None, x=None, object_type="body"): # [6, 10, 14, 18, 20] elif self.robot_type is "h1_hands": link_index = 20 - - - # Extract Jacobian for specific link env_idx = 0 # Assuming single environment # shape is (1, 13, 6, 12) @@ -317,9 +304,6 @@ def J(self, name, q=None, x=None, object_type="body"): return np.copy(self._J6N) - - - def M(self, q=None): """ Returns the inertia matrix for the controlled arm_joints. @@ -405,6 +389,7 @@ def quaternion(self, name, q=None): return quat_np + def C(self, q=None, dq=None): """NOTE: The Coriolis and centrifugal effects (and gravity) are already accounted for by Mujoco in the qfrc_bias variable. There's @@ -412,10 +397,9 @@ def C(self, q=None, dq=None): To prevent accounting for these effects twice, this function will return an error instead of qfrc_bias again. """ - raise NotImplementedError( - "Coriolis and centrifugal effects already accounted " - + "for in the term return by the gravity function." - ) + coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.joint_pos_addrs)[0] + return coriolis + def T(self, name, q=None, x=None): """Get the transform matrix of the specified body. @@ -486,7 +470,7 @@ def get_joint_positions(self): """Get current joint positions""" if hasattr(self, 'articulation'): all_positions = self.articulation.get_joint_positions() - return all_positions[self.joint_vel_addrs] + return all_positions[self.joint_pos_addrs] return None def get_joint_velocities(self): @@ -512,4 +496,3 @@ def set_joint_velocities(self, dq): full_velocities[self.joint_vel_addrs] = dq self.articulation.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 index b322aab2..77701e1a 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -1,4 +1,5 @@ import numpy as np +import math from isaacsim import SimulationApp from .interface import Interface simulation_app = SimulationApp({"headless": False}) @@ -12,6 +13,8 @@ from isaacsim.core.api.robots import Robot # type: ignore from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics# type: ignore from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore +from isaacsim.robot.policy.examples.robots import H1FlatTerrainPolicy # type: ignore +import omni.isaac.core.utils.numpy.rotations as rot_utils # type: ignore class IsaacSim(Interface): @@ -33,6 +36,7 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.dt = dt # time step #self.count = 0 # keep track of how many times send forces is called self.prim_path = "/World/robot" + #remove? self.name = self.robot_config.robot_type @@ -50,24 +54,31 @@ def connect(self, joint_names=None, camera_id=-1): 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() - #if self.robot_config.robot_type == "h1": - # robot_usd_path = "/home/steffen/Downloads/h1_wrist.usd" - #else: 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}") - - stage_utils.add_reference_to_stage( - usd_path=robot_usd_path, - prim_path=self.prim_path, - ) + if self.robot_config.robot_type.startswith("h1"): + self.h1 = H1FlatTerrainPolicy( + prim_path=self.prim_path, + name=self.name, + usd_path=robot_usd_path, + position=np.array([0, 0 , 0]), + orientation=rot_utils.euler_angles_to_quats(np.array([0, 0, 0]), degrees=True), + ) + else: + 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.name)) + + - robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) self.world.reset() @@ -136,7 +147,7 @@ def connect(self, joint_names=None, camera_id=-1): self.prim_path, ) - if self.robot_config.robot_type == "h1_hands": + if self.robot_config.robot_type.startswith("h1"): self.world.add_physics_callback("send_actions", self.send_actions) @@ -192,17 +203,57 @@ def disconnect(self): self.simulation_app.close() # close Isaac Sim print("IsaacSim connection closed...") + + def send_forces(self, u): """Applies the torques u to the joints specified in indices.""" + #print ("send forces") # Create full torque vector for all DOFs full_torques = np.zeros(self.robot_config.N_ALL_JOINTS) # Apply control torques to the controlled joints - full_torques[self.joint_vel_addrs] = u + full_torques[self.joint_pos_addrs] = u # Apply the control signal #TODO maybe outsource as done for position self.articulation_view.set_joint_efforts(full_torques) # Move simulation ahead one time step self.world.step(render=True) + ''' + + + def send_forces(self, u): + """Applies the torques u to the joints specified in indices.""" + + #def test_single_joint_force(self, joint_index, force_value=2.0): + + + joint_list = ['right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_hand_joint'] + + # Create zero torque array + test_torques = np.zeros(self.robot_config.N_ALL_JOINTS) + + test_torques[self.joint_pos_addrs] = u * 0.1 + + + print(f"Applied torque: {u} ") + print(f"Torque array: {test_torques}") + + # Apply the torque + self.articulation_view.set_joint_efforts(test_torques) + + # Let it run for a moment to observe + for _ in range(100): # Run for ~1 second at 100Hz + self.world.step(render=True) + + print("Observe which joint moved and in what direction") + print("Press Enter to continue to next joint...") + input() + ''' + + def send_target_angles(self, q): @@ -339,19 +390,24 @@ def set_gains_force_control(self): stiffness[idx] = 0.0 # Zero stiffness = force control damping[idx] = 0.1 # Low damping for responsiveness - # Keep finger joints with some stiffness if you want them stable - if self.robot_config.N_ALL_JOINTS > self.robot_config.N_JOINTS: - dof_names = self.articulation_view.dof_names - finger_joint_indices = list(range(self.robot_config.N_JOINTS, self.robot_config.N_ALL_JOINTS)) - for idx in finger_joint_indices: - if "finger" in dof_names[idx].lower(): - stiffness[idx] = 50.0 # Moderate stiffness for fingers - damping[idx] = 5.0 # Moderate damping for fingers - + self.articulation_view.set_gains(stiffness, damping) print(f"Set gains for force control for arm joints {self.joint_pos_addrs}") + + def set_gains_force_control_h1(self): + stiffness = np.ones(self.robot_config.N_ALL_JOINTS) * 100.0 + damping = np.ones(self.robot_config.N_ALL_JOINTS) * 10.0 + + for idx in self.joint_pos_addrs: + stiffness[idx] = 20.0 # 4. # Small but non-zero stiffness + damping[idx] = 5.0 # 1.0 # Higher damping for stability + + self.articulation_view.set_gains(stiffness, damping) + + + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): """Add virtual end effector link as an Xform under the specified parent link""" # Full path to parent From 4d5123666e8836b895c8bfce86ab478a236a4fc4 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Wed, 6 Aug 2025 14:24:19 -0400 Subject: [PATCH 35/42] fixed indexing of jacobian due to non-fixed base --- abr_control/arms/isaacsim_config.py | 125 ++++++++++------- abr_control/interfaces/nv_isaacsim.py | 193 +++++++++++++------------- examples/IsaacSim/force_osc_xyz.py | 40 +++--- 3 files changed, 191 insertions(+), 167 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 315407c7..eccafdb7 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -77,7 +77,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.ee_link_name = "EE" # name used for virtual end-effector, overwritten if one exists already if self.robot_type == "ur5": - self.ctrlr_dof = [True, True, True, False, False, False] + #self.ctrlr_dof = [True, True, True, False, False, False] 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" @@ -87,7 +87,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "jaco2": - self.ctrlr_dof = [True, True, True, False, False, False] + #self.ctrlr_dof = [True, True, True, False, False, False] self.robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" self.has_EE = True # jaco2 has an end-effector self.ee_link_name = "j2n6s300_end_effector" @@ -97,10 +97,11 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") elif self.robot_type == "h1": - self.ctrlr_dof = [True, True, True, False] + #self.ctrlr_dof = [True, True, True, False] 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_link_name = "right_elbow_link" START_ANGLES = "0. 0. 0. 0." self.target_min = np.array([0.1, -0.55, 1.4]) self.controlled_joints = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint'] @@ -108,9 +109,9 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "h1_hands": - self.ctrlr_dof = [True, True, True, False, False] + #self.ctrlr_dof = [True, True, True, False, False] self.robot_path = "/Isaac/Robots/Unitree/H1/h1_with_hand.usd" - self.has_EE = True # H1 has no end-effector + 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" START_ANGLES = "0. 0. 0. 0. 0." @@ -124,7 +125,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F - def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_vel_addrs, prim_path): + def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_alt_pos_addrs, joint_vel_addrs, prim_path): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -148,6 +149,8 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.articulation = articulation self.articulation_view = articulation_view self.joint_pos_addrs = np.copy(joint_pos_addrs) + # alternative joint addresses, used for the Jacobian + self.joint_alt_pos_addrs = np.copy(joint_alt_pos_addrs) self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path self.N_JOINTS = len(self.joint_vel_addrs) @@ -178,6 +181,12 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self._x = np.ones(4) self.N_ALL_JOINTS = N_ALL_JOINTS + ''' + def g(self, q=None): + g_full = self.articulation_view.get_generalized_gravity_forces()[0] + return -g_full[self.joint_pos_addrs] + ''' + def g(self, q=None): """ @@ -194,6 +203,7 @@ def g(self, q=None): return -gravity #return -g + @@ -220,75 +230,55 @@ def dJ(self, name, q=None, dq=None, x=None): - - def J(self, name, q=None, x=None, object_type="body"): - if name == "EE": - name = self.ee_link_name - - # Check for unsupported features - if x is not None and not np.allclose(x, 0): - raise Exception("x offset currently not supported, set to None") - + + + def J(self, name, q=None, x=None, object_type="body"): if object_type == "body": - # Get Jacobians from articulation view - ensure proper tensor handling jacobians = self.articulation_view.get_jacobians(clone=True) - - # Convert to numpy if it's a tensor - if hasattr(jacobians, 'cpu'): - jacobians = jacobians.cpu().numpy() - elif hasattr(jacobians, 'numpy'): - jacobians = jacobians.numpy() - - # Check if ArticulationView has any environments if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") - + + # TODO this could be fixed by attachind virtual EE if self.robot_type is "ur5": link_index = 5 - elif self.robot_type is "jaco2": link_index = 5 # [6, 10, 14, 18] elif self.robot_type is "h1": - link_index = 18 + #link_index = 18 + link_index = 24 # [6, 10, 14, 18, 20] elif self.robot_type is "h1_hands": - link_index = 20 - + #link_index = 20 + link_index = 25 + + env_idx = 0 # Assuming single environment - - # shape is (1, 13, 6, 12) + J_full = jacobians[env_idx, link_index, :, :] if len(jacobians.shape) == 4: - # Shape: (num_envs, num_bodies, 6, num_dofs) - J_full = jacobians[env_idx, link_index, :, :] - elif len(jacobians.shape) == 3: - # Shape: (num_envs, num_bodies * 6, num_dofs) - start_row = link_index * 6 - end_row = start_row + 6 - J_full = jacobians[env_idx, start_row:end_row, :] + # Fixed articulation base (robotic manipulators): (env, num_bodies - 1, 6, num_dof) + if self.is_fixed_base(): + jacobian_columns = self.joint_pos_addrs + + # Non-fixed articulation base (mobile robots): (env, num_bodies, 6, num_dof + base_dofs) + else: + jacobian_base_offset = 2 # Empirically determined for H1 + jacobian_columns = self.joint_pos_addrs + jacobian_base_offset + else: raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") - - # Extract only the columns for controllable DOFs - # This gives us the end-effector Jacobian w.r.t. only the arm joints - J = J_full[:, self.joint_pos_addrs] - - # Check for NaN or inf values - if np.any(np.isnan(J)) or np.any(np.isinf(J)): - raise RuntimeError("Jacobian contains NaN or infinite values") + + + # Extract only the columns for controllable DOFs + # This gives us the end-effector Jacobian w.r.t. only the right arm joints + J = J_full[:, jacobian_columns] + # Assign to internal storage # Linear velocity Jacobian (first 3 rows) self._J6N[:3, :] = J[:3, :] # Angular velocity Jacobian (last 3 rows) self._J6N[3:, :] = J[3:, :] - - #print(f"Robot type: {self.robot_type}") - #print(f"Link index: {link_index}") - #print(f"Arm joint indices: {self.joint_pos_addrs}") - #print(f"J_full shape: {J_full.shape}") - #print(f"J shape after filtering: {J.shape}") - #print(f"self.joint_pos_addrs: {self.joint_pos_addrs}") # Additional validation - check if Jacobian makes sense (non-zero values) if np.allclose(J, 0): @@ -304,6 +294,28 @@ def J(self, name, q=None, x=None, object_type="body"): return np.copy(self._J6N) + + + def M(self, q=None): + """ + Returns the inertia matrix for the controlled arm joints. + """ + M = self.articulation_view.get_mass_matrices() + M_full = M[0] + + # Fixed articulation base (robotic manipulators): + if self.is_fixed_base(): + M_arm = M_full[np.ix_(self.joint_pos_addrs, self.joint_pos_addrs)] + + # Non-fixed articulation base (mobile robots): + else: + controlled_indices = self.joint_pos_addrs + 6 + M_arm = M_full[np.ix_(controlled_indices, controlled_indices)] + + return np.copy(M_arm) + + + ''' def M(self, q=None): """ Returns the inertia matrix for the controlled arm_joints. @@ -321,6 +333,7 @@ def M(self, q=None): # extract only the controlled DOF M_arm = M_full[np.ix_(self.joint_pos_addrs, self.joint_pos_addrs)] return np.copy(M_arm) + ''' def R(self, name, q=None, object_type="body"): @@ -496,3 +509,9 @@ def set_joint_velocities(self, dq): full_velocities[self.joint_vel_addrs] = dq self.articulation.set_joint_velocities(full_velocities) + + def is_fixed_base(self): + num_dof = self.articulation_view.num_dof + jacobian_shape = self.articulation_view.get_jacobian_shape()[2] + return num_dof == jacobian_shape + diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 77701e1a..3e8943a8 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -110,29 +110,43 @@ def connect(self, joint_names=None, camera_id=-1): # Get joint information self.joint_pos_addrs = [] + self.joint_alt_pos_addrs = [] self.joint_vel_addrs = [] self.joint_dyn_addrs = [] if joint_names is None: print("No joint names provided, using all controllable joints in the articulation.") - # Get all controllable joints in the articulation - joint_names = self.articulation.dof_names - else: - # Handle joint name mapping - joint_names = self._map_joint_names(joint_names) + joint_names = self.robot_config.controlled_joints + + + # Validate joint names and get indices - all_joint_names = self.articulation.dof_names - print(all_joint_names) + self.all_dof_names = self.articulation.dof_names + # joint_names = self.articulation_view.joint_names # The 24-joint list + print(f"All dof names: {self.all_dof_names}") + print(f"All link names: {self.articulation_view.body_names}") + print(f"Provided joint names: {joint_names}") + + for name in joint_names: - if name not in all_joint_names: + if name not in self.all_dof_names: raise Exception(f"Joint name {name} does not exist in robot model") - - joint_idx = all_joint_names.index(name) - self.joint_pos_addrs.append(joint_idx) - self.joint_vel_addrs.append(joint_idx) + #print(f"name: {name}") + #link_name = name.replace("joint", "link") + #print(f"link_name: {link_name}") + + + #joint_idx = self.articulation_view.get_joint_index(name) + dof_idx = self.articulation_view.get_dof_index(name) + #print(f"dof_index: {dof_idx}") + #print(f"joint_idx: {joint_idx}") + # link_idx = self.articulation_view.get_link_index(link_name) + # print(f"link_idx: {link_idx}") + self.joint_pos_addrs.append(dof_idx) + self.joint_vel_addrs.append(dof_idx) #TODO check if joint_dyn_addrs necessary - self.joint_dyn_addrs.append(joint_idx) + self.joint_dyn_addrs.append(dof_idx) # Connect robot config with simulation data @@ -143,6 +157,7 @@ def connect(self, joint_names=None, camera_id=-1): self.articulation, self.articulation_view, self.joint_pos_addrs, + self.joint_alt_pos_addrs, self.joint_vel_addrs, self.prim_path, ) @@ -152,106 +167,83 @@ def connect(self, joint_names=None, camera_id=-1): - - - - def _map_joint_names(self, joint_names): - """ - Map joint names from MuJoCo format to IsaacSim format - """ - # Get actual joint names from the robot - actual_joint_names = self.articulation.dof_names - - if self.name is "h1": - joint_list = [#'torso_joint', - 'right_shoulder_pitch_joint', - 'right_shoulder_roll_joint', - 'right_shoulder_yaw_joint', - 'right_elbow_joint'] , - #'right_wrist_joint'] - return np.array(joint_list) - - elif self.name is "h1_hands": - joint_list = ['right_shoulder_pitch_joint', - 'right_shoulder_roll_joint', - 'right_shoulder_yaw_joint', - 'right_elbow_joint', - 'right_hand_joint'] - return np.array(joint_list) - else: - # If input names are in MuJoCo format (joint0, joint1, etc.) - if all(name.startswith('joint') and name[5:].isdigit() for name in joint_names): - # Map by index: joint0 -> first joint, joint1 -> second joint, etc. - mapped_names = [] - for name in joint_names: - joint_idx = int(name[5:]) # Extract number from "jointX" - if joint_idx < len(actual_joint_names): - mapped_names.append(actual_joint_names[joint_idx]) - else: - raise Exception(f"Joint index {joint_idx} out of range. Robot has {len(actual_joint_names)} joints.") - return mapped_names - - # If names are already in correct format, return as-is - return joint_names - def disconnect(self): """Any socket closing etc that must be done to properly shut down""" self.simulation_app.close() # close Isaac Sim print("IsaacSim connection closed...") - - + ''' def send_forces(self, u): """Applies the torques u to the joints specified in indices.""" - #print ("send forces") - # Create full torque vector for all DOFs - full_torques = np.zeros(self.robot_config.N_ALL_JOINTS) + + # Debug: Check array sizes and indices + print(f"=== SEND_FORCES DEBUG ===") + print(f"robot_config.N_ALL_JOINTS: {self.robot_config.N_ALL_JOINTS}") + print(f"articulation_view joint count: {len(self.articulation_view.joint_names)}") + print(f"joint_pos_addrs: {self.joint_pos_addrs}") + print(f"u shape: {u.shape}") + print(f"Max index in joint_pos_addrs: {max(self.joint_pos_addrs) if self.joint_pos_addrs else 'None'}") + + # Use the correct joint count for the articulation view + total_joints = len(self.articulation_view.joint_names) # Should be 24 + full_torques = np.zeros(total_joints) + # Apply control torques to the controlled joints full_torques[self.joint_pos_addrs] = u + + print(f"full_torques shape: {full_torques.shape}") + print(f"Non-zero torques at indices: {np.nonzero(full_torques)[0]}") + # Apply the control signal - #TODO maybe outsource as done for position self.articulation_view.set_joint_efforts(full_torques) + # Move simulation ahead one time step self.world.step(render=True) - ''' + ''' - def send_forces(self, u): - """Applies the torques u to the joints specified in indices.""" + + def debug_dof_mapping(self): + print("=== DOF MAPPING DEBUG ===") + print(f"robot_config.N_ALL_JOINTS: {self.robot_config.N_ALL_JOINTS}") + print(f"articulation.dof_names length: {len(self.articulation.dof_names)}") + print(f"articulation_view.dof_names length: {len(self.articulation_view.dof_names)}") + print(f"articulation_view.joint_names length: {len(self.articulation_view.joint_names)}") - #def test_single_joint_force(self, joint_index, force_value=2.0): + print("\nDOF names (actuated joints):") + dof_names = self.articulation.dof_names + for i, name in enumerate(dof_names): + print(f" {i}: {name}") + print("\nLooking for controlled joints in DOF names:") + for joint_name in self.robot_config.controlled_joints: + if joint_name in dof_names: + idx = dof_names.index(joint_name) + print(f" {joint_name}: DOF index {idx}") + else: + print(f" {joint_name}: NOT FOUND in DOF names!") - joint_list = ['right_shoulder_pitch_joint', - 'right_shoulder_roll_joint', - 'right_shoulder_yaw_joint', - 'right_elbow_joint', - 'right_hand_joint'] - - # Create zero torque array - test_torques = np.zeros(self.robot_config.N_ALL_JOINTS) - test_torques[self.joint_pos_addrs] = u * 0.1 - - print(f"Applied torque: {u} ") - print(f"Torque array: {test_torques}") - - # Apply the torque - self.articulation_view.set_joint_efforts(test_torques) - - # Let it run for a moment to observe - for _ in range(100): # Run for ~1 second at 100Hz - self.world.step(render=True) - - print("Observe which joint moved and in what direction") - print("Press Enter to continue to next joint...") - input() - ''' + def send_forces(self, u): + """Applies the torques u to the joints specified in indices.""" + #print ("send forces") + # Create full torque vector for all DOFs + full_torques = np.zeros(self.robot_config.N_ALL_JOINTS) + # Apply control torques to the controlled joints + full_torques[self.joint_pos_addrs] = u + # Apply the control signal + #TODO maybe outsource as done for position + self.articulation_view.set_joint_efforts(full_torques) + # Move simulation ahead one time step + self.world.step(render=True) + + + @@ -395,19 +387,33 @@ def set_gains_force_control(self): print(f"Set gains for force control for arm joints {self.joint_pos_addrs}") - + + def set_gains_force_control_h1(self): + + self.articulation_view.switch_control_mode( + mode="effort", + joint_indices=self.joint_pos_addrs + ) + stiffness = np.ones(self.robot_config.N_ALL_JOINTS) * 100.0 damping = np.ones(self.robot_config.N_ALL_JOINTS) * 10.0 - for idx in self.joint_pos_addrs: - stiffness[idx] = 20.0 # 4. # Small but non-zero stiffness - damping[idx] = 5.0 # 1.0 # Higher damping for stability + # Use the correct DOF indices + for dof_idx in self.joint_pos_addrs: + stiffness[dof_idx] = 20.0 + damping[dof_idx] = 5.0 + print(f"Set gains for DOF {dof_idx}: {self.all_dof_names[dof_idx]}") + + # Reshape for ArticulationView: (M, K) + stiffness = np.expand_dims(stiffness, axis=0) + damping = np.expand_dims(damping, axis=0) self.articulation_view.set_gains(stiffness, damping) + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): """Add virtual end effector link as an Xform under the specified parent link""" # Full path to parent @@ -423,3 +429,4 @@ def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): ee_prim.AddTranslateOp().Set(Gf.Vec3d(*offset)) print(f"Created virtual EE link at {ee_prim_path}") + diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index 1f81daa1..4c3f0cbf 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -17,7 +17,7 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "ur5" #"h1_hands" + arm_model = "h1" robot_config = arm(arm_model) dt = 0.007 # 143 Hz @@ -27,16 +27,17 @@ # create our IsaacSim interface interface = IsaacSim(robot_config, dt=dt) -interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) +#joint_names = [f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))] +#interface.connect(joint_names=robot_config.controlled_joints) +interface.connect() interface.send_target_angles(robot_config.START_ANGLES) -#isaac_target = interface.create_target_prim(prim_path=target_prim_path) -interface.create_target_prim(target_prim_path) +isaac_target = interface.create_target_prim(prim_path=target_prim_path) -interface.set_gains_force_control() - +#interface.set_gains_force_control() +interface.set_gains_force_control_h1() # damp the movements of the arm damping = Damping(robot_config, kv=30) #kv=10) @@ -47,7 +48,8 @@ null_controllers=[damping], vmax=[0.5, 0], # [m/s, rad/s] # control (x, y, z) out of [x, y, z, alpha, beta, gamma] - ctrlr_dof=[True, True, True, False, False, False], + ctrlr_dof= [True, True, True, False, False, False] + #ctrlr_dof= robot_config.ctrlr_dof ) # set up lists for tracking data @@ -59,18 +61,6 @@ red = [0.9, 0, 0, 0.5] np.random.seed(0) -''' -def gen_target(interface): - target_xyz = (np.random.rand(3) + np.array([-0.5, -0.5, 0.5])) * np.array( - [1, 1, 0.5] - ) - interface.set_xyz(target_prim_path, target_xyz) - - -def gen_target(interface): - target_xyz = (np.random.rand(3) + np.array([0.1, -0.5, 0.5])) * np.array([1, -0.1, 1.5]) - interface.set_xyz(target_prim_path, target_xyz) -''' def gen_target(interface): target_min = robot_config.target_min @@ -78,6 +68,12 @@ def gen_target(interface): target_xyz = (target_min + np.random.rand(3)) * target_range interface.set_xyz(target_prim_path, target_xyz) + + +interface.debug_dof_mapping() + + + try: # get the end-effector's initial position feedback = interface.get_feedback() @@ -104,7 +100,7 @@ def gen_target(interface): ), ] ) - + # calculate the control signal u = ctrlr.generate( q=feedback["q"], @@ -112,8 +108,10 @@ def gen_target(interface): target=target, ) + interface.send_forces(u) - #interface.world.step(render=True) + # interface.world.step(render=True) + # calculate end-effector position From 0e1a270ee6169801aabc6917c3381e9107dbbd7b Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 7 Aug 2025 12:56:08 -0400 Subject: [PATCH 36/42] fixed method M --- abr_control/arms/isaacsim_config.py | 102 +++++++++---------------- abr_control/interfaces/nv_isaacsim.py | 106 ++++++++++++++++++++------ 2 files changed, 118 insertions(+), 90 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index eccafdb7..f000e0cb 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -83,7 +83,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.EE_parent_link = "wrist_3_link" START_ANGLES = "0 -.67 -.67 0 0 0" self.target_min = np.array([-0.5, -0.5, 0.5]) - self.controlled_joints = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] + 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 with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "jaco2": @@ -93,7 +93,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.ee_link_name = "j2n6s300_end_effector" START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" self.target_min = np.array([-0.5, -0.5, 0.5]) - self.controlled_joints = ['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_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 with name '{self.ee_link_name}' specified in UDS, using it ...") elif self.robot_type == "h1": @@ -104,7 +104,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F #self.ee_link_name = "right_elbow_link" START_ANGLES = "0. 0. 0. 0." self.target_min = np.array([0.1, -0.55, 1.4]) - self.controlled_joints = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint'] + self.controlled_dof = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint'] print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") @@ -116,7 +116,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F self.ee_link_name = "right_hand_link" START_ANGLES = "0. 0. 0. 0. 0." self.target_min = np.array([0.1, -0.55, 1.4]) - self.controlled_joints = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint','right_hand_joint'] + self.controlled_dof = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint','right_hand_joint'] print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") @@ -125,7 +125,7 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F - def _connect(self, world, stage, articulation, articulation_view, joint_pos_addrs, joint_alt_pos_addrs, joint_vel_addrs, prim_path): + def _connect(self, world, stage, articulation, articulation_view, dof_indices, joint_indices, joint_vel_addrs, prim_path): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -135,7 +135,7 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr ---------- sim: MjSim The Mujoco Simulator object created by the Mujoco Interface class - joint_pos_addrs: np.array of ints + dof_indices: np.array of ints The index of the robot joints in the Mujoco simulation data joint position array joint_vel_addrs: np.array of ints @@ -148,43 +148,42 @@ def _connect(self, world, stage, articulation, articulation_view, joint_pos_addr self.stage = stage self.articulation = articulation self.articulation_view = articulation_view - self.joint_pos_addrs = np.copy(joint_pos_addrs) - # alternative joint addresses, used for the Jacobian - self.joint_alt_pos_addrs = np.copy(joint_alt_pos_addrs) + self.dof_indices = np.copy(dof_indices) + self.joint_indices = np.copy(joint_indices) self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path - self.N_JOINTS = len(self.joint_vel_addrs) - N_ALL_JOINTS = self.articulation_view.num_dof + 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 # need to calculate the joint_vel_addrs indices in flat vectors returned # for the Jacobian self.jac_indices = np.hstack( # 6 because position and rotation Jacobians are 3 x N_JOINTS - [self.joint_vel_addrs + (ii * N_ALL_JOINTS) for ii in range(3)] + [self.joint_vel_addrs + (ii * self.N_ALL_JOINTS) for ii in range(3)] ) # for the inertia matrix self.M_indices = [ - ii * N_ALL_JOINTS + jj - for jj in self.joint_vel_addrs - for ii in self.joint_vel_addrs + ii * self.N_ALL_JOINTS + jj + for jj in self.dof_indices + for ii in self.dof_indices ] - # a place to store data returned from Mujoco + # a place to store data returned from IsaacSim self._g = np.zeros(self.N_JOINTS) - self._J3NP = np.zeros((3, N_ALL_JOINTS)) - self._J3NR = np.zeros((3, N_ALL_JOINTS)) + self._J3NP = np.zeros((3, self.N_ALL_JOINTS)) + self._J3NR = np.zeros((3, self.N_ALL_JOINTS)) self._J6N = np.zeros((6, self.N_JOINTS)) - self._MNN = np.zeros((N_ALL_JOINTS, N_ALL_JOINTS)) + self._MNN = np.zeros((self.N_ALL_JOINTS, self.N_ALL_JOINTS)) self._R9 = np.zeros(9) self._R = np.zeros((3, 3)) self._x = np.ones(4) - self.N_ALL_JOINTS = N_ALL_JOINTS ''' def g(self, q=None): g_full = self.articulation_view.get_generalized_gravity_forces()[0] - return -g_full[self.joint_pos_addrs] + return -g_full[self.dof_indices] ''' @@ -197,8 +196,8 @@ def g(self, q=None): np.ndarray: Generalized bias forces for controlled DOF """ # Compute gravity and Coriolis/centrifugal separately - gravity = self.articulation_view.get_generalized_gravity_forces(joint_indices=self.joint_pos_addrs)[0] - #coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.joint_pos_addrs)[0] + gravity = self.articulation_view.get_generalized_gravity_forces(joint_indices=self.dof_indices)[0] + #coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.dof_indices)[0] #g = gravity + coriolis return -gravity @@ -229,7 +228,6 @@ def dJ(self, name, q=None, dq=None, x=None): raise NotImplementedError - def J(self, name, q=None, x=None, object_type="body"): @@ -237,7 +235,10 @@ def J(self, name, q=None, x=None, object_type="body"): jacobians = self.articulation_view.get_jacobians(clone=True) if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") - + + print("###################################") + print("jacobians shape: ", jacobians.shape) + print("###################################") # TODO this could be fixed by attachind virtual EE if self.robot_type is "ur5": link_index = 5 @@ -258,12 +259,12 @@ def J(self, name, q=None, x=None, object_type="body"): if len(jacobians.shape) == 4: # Fixed articulation base (robotic manipulators): (env, num_bodies - 1, 6, num_dof) if self.is_fixed_base(): - jacobian_columns = self.joint_pos_addrs + jacobian_columns = self.dof_indices # Non-fixed articulation base (mobile robots): (env, num_bodies, 6, num_dof + base_dofs) else: jacobian_base_offset = 2 # Empirically determined for H1 - jacobian_columns = self.joint_pos_addrs + jacobian_base_offset + jacobian_columns = self.dof_indices + jacobian_base_offset else: raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") @@ -294,46 +295,15 @@ def J(self, name, q=None, x=None, object_type="body"): return np.copy(self._J6N) - - def M(self, q=None): """ Returns the inertia matrix for the controlled arm joints. """ - M = self.articulation_view.get_mass_matrices() - M_full = M[0] - - # Fixed articulation base (robotic manipulators): - if self.is_fixed_base(): - M_arm = M_full[np.ix_(self.joint_pos_addrs, self.joint_pos_addrs)] - - # Non-fixed articulation base (mobile robots): - else: - controlled_indices = self.joint_pos_addrs + 6 - M_arm = M_full[np.ix_(controlled_indices, controlled_indices)] - - return np.copy(M_arm) - - - ''' - def M(self, q=None): - """ - Returns the inertia matrix for the controlled arm_joints. - Args: - q (np.ndarray, optional): Joint positions - indices (optional): Specific articulation indices - - Returns: - np.ndarray: Inertia matrix - """ - # get_mass_matrices returns (num_envs, dof_count, dof_count) - M = self.articulation_view.get_mass_matrices() - # If you have only one robot in ArticulationView - M_full = M[0] - # extract only the controlled DOF - M_arm = M_full[np.ix_(self.joint_pos_addrs, self.joint_pos_addrs)] - return np.copy(M_arm) - ''' + # get_mass_matrices returns (num_envs, joint_count, joint_count) + self._MNN= self.articulation_view.get_mass_matrices()[0] + # extract only the controlled joints + M = self._MNN[np.ix_(self.joint_indices, self.joint_indices)] + return np.copy(M) def R(self, name, q=None, object_type="body"): @@ -410,7 +380,7 @@ def C(self, q=None, dq=None): To prevent accounting for these effects twice, this function will return an error instead of qfrc_bias again. """ - coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.joint_pos_addrs)[0] + coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.dof_indices)[0] return coriolis @@ -483,7 +453,7 @@ def get_joint_positions(self): """Get current joint positions""" if hasattr(self, 'articulation'): all_positions = self.articulation.get_joint_positions() - return all_positions[self.joint_pos_addrs] + return all_positions[self.dof_indices] return None def get_joint_velocities(self): @@ -498,7 +468,7 @@ def set_joint_positions(self, q): """Set joint positions""" if hasattr(self, 'articulation'): full_positions = self.articulation.get_joint_positions() - full_positions[self.joint_vel_addrs] = q + full_positions[self.dof_indices] = q self.articulation.set_joint_positions(full_positions) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 3e8943a8..7a897fec 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -38,6 +38,11 @@ def __init__(self, robot_config, dt=0.001, force_download=False): self.prim_path = "/World/robot" #remove? self.name = self.robot_config.robot_type + + + + self.prev_target = None + self.prev_joints = None def connect(self, joint_names=None, camera_id=-1): @@ -109,41 +114,47 @@ def connect(self, joint_names=None, camera_id=-1): # Get joint information - self.joint_pos_addrs = [] - self.joint_alt_pos_addrs = [] + #self.joint_pos_addrs = [] + self.dof_indices = [] + self.joint_indices = [] self.joint_vel_addrs = [] self.joint_dyn_addrs = [] if joint_names is None: print("No joint names provided, using all controllable joints in the articulation.") - joint_names = self.robot_config.controlled_joints + joint_names = self.robot_config.controlled_dof - # Validate joint names and get indices + # Validate dof names and get indices self.all_dof_names = self.articulation.dof_names - # joint_names = self.articulation_view.joint_names # The 24-joint list - print(f"All dof names: {self.all_dof_names}") - print(f"All link names: {self.articulation_view.body_names}") - print(f"Provided joint names: {joint_names}") + self.all_joint_names = self.articulation_view.joint_names + self.all_body_names = self.articulation_view.body_names + #self.all_link_names = self.articulation_view.link_names + print(f"len dof names: {len(self.all_dof_names)}") + print(f"len joint names: {len(self.all_joint_names)}") + print(f"len body names: {len(self.all_body_names)}") + #print(f"len link names: {len(self.all_link_names)}") + #print(f"Provided joint names: {joint_names}") for name in joint_names: - if name not in self.all_dof_names: + if name not in self.all_dof_names and name not in self.all_joint_names: raise Exception(f"Joint name {name} does not exist in robot model") #print(f"name: {name}") #link_name = name.replace("joint", "link") #print(f"link_name: {link_name}") - #joint_idx = self.articulation_view.get_joint_index(name) + joint_idx = self.articulation_view.get_joint_index(name) dof_idx = self.articulation_view.get_dof_index(name) #print(f"dof_index: {dof_idx}") #print(f"joint_idx: {joint_idx}") # link_idx = self.articulation_view.get_link_index(link_name) # print(f"link_idx: {link_idx}") - self.joint_pos_addrs.append(dof_idx) + self.dof_indices.append(dof_idx) + self.joint_indices.append(joint_idx) self.joint_vel_addrs.append(dof_idx) #TODO check if joint_dyn_addrs necessary self.joint_dyn_addrs.append(dof_idx) @@ -156,8 +167,8 @@ def connect(self, joint_names=None, camera_id=-1): self.stage, self.articulation, self.articulation_view, - self.joint_pos_addrs, - self.joint_alt_pos_addrs, + self.dof_indices, + self.joint_indices, self.joint_vel_addrs, self.prim_path, ) @@ -220,7 +231,7 @@ def debug_dof_mapping(self): print(f" {i}: {name}") print("\nLooking for controlled joints in DOF names:") - for joint_name in self.robot_config.controlled_joints: + for joint_name in self.robot_config.controlled_dof: if joint_name in dof_names: idx = dof_names.index(joint_name) print(f" {joint_name}: DOF index {idx}") @@ -233,9 +244,9 @@ def send_forces(self, u): """Applies the torques u to the joints specified in indices.""" #print ("send forces") # Create full torque vector for all DOFs - full_torques = np.zeros(self.robot_config.N_ALL_JOINTS) + full_torques = np.zeros(self.robot_config.N_ALL_DOF) # Apply control torques to the controlled joints - full_torques[self.joint_pos_addrs] = u + full_torques[self.dof_indices] = u # Apply the control signal #TODO maybe outsource as done for position self.articulation_view.set_joint_efforts(full_torques) @@ -374,17 +385,17 @@ def set_gains_force_control(self): """Properly set gains for arm joints (DOFs 0-5) and finger joints if present""" # Get current gains or set defaults - stiffness = np.ones(self.robot_config.N_ALL_JOINTS) * 100.0 # Default high stiffness - damping = np.ones(self.robot_config.N_ALL_JOINTS) * 10.0 # Default damping + stiffness = np.ones(self.robot_config.N_ALL_DOF) * 100.0 # Default high stiffness + damping = np.ones(self.robot_config.N_ALL_DOF) * 10.0 # Default damping # Set controlled arm joints to zero stiffness for force control - for idx in self.joint_pos_addrs: + for idx in self.dof_indices: stiffness[idx] = 0.0 # Zero stiffness = force control damping[idx] = 0.1 # Low damping for responsiveness self.articulation_view.set_gains(stiffness, damping) - print(f"Set gains for force control for arm joints {self.joint_pos_addrs}") + print(f"Set gains for force control for arm joints {self.dof_indices}") @@ -393,14 +404,14 @@ def set_gains_force_control_h1(self): self.articulation_view.switch_control_mode( mode="effort", - joint_indices=self.joint_pos_addrs + joint_indices=self.dof_indices ) - stiffness = np.ones(self.robot_config.N_ALL_JOINTS) * 100.0 - damping = np.ones(self.robot_config.N_ALL_JOINTS) * 10.0 + stiffness = np.ones(self.robot_config.N_ALL_DOF) * 100.0 + damping = np.ones(self.robot_config.N_ALL_DOF) * 10.0 # Use the correct DOF indices - for dof_idx in self.joint_pos_addrs: + for dof_idx in self.dof_indices: stiffness[dof_idx] = 20.0 damping[dof_idx] = 5.0 print(f"Set gains for DOF {dof_idx}: {self.all_dof_names[dof_idx]}") @@ -430,3 +441,50 @@ def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): print(f"Created virtual EE link at {ee_prim_path}") + + def debug_target_and_joints(self): + """Debug target movement and controlled joint response""" + + target_pos = self.get_xyz("/World/target") + + # Target movement + if self.prev_target is not None: + target_change = target_pos - self.prev_target + target_change_mag = np.linalg.norm(target_change) + if target_change_mag > 0.01: + print(f"Target moved: {target_change} (mag: {target_change_mag:.4f})") + + # Get current joint positions + joint_positions = self.articulation_view.get_joint_positions() + joint_velocities = self.articulation_view.get_joint_velocities() + + current_joints = [] + for idx in self.dof_indices: + pos = joint_positions[0, idx] if joint_positions.ndim > 1 else joint_positions[idx] + current_joints.append(pos) + current_joints = np.array(current_joints) + + # Joint movements + if self.prev_joints is not None: + joint_changes = current_joints - self.prev_joints + if target_change_mag > 0.01: + print("Joint Changes:") + dof_names = self.articulation.dof_names + for i, idx in enumerate(self.dof_indices): + joint_name = dof_names[idx].replace('_joint', '').replace('right_', '') + pos_change_deg = joint_changes[i] * 180 / 3.14159 + vel = joint_velocities[0, idx] if joint_velocities.ndim > 1 else joint_velocities[idx] + if target_change_mag > 0.01: + print(f" {joint_name}: {joint_changes[i]:.4f}rad ({pos_change_deg:.2f}°) vel:{vel:.3f}") + else: + print("Initial joint positions:") + dof_names = self.articulation.dof_names + for i, idx in enumerate(self.dof_indices): + joint_name = dof_names[idx].replace('_joint', '').replace('right_', '') + pos_deg = current_joints[i] * 180 / 3.14159 + print(f" {joint_name}: {current_joints[i]:.4f}rad ({pos_deg:.2f}°)") + + # Store for next comparison + self.prev_target = target_pos.copy() + self.prev_joints = current_joints.copy() + From 24db8ef09cb5622a54974d5f22dfc8e08601cc7f Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Mon, 11 Aug 2025 09:31:22 -0400 Subject: [PATCH 37/42] force control OSC working for H1 without gravity --- abr_control/arms/isaacsim_config.py | 178 +++++------- abr_control/interfaces/nv_isaacsim.py | 292 +++----------------- examples/IsaacSim/force_floating_control.py | 11 +- examples/IsaacSim/force_osc_xyz.py | 39 +-- 4 files changed, 123 insertions(+), 397 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index f000e0cb..9a65ab7f 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -34,7 +34,7 @@ class IsaacsimConfig: - def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=False): + def __init__(self, robot_type): """Loads the Isaacsim model from the specified xml file Parameters @@ -70,14 +70,9 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F """ self.robot_type = robot_type - #TODO obsolete, from mujoco - self.use_sim_state = use_sim_state - - self.robot_path = None self.ee_link_name = "EE" # name used for virtual end-effector, overwritten if one exists already if self.robot_type == "ur5": - #self.ctrlr_dof = [True, True, True, False, False, False] 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" @@ -87,7 +82,6 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F print(f"Virtual end effector with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "jaco2": - #self.ctrlr_dof = [True, True, True, False, False, False] self.robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" self.has_EE = True # jaco2 has an end-effector self.ee_link_name = "j2n6s300_end_effector" @@ -97,7 +91,6 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...") elif self.robot_type == "h1": - #self.ctrlr_dof = [True, True, True, False] 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" @@ -105,11 +98,10 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F START_ANGLES = "0. 0. 0. 0." self.target_min = np.array([0.1, -0.55, 1.4]) 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 with name '{self.ee_link_name}' is attached as robot has none.") elif self.robot_type == "h1_hands": - #self.ctrlr_dof = [True, True, True, False, False] 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" @@ -117,15 +109,14 @@ def __init__(self, robot_type, folder=None, use_sim_state=True, force_download=F START_ANGLES = "0. 0. 0. 0. 0." self.target_min = np.array([0.1, -0.55, 1.4]) 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 with name '{self.ee_link_name}' specified in UDS, using it ...") - - self.START_ANGLES = np.array(START_ANGLES.split(), dtype=float) - def _connect(self, world, stage, articulation, articulation_view, dof_indices, joint_indices, joint_vel_addrs, prim_path): + def _connect(self, world, stage, articulation, articulation_view, dof_indices, joint_indices, prim_path): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -138,9 +129,7 @@ def _connect(self, world, stage, articulation, articulation_view, dof_indices, j dof_indices: np.array of ints The index of the robot joints in the Mujoco simulation data joint position array - joint_vel_addrs: np.array of ints - The index of the robot joints in the Mujoco simulation data joint - Jacobian, inertia matrix, and gravity vector + """ # get access to the Isaac simulation @@ -150,22 +139,21 @@ def _connect(self, world, stage, articulation, articulation_view, dof_indices, j self.articulation_view = articulation_view self.dof_indices = np.copy(dof_indices) self.joint_indices = np.copy(joint_indices) - self.joint_vel_addrs = np.copy(joint_vel_addrs) self.prim_path = prim_path 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 + # offset for non-fixed articulation base (mobile robots), necessary for indexing jacobian matrices etc correctly + self.base_offset = 6 if not self.is_fixed_base() else 0 - # need to calculate the joint_vel_addrs indices in flat vectors returned - # for the Jacobian self.jac_indices = np.hstack( # 6 because position and rotation Jacobians are 3 x N_JOINTS - [self.joint_vel_addrs + (ii * self.N_ALL_JOINTS) for ii in range(3)] + [self.dof_indices + (ii * self.N_ALL_DOF) for ii in range(3)] ) # for the inertia matrix self.M_indices = [ - ii * self.N_ALL_JOINTS + jj + ii * self.N_ALL_DOF + jj for jj in self.dof_indices for ii in self.dof_indices ] @@ -175,36 +163,23 @@ def _connect(self, world, stage, articulation, articulation_view, dof_indices, j self._J3NP = np.zeros((3, self.N_ALL_JOINTS)) self._J3NR = np.zeros((3, self.N_ALL_JOINTS)) self._J6N = np.zeros((6, self.N_JOINTS)) - self._MNN = np.zeros((self.N_ALL_JOINTS, self.N_ALL_JOINTS)) + self._MNN = np.zeros((self.N_ALL_DOF, self.N_ALL_DOF)) self._R9 = np.zeros(9) self._R = np.zeros((3, 3)) self._x = np.ones(4) - ''' + + # works but gets bumpy when out of reach and weird for higher targets def g(self, q=None): - g_full = self.articulation_view.get_generalized_gravity_forces()[0] - return -g_full[self.dof_indices] - ''' + + full_gravity = self.articulation_view.get_generalized_gravity_forces()[0] + # Drop the first 6 base DOFs for floating-base robots + joint_gravity = full_gravity[6:] + #return joint_gravity[self.dof_indices] - def g(self, q=None): - """ - Returns the gravity and Coriolis/centrifugal forces for the controlled arm_joints. - Args: - q (np.ndarray, optional): Joint positions - Returns: - np.ndarray: Generalized bias forces for controlled DOF - """ - # Compute gravity and Coriolis/centrifugal separately - gravity = self.articulation_view.get_generalized_gravity_forces(joint_indices=self.dof_indices)[0] - #coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.dof_indices)[0] - #g = gravity + coriolis - - return -gravity - #return -g - - - + return np.zeros(len(self.dof_indices)) # No gravity compensation in this example + def dJ(self, name, q=None, dq=None, x=None): """Returns the derivative of the Jacobian wrt to time @@ -222,23 +197,15 @@ def dJ(self, name, q=None, dq=None, x=None): x: float numpy.array, optional (Default: None) """ # TODO if ever required - # Note from Emo in Mujoco forums: - # 'You would have to use a finate-difference approximation in the - # general case, check differences.cpp' raise NotImplementedError - - def J(self, name, q=None, x=None, object_type="body"): if object_type == "body": jacobians = self.articulation_view.get_jacobians(clone=True) if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") - print("###################################") - print("jacobians shape: ", jacobians.shape) - print("###################################") # TODO this could be fixed by attachind virtual EE if self.robot_type is "ur5": link_index = 5 @@ -253,28 +220,16 @@ def J(self, name, q=None, x=None, object_type="body"): #link_index = 20 link_index = 25 - env_idx = 0 # Assuming single environment J_full = jacobians[env_idx, link_index, :, :] if len(jacobians.shape) == 4: - # Fixed articulation base (robotic manipulators): (env, num_bodies - 1, 6, num_dof) - if self.is_fixed_base(): - jacobian_columns = self.dof_indices - - # Non-fixed articulation base (mobile robots): (env, num_bodies, 6, num_dof + base_dofs) - else: - jacobian_base_offset = 2 # Empirically determined for H1 - jacobian_columns = self.dof_indices + jacobian_base_offset - + # apply offset for non-fixed articulation base (mobile robots + indices = self.dof_indices + self.base_offset + # Extract only the columns for controllable DOFs + J = J_full[:, indices] else: raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") - - - # Extract only the columns for controllable DOFs - # This gives us the end-effector Jacobian w.r.t. only the right arm joints - J = J_full[:, jacobian_columns] - # Assign to internal storage # Linear velocity Jacobian (first 3 rows) self._J6N[:3, :] = J[:3, :] @@ -301,11 +256,12 @@ def M(self, q=None): """ # get_mass_matrices returns (num_envs, joint_count, joint_count) self._MNN= self.articulation_view.get_mass_matrices()[0] - # extract only the controlled joints - M = self._MNN[np.ix_(self.joint_indices, self.joint_indices)] - return np.copy(M) - + # apply offset for non-fixed articulation base (mobile robots) + indices = self.dof_indices + self.base_offset + M = self._MNN[np.ix_(indices, indices)] + return M + def R(self, name, q=None, object_type="body"): """ Returns the rotation matrix of the specified object in Isaac Sim. @@ -366,19 +322,13 @@ def quaternion(self, name, q=None): # Get 4x4 world transform matrix matrix = omni.usd.get_world_transform_matrix(prim) quat = matrix.ExtractRotationQuat() - - # Convert to [w, x, y, z] NumPy array quat_np = np.array([quat.GetReal(), *quat.GetImaginary()]) return quat_np - def C(self, q=None, dq=None): - """NOTE: The Coriolis and centrifugal effects (and gravity) are - already accounted for by Mujoco in the qfrc_bias variable. There's - no easy way to separate these, so all are returned by the g function. - To prevent accounting for these effects twice, this function will - return an error instead of qfrc_bias again. + def C(self): + """NOTE: The Coriolis and centrifugal effects are neglected atm, as this method is not called anywhere """ coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.dof_indices)[0] return coriolis @@ -421,6 +371,38 @@ def Tx(self, name, q=None, x=None, object_type="body"): position = matrix.ExtractTranslation() return np.array([position[0], position[1], position[2]], dtype=np.float64) + + ''' + def get_xyz(self, prim_path): + """Returns the xyz position of the specified object + + prim_path : string + path of the object you want the xyz position of + """ + transform_matrix = self.get_transform(prim_path) + translation = transform_matrix.ExtractTranslation() + return np.array([translation[0], translation[1], translation[2]], dtype=np.float64) + + + + # TODO check if necessary + def get_transform(self, prim_path): + _cube = self.stage.GetPrimAtPath(prim_path) + # Check if it's an Xformable + if not _cube.IsValid() or not UsdGeom.Xformable(_cube): + print(f"Prim at {_cube.GetPath()} is not a valid Xformable.") + else: + xformable = UsdGeom.Xformable(_cube) + # Get the local transformation matrix + transform_matrix = xformable.GetLocalTransformation() + + return transform_matrix + + ''' + + + + def T_inv(self, name, q=None, x=None): @@ -448,37 +430,23 @@ def _get_prim_path(self, name): return prim.GetPath() return None - def get_joint_positions(self): - """Get current joint positions""" - if hasattr(self, 'articulation'): - all_positions = self.articulation.get_joint_positions() - return all_positions[self.dof_indices] - return None - + all_positions = self.articulation.get_joint_positions() + return all_positions[self.dof_indices] + def get_joint_velocities(self): - """Get current joint velocities""" - if hasattr(self, 'articulation'): - all_velocities = self.articulation.get_joint_velocities() - return all_velocities[self.joint_vel_addrs] - return None - + all_velocities = self.articulation.get_joint_velocities() + return all_velocities[self.dof_indices] def set_joint_positions(self, q): - """Set joint positions""" - if hasattr(self, 'articulation'): - full_positions = self.articulation.get_joint_positions() - full_positions[self.dof_indices] = q - self.articulation.set_joint_positions(full_positions) + full_positions = self.articulation.get_joint_positions() + full_positions[self.dof_indices] = q + self.articulation.set_joint_positions(full_positions) - def set_joint_velocities(self, dq): - """Set joint velocities""" - if hasattr(self, 'articulation'): - full_velocities = self.articulation.get_joint_velocities() - full_velocities[self.joint_vel_addrs] = dq - self.articulation.set_joint_velocities(full_velocities) - + full_velocities = self.articulation.get_joint_velocities() + full_velocities[self.dof_indices] = dq + self.articulation.set_joint_velocities(full_velocities) def is_fixed_base(self): num_dof = self.articulation_view.num_dof diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 7a897fec..8e858cd1 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -1,5 +1,4 @@ import numpy as np -import math from isaacsim import SimulationApp from .interface import Interface simulation_app = SimulationApp({"headless": False}) @@ -8,10 +7,8 @@ import omni.isaac.core.utils.stage as stage_utils # type: ignore from omni.isaac.core import World # type: ignore from omni.isaac.core.articulations import Articulation, ArticulationView # type: ignore -#TODO change import -#TODO is "Robot" even necessary -from isaacsim.core.api.robots import Robot # type: ignore -from pxr import UsdGeom, Gf, UsdShade, Sdf, UsdPhysics# type: ignore +from omni.isaac.core.robots import Robot # type: ignore +from pxr import UsdGeom, Gf, UsdShade, Sdf # type: ignore from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore from isaacsim.robot.policy.examples.robots import H1FlatTerrainPolicy # type: ignore import omni.isaac.core.utils.numpy.rotations as rot_utils # type: ignore @@ -29,23 +26,15 @@ class IsaacSim(Interface): simulation time step in seconds """ - def __init__(self, robot_config, dt=0.001, force_download=False): + def __init__(self, robot_config, dt=0.001): super().__init__(robot_config) self.robot_config = robot_config self.dt = dt # time step - #self.count = 0 # keep track of how many times send forces is called self.prim_path = "/World/robot" - #remove? - self.name = self.robot_config.robot_type - - self.prev_target = None - self.prev_joints = None - - - def connect(self, joint_names=None, camera_id=-1): + def connect(self, joint_names=None): """ joint_names: list, optional (Default: None) list of joint names to send control signal to and get feedback from @@ -60,7 +49,6 @@ def connect(self, joint_names=None, camera_id=-1): 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}" @@ -69,7 +57,7 @@ def connect(self, joint_names=None, camera_id=-1): if self.robot_config.robot_type.startswith("h1"): self.h1 = H1FlatTerrainPolicy( prim_path=self.prim_path, - name=self.name, + name=self.robot_config.robot_type, usd_path=robot_usd_path, position=np.array([0, 0 , 0]), orientation=rot_utils.euler_angles_to_quats(np.array([0, 0, 0]), degrees=True), @@ -80,23 +68,19 @@ def connect(self, joint_names=None, camera_id=-1): usd_path=robot_usd_path, prim_path=self.prim_path, ) - robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.name)) - - - + robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.robot_config.robot_type)) + self.world.reset() - self.articulation = Articulation(prim_path=self.prim_path, name=self.name + "_articulation") + self.articulation = Articulation(prim_path=self.prim_path, name=self.robot_config.robot_type + "_articulation") self.articulation.initialize() self.world.scene.add(self.articulation) # Add to scene if not already added by higher-level env #TODO remove and replace with articulation - self.articulation_view = ArticulationView(prim_paths_expr=self.prim_path, name=self.name + "_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 EE if none exists @@ -110,55 +94,28 @@ def connect(self, joint_names=None, camera_id=-1): # Reset the world to initialize physics self.world.reset() - - # Get joint information - #self.joint_pos_addrs = [] self.dof_indices = [] self.joint_indices = [] - self.joint_vel_addrs = [] self.joint_dyn_addrs = [] if joint_names is None: print("No joint names provided, using all controllable joints in the articulation.") joint_names = self.robot_config.controlled_dof - - - - # Validate dof names and get indices self.all_dof_names = self.articulation.dof_names self.all_joint_names = self.articulation_view.joint_names self.all_body_names = self.articulation_view.body_names - #self.all_link_names = self.articulation_view.link_names - print(f"len dof names: {len(self.all_dof_names)}") - print(f"len joint names: {len(self.all_joint_names)}") - print(f"len body names: {len(self.all_body_names)}") - #print(f"len link names: {len(self.all_link_names)}") - #print(f"Provided joint names: {joint_names}") - for name in joint_names: if name not in self.all_dof_names and name not in self.all_joint_names: raise Exception(f"Joint name {name} does not exist in robot model") - #print(f"name: {name}") - #link_name = name.replace("joint", "link") - #print(f"link_name: {link_name}") - - joint_idx = self.articulation_view.get_joint_index(name) dof_idx = self.articulation_view.get_dof_index(name) - #print(f"dof_index: {dof_idx}") - #print(f"joint_idx: {joint_idx}") - # link_idx = self.articulation_view.get_link_index(link_name) - # print(f"link_idx: {link_idx}") self.dof_indices.append(dof_idx) self.joint_indices.append(joint_idx) - self.joint_vel_addrs.append(dof_idx) - #TODO check if joint_dyn_addrs necessary - self.joint_dyn_addrs.append(dof_idx) - + # Connect robot config with simulation data print("Connecting to robot config...") @@ -169,17 +126,11 @@ def connect(self, joint_names=None, camera_id=-1): self.articulation_view, self.dof_indices, self.joint_indices, - self.joint_vel_addrs, self.prim_path, ) if self.robot_config.robot_type.startswith("h1"): - self.world.add_physics_callback("send_actions", self.send_actions) - - - - - + self.world.add_physics_callback("keep_standing", self.keep_standing) def disconnect(self): @@ -187,78 +138,19 @@ def disconnect(self): self.simulation_app.close() # close Isaac Sim print("IsaacSim connection closed...") - ''' - def send_forces(self, u): - """Applies the torques u to the joints specified in indices.""" - - # Debug: Check array sizes and indices - print(f"=== SEND_FORCES DEBUG ===") - print(f"robot_config.N_ALL_JOINTS: {self.robot_config.N_ALL_JOINTS}") - print(f"articulation_view joint count: {len(self.articulation_view.joint_names)}") - print(f"joint_pos_addrs: {self.joint_pos_addrs}") - print(f"u shape: {u.shape}") - print(f"Max index in joint_pos_addrs: {max(self.joint_pos_addrs) if self.joint_pos_addrs else 'None'}") - - # Use the correct joint count for the articulation view - total_joints = len(self.articulation_view.joint_names) # Should be 24 - full_torques = np.zeros(total_joints) - - # Apply control torques to the controlled joints - full_torques[self.joint_pos_addrs] = u - - print(f"full_torques shape: {full_torques.shape}") - print(f"Non-zero torques at indices: {np.nonzero(full_torques)[0]}") - - # Apply the control signal - self.articulation_view.set_joint_efforts(full_torques) - - # Move simulation ahead one time step - self.world.step(render=True) - ''' - - - - def debug_dof_mapping(self): - print("=== DOF MAPPING DEBUG ===") - print(f"robot_config.N_ALL_JOINTS: {self.robot_config.N_ALL_JOINTS}") - print(f"articulation.dof_names length: {len(self.articulation.dof_names)}") - print(f"articulation_view.dof_names length: {len(self.articulation_view.dof_names)}") - print(f"articulation_view.joint_names length: {len(self.articulation_view.joint_names)}") - - print("\nDOF names (actuated joints):") - dof_names = self.articulation.dof_names - for i, name in enumerate(dof_names): - print(f" {i}: {name}") - - print("\nLooking for controlled joints in DOF names:") - for joint_name in self.robot_config.controlled_dof: - if joint_name in dof_names: - idx = dof_names.index(joint_name) - print(f" {joint_name}: DOF index {idx}") - else: - print(f" {joint_name}: NOT FOUND in DOF names!") - - def send_forces(self, u): """Applies the torques u to the joints specified in indices.""" - #print ("send forces") # 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 + full_torques[self.dof_indices] = u # Apply the control signal - #TODO maybe outsource as done for position 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): """Moves the arm to the specified joint angles @@ -271,54 +163,15 @@ def send_target_angles(self, q): def get_feedback(self): """Return a dictionary of information needed by the controller. - Returns the joint angles and joint velocities in [rad] and [rad/sec], respectively """ self.q = self.robot_config.get_joint_positions() self.dq = self.robot_config.get_joint_velocities() return {"q": self.q, "dq": self.dq} - - - def get_xyz(self, prim_path): - """Returns the xyz position of the specified object - - prim_path : string - path of the object you want the xyz position of - """ - transform_matrix = self.get_transform(prim_path) - translation = transform_matrix.ExtractTranslation() - return np.array([translation[0], translation[1], translation[2]], dtype=np.float64) - - - #TODO check if overlap to def quaternion - def get_orientation(self, prim_path): - """Returns the orientation of an object in IsaacSim - Parameters - ---------- - name : string - the name of the object of interest - """ - transform_matrix = self.get_transform(prim_path) - quat = transform_matrix.ExtractRotationQuat() - quat_np = np.array([quat.GetReal(), *quat.GetImaginary()]) - return quat_np - - def get_transform(self, prim_path): - _cube = self.stage.GetPrimAtPath(prim_path) - # Check if it's an Xformable - if not _cube.IsValid() or not UsdGeom.Xformable(_cube): - print(f"Prim at {_cube.GetPath()} is not a valid Xformable.") - else: - xformable = UsdGeom.Xformable(_cube) - # Get the local transformation matrix - transform_matrix = xformable.GetLocalTransformation() - - return transform_matrix - - - def set_xyz(self, prim_path, xyz, orientation=np.array([0., 0., 0., 1.])): + + def set_xyz(self, prim_path, xyz): """Set the position of an object in the environment. prim_path : string @@ -326,36 +179,25 @@ def set_xyz(self, prim_path, xyz, orientation=np.array([0., 0., 0., 1.])): xyz : np.array the [x,y,z] location of the target [meters] """ - _cube = self.stage.GetPrimAtPath(prim_path) - xformable = UsdGeom.Xformable(_cube) + prim = self.stage.GetPrimAtPath(prim_path) + xformable = UsdGeom.Xformable(prim) transform_matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(xyz[0], xyz[1], xyz[2])) xformable.MakeMatrixXform().Set(transform_matrix) + - - # method for keep_standing - def send_actions(self, dt): - pelvis_prim_path = '/World/robot/pelvis' - prim = self.stage.GetPrimAtPath(pelvis_prim_path) + # method that humanoid does not fall over + def keep_standing(self, dt): + 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)) - #prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(0.70711 ,0.70711 ,0.0 ,0.0)) - - - #TODO check if position is even set here. maybe remove # Create a visual-only cube (no collision) - def create_target_prim(self, prim_path="/World/target_cube", position=np.array([0, 0, 1.0]), size = .1, color=np.array([0, 0, 1.0])): + def create_target_prim(self, prim_path="/World/target", size = .1, color=np.array([0, 0, 1.0])): # Create cube geometry cube_prim = UsdGeom.Cube.Define(self.stage, prim_path) - cube_prim.CreateSizeAttr(size) # Unit cube + cube_prim.CreateSizeAttr(size) - # Set transform (position and scale) - xformable = UsdGeom.Xformable(cube_prim) - transform_matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(position[0], position[1], position[2])) - xformable.MakeMatrixXform().Set(transform_matrix) - # xformable.AddTranslateOp().Set(Gf.Vec3f(*position)) - # Create and apply material for color material_path = prim_path + "/Material" @@ -373,7 +215,7 @@ def create_target_prim(self, prim_path="/World/target_cube", position=np.array([ # Bind material to cube 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) @@ -383,48 +225,26 @@ def create_target_prim(self, prim_path="/World/target_cube", position=np.array([ def set_gains_force_control(self): """Properly set gains for arm joints (DOFs 0-5) and finger joints if present""" - # Get current gains or set defaults stiffness = np.ones(self.robot_config.N_ALL_DOF) * 100.0 # Default high stiffness damping = np.ones(self.robot_config.N_ALL_DOF) * 10.0 # Default damping - # Set controlled arm joints to zero stiffness for force control + if self.robot_config.is_fixed_base(): + controlled_s = 0.0 + controlled_d = 0.1 + else: + controlled_s = 2.0 + controlled_d = 0.5 + for idx in self.dof_indices: - stiffness[idx] = 0.0 # Zero stiffness = force control - damping[idx] = 0.1 # Low damping for responsiveness + stiffness[idx] = controlled_s + damping[idx] = controlled_d - self.articulation_view.set_gains(stiffness, damping) print(f"Set gains for force control for arm joints {self.dof_indices}") - - def set_gains_force_control_h1(self): - - self.articulation_view.switch_control_mode( - mode="effort", - joint_indices=self.dof_indices - ) - - stiffness = np.ones(self.robot_config.N_ALL_DOF) * 100.0 - damping = np.ones(self.robot_config.N_ALL_DOF) * 10.0 - - # Use the correct DOF indices - for dof_idx in self.dof_indices: - stiffness[dof_idx] = 20.0 - damping[dof_idx] = 5.0 - print(f"Set gains for DOF {dof_idx}: {self.all_dof_names[dof_idx]}") - - # Reshape for ArticulationView: (M, K) - stiffness = np.expand_dims(stiffness, axis=0) - damping = np.expand_dims(damping, axis=0) - - self.articulation_view.set_gains(stiffness, damping) - - - - def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): """Add virtual end effector link as an Xform under the specified parent link""" # Full path to parent @@ -440,51 +260,3 @@ def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): ee_prim.AddTranslateOp().Set(Gf.Vec3d(*offset)) print(f"Created virtual EE link at {ee_prim_path}") - - - def debug_target_and_joints(self): - """Debug target movement and controlled joint response""" - - target_pos = self.get_xyz("/World/target") - - # Target movement - if self.prev_target is not None: - target_change = target_pos - self.prev_target - target_change_mag = np.linalg.norm(target_change) - if target_change_mag > 0.01: - print(f"Target moved: {target_change} (mag: {target_change_mag:.4f})") - - # Get current joint positions - joint_positions = self.articulation_view.get_joint_positions() - joint_velocities = self.articulation_view.get_joint_velocities() - - current_joints = [] - for idx in self.dof_indices: - pos = joint_positions[0, idx] if joint_positions.ndim > 1 else joint_positions[idx] - current_joints.append(pos) - current_joints = np.array(current_joints) - - # Joint movements - if self.prev_joints is not None: - joint_changes = current_joints - self.prev_joints - if target_change_mag > 0.01: - print("Joint Changes:") - dof_names = self.articulation.dof_names - for i, idx in enumerate(self.dof_indices): - joint_name = dof_names[idx].replace('_joint', '').replace('right_', '') - pos_change_deg = joint_changes[i] * 180 / 3.14159 - vel = joint_velocities[0, idx] if joint_velocities.ndim > 1 else joint_velocities[idx] - if target_change_mag > 0.01: - print(f" {joint_name}: {joint_changes[i]:.4f}rad ({pos_change_deg:.2f}°) vel:{vel:.3f}") - else: - print("Initial joint positions:") - dof_names = self.articulation.dof_names - for i, idx in enumerate(self.dof_indices): - joint_name = dof_names[idx].replace('_joint', '').replace('right_', '') - pos_deg = current_joints[i] * 180 / 3.14159 - print(f" {joint_name}: {current_joints[i]:.4f}rad ({pos_deg:.2f}°)") - - # Store for next comparison - self.prev_target = target_pos.copy() - self.prev_joints = current_joints.copy() - diff --git a/examples/IsaacSim/force_floating_control.py b/examples/IsaacSim/force_floating_control.py index 623d6ec5..ad24d3e3 100644 --- a/examples/IsaacSim/force_floating_control.py +++ b/examples/IsaacSim/force_floating_control.py @@ -16,21 +16,20 @@ arm_model = sys.argv[1] else: arm_model = "h1_hands" - #arm_model = "ur5" -# initialize our robot config + #arm_model = "jaco2" robot_config = arm(arm_model) -# create the IsaacSim interface and connect up interface = IsaacSim(robot_config, dt = 0.007) -interface.connect(joint_names=[f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))]) +interface.connect(joint_names=robot_config.controlled_dof) + + + interface.send_target_angles(robot_config.START_ANGLES) interface.set_gains_force_control() - # 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 = [] diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index 4c3f0cbf..16ba22f0 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -10,14 +10,12 @@ from abr_control.controllers import OSC, Damping from abr_control.interfaces.nv_isaacsim import IsaacSim from abr_control.utils import transformations -import time -last_time = time.time() if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "h1" + arm_model = "ur5" robot_config = arm(arm_model) dt = 0.007 # 143 Hz @@ -27,17 +25,20 @@ # create our IsaacSim interface interface = IsaacSim(robot_config, dt=dt) -#joint_names = [f"joint{ii}" for ii in range(len(robot_config.START_ANGLES))] -#interface.connect(joint_names=robot_config.controlled_joints) +#interface.connect(joint_names=robot_config.controlled_dof) interface.connect() interface.send_target_angles(robot_config.START_ANGLES) isaac_target = interface.create_target_prim(prim_path=target_prim_path) +interface.set_gains_force_control() + +# disable gravity +interface.articulation_view.set_body_disable_gravity(True) +gravity = interface.articulation_view.get_body_disable_gravity()[0] +print("Gravity disabled: ", gravity) -#interface.set_gains_force_control() -interface.set_gains_force_control_h1() # damp the movements of the arm damping = Damping(robot_config, kv=30) #kv=10) @@ -47,16 +48,13 @@ kp= 300, # 200 null_controllers=[damping], vmax=[0.5, 0], # [m/s, rad/s] - # control (x, y, z) out of [x, y, z, alpha, beta, gamma] ctrlr_dof= [True, True, True, False, False, False] - #ctrlr_dof= robot_config.ctrlr_dof ) # set up lists for tracking data ee_track = [] target_track = [] -#target_geom = "target" green = [0, 0.9, 0, 0.5] red = [0.9, 0, 0, 0.5] @@ -69,11 +67,6 @@ def gen_target(interface): interface.set_xyz(target_prim_path, target_xyz) - -interface.debug_dof_mapping() - - - try: # get the end-effector's initial position feedback = interface.get_feedback() @@ -85,18 +78,13 @@ def gen_target(interface): count = 0 print("\nSimulation starting...\n") while 1: - current_time = time.time() - dt = current_time - last_time - #print(f"Control dt: {dt:.6f}s, Freq: {1/dt:.1f}Hz") - last_time = current_time - # get joint angle and velocity feedback + feedback = interface.get_feedback() - target = np.hstack( [ - interface.get_xyz(target_prim_path), + robot_config.Tx(target_prim_path), transformations.euler_from_quaternion( - interface.get_orientation(target_prim_path), "rxyz" + robot_config.quaternion(target_prim_path), "rxyz" ), ] ) @@ -108,10 +96,9 @@ def gen_target(interface): target=target, ) - interface.send_forces(u) - # interface.world.step(render=True) - + #interface.world.step(render=True) + # calculate end-effector position From 090944b319e90cdefdbdcbe9e189964f4941d6b2 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 14 Aug 2025 12:36:25 -0400 Subject: [PATCH 38/42] fixed offset for virtual EE --- abr_control/arms/isaacsim_config.py | 191 ++++++++++---------------- abr_control/interfaces/nv_isaacsim.py | 44 ++---- 2 files changed, 86 insertions(+), 149 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 9a65ab7f..cadd6454 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -76,10 +76,12 @@ def __init__(self, robot_type): 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.] START_ANGLES = "0 -.67 -.67 0 0 0" self.target_min = np.array([-0.5, -0.5, 0.5]) + self.target_range = [1, 1, 0.5] 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 with name '{self.ee_link_name}' is attached as robot has none.") + print(f"Virtual end effector with name '{self.ee_link_name}' is attached, as robot has none.") elif self.robot_type == "jaco2": self.robot_path = "/Isaac/Robots/Kinova/Jaco2/J2N6S300/j2n6s300_instanceable.usd" @@ -87,6 +89,7 @@ def __init__(self, robot_type): self.ee_link_name = "j2n6s300_end_effector" START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" self.target_min = np.array([-0.5, -0.5, 0.5]) + self.target_range = [1, 1, 0.5] 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 with name '{self.ee_link_name}' specified in UDS, using it ...") @@ -94,12 +97,18 @@ def __init__(self, robot_type): 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_link_name = "right_elbow_link" + self.ee_offset=[0.26455, 0.00118, -0.0209] # for H1 START_ANGLES = "0. 0. 0. 0." - self.target_min = np.array([0.1, -0.55, 1.4]) + # self.target_min = np.array([0.1, -0.5, 1.4]) + self.target_min = np.array([0.12, -0.4, 1.3]) + self.target_range = np.array([ + 0.4 - 0.12, # x range + 0.12 - (-0.4), # y range + 2.2 - 1.3 # z range + ]) 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 with name '{self.ee_link_name}' is attached as robot has none.") + print(f"Virtual end effector with name '{self.ee_link_name}' is attached, as robot has none.") elif self.robot_type == "h1_hands": self.robot_path = "/Isaac/Robots/Unitree/H1/h1_with_hand.usd" @@ -107,7 +116,13 @@ def __init__(self, robot_type): #self.EE_parent_link = "right_elbow_link" self.ee_link_name = "right_hand_link" START_ANGLES = "0. 0. 0. 0. 0." - self.target_min = np.array([0.1, -0.55, 1.4]) + # self.target_min = np.array([0.1, -0.55, 1.4]) + self.target_min = np.array([0.12, -0.4, 1.3]) + self.target_range = np.array([ + 0.49 - 0.12, # x range + 0.12 - (-0.4), # y range + 2.2 - 1.3 # z range + ]) 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 with name '{self.ee_link_name}' specified in UDS, using it ...") @@ -116,7 +131,7 @@ def __init__(self, robot_type): - def _connect(self, world, stage, articulation, articulation_view, dof_indices, joint_indices, prim_path): + def _connect(self, world, stage, articulation_view, dof_indices, joint_indices, prim_path): """Called by the interface once the Mujoco simulation is created, this connects the config to the simulator so it can access the @@ -135,7 +150,6 @@ def _connect(self, world, stage, articulation, articulation_view, dof_indices, j # get access to the Isaac simulation self.world = world self.stage = stage - self.articulation = articulation self.articulation_view = articulation_view self.dof_indices = np.copy(dof_indices) self.joint_indices = np.copy(joint_indices) @@ -143,7 +157,7 @@ def _connect(self, world, stage, articulation, articulation_view, dof_indices, j 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 - # offset for non-fixed articulation base (mobile robots), necessary for indexing jacobian matrices etc correctly + # offset for non-fixed base (mobile robots), necessary for indexing jacobian matrices etc correctly self.base_offset = 6 if not self.is_fixed_base() else 0 self.jac_indices = np.hstack( @@ -200,69 +214,62 @@ def dJ(self, name, q=None, dq=None, x=None): raise NotImplementedError - def J(self, name, q=None, x=None, object_type="body"): - if object_type == "body": - jacobians = self.articulation_view.get_jacobians(clone=True) - if jacobians.shape[0] == 0: - raise RuntimeError("ArticulationView contains no environments. Make sure it's properly initialized and contains articulations.") + def J(self, name, q=None, x=None): + jacobians = self.articulation_view.get_jacobians(clone=True) + if jacobians.shape[0] == 0: + raise RuntimeError("ArticulationView contains no environments.") - # TODO this could be fixed by attachind virtual EE - if self.robot_type is "ur5": - link_index = 5 - elif self.robot_type is "jaco2": - link_index = 5 + # TODO this could be fixed by attachind virtual EE + if self.robot_type is "ur5": + link_index = 5 + elif self.robot_type is "jaco2": + link_index = 5 # [6, 10, 14, 18] - elif self.robot_type is "h1": - #link_index = 18 - link_index = 24 - # [6, 10, 14, 18, 20] - elif self.robot_type is "h1_hands": - #link_index = 20 - link_index = 25 - - env_idx = 0 # Assuming single environment - J_full = jacobians[env_idx, link_index, :, :] - if len(jacobians.shape) == 4: - # apply offset for non-fixed articulation base (mobile robots - indices = self.dof_indices + self.base_offset - # Extract only the columns for controllable DOFs - J = J_full[:, indices] - else: - raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") - - # Assign to internal storage - # Linear velocity Jacobian (first 3 rows) - self._J6N[:3, :] = J[:3, :] - # Angular velocity Jacobian (last 3 rows) - self._J6N[3:, :] = J[3:, :] + elif self.robot_type is "h1": + #link_index = 18 + link_index = 24 + # [6, 10, 14, 18, 20] + elif self.robot_type is "h1_hands": + #link_index = 20 + link_index = 25 + + env_idx = 0 # Assuming single environment + J_full = jacobians[env_idx, link_index, :, :] + if len(jacobians.shape) == 4: + # apply offset for non-fixed base (mobile robots) + indices = self.dof_indices + self.base_offset + # Extract only the columns for controllable DOFs + J = J_full[:, indices] + else: + raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") - # Additional validation - check if Jacobian makes sense (non-zero values) - if np.allclose(J, 0): - raise RuntimeError("Jacobian is all zeros - check robot configuration and joint addresses") + # Assign to internal storage + # Linear velocity Jacobian (first 3 rows) + self._J6N[:3, :] = J[:3, :] + # Angular velocity Jacobian (last 3 rows) + self._J6N[3:, :] = J[3:, :] - elif object_type == "geom": - raise NotImplementedError("Calculating the Jacobian for 'geom' is not yet implemented.") - elif object_type == "site": - raise NotImplementedError("Calculating the Jacobian for 'site' is not yet implemented.") - else: - raise ValueError(f"Invalid object type specified: {object_type}") + # Additional validation - check if Jacobian makes sense (non-zero values) + 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): """ Returns the inertia matrix for the controlled arm joints. """ # get_mass_matrices returns (num_envs, joint_count, joint_count) self._MNN= self.articulation_view.get_mass_matrices()[0] - # apply offset for non-fixed articulation base (mobile robots) + # apply offset for non-fixed base (mobile robots) indices = self.dof_indices + self.base_offset M = self._MNN[np.ix_(indices, indices)] return M - def R(self, name, q=None, object_type="body"): + def R(self, name, q=None): """ Returns the rotation matrix of the specified object in Isaac Sim. @@ -272,19 +279,8 @@ def R(self, name, q=None, object_type="body"): The name of the object (body, geom, or site) in the USD stage. q : np.ndarray, optional Joint positions (not used here unless you want to simulate a different state). - object_type : str - One of "body", "geom", or "site". """ - - if object_type == "body": - prim_path = self._get_prim_path(name) - elif object_type in ["site", "geom"]: - # Assume full path is given or fixed base path - prim_path = f"{self.prim_path}/{name}" - else: - raise ValueError(f"Unsupported object type: {object_type}") - - + prim_path = self._get_prim_path(name) prim = self.stage.GetPrimAtPath(prim_path) if not prim.IsValid(): raise RuntimeError(f"Prim '{prim_path}' not found") @@ -304,7 +300,7 @@ def R(self, name, q=None, object_type="body"): def quaternion(self, name, q=None): - """Returns the quaternion of the specified body + """Returns the quaternion Parameters ---------- @@ -335,7 +331,7 @@ def C(self): def T(self, name, q=None, x=None): - """Get the transform matrix of the specified body. + """Get the transform matrix. Parameters ---------- @@ -350,19 +346,10 @@ def T(self, name, q=None, x=None): raise NotImplementedError - def Tx(self, name, q=None, x=None, object_type="body"): + def Tx(self, name, q=None, x=None): """Simplified version that only gets current position without state changes.""" if name == "EE": name = self.ee_link_name - # Get prim path - if object_type in ["body", "link"]: - prim_path = self._get_prim_path(name) - elif object_type == "joint": - # For joints, you might want the parent link position - prim_path = self._get_prim_path(name) - else: - raise ValueError(f"Unsupported object_type: {object_type}") - - # Get world position + prim_path = self._get_prim_path(name) prim = self.stage.GetPrimAtPath(prim_path) if not prim.IsValid(): raise RuntimeError(f"Invalid prim at path: {prim_path}") @@ -371,42 +358,10 @@ def Tx(self, name, q=None, x=None, object_type="body"): position = matrix.ExtractTranslation() return np.array([position[0], position[1], position[2]], dtype=np.float64) - - ''' - def get_xyz(self, prim_path): - """Returns the xyz position of the specified object - - prim_path : string - path of the object you want the xyz position of - """ - transform_matrix = self.get_transform(prim_path) - translation = transform_matrix.ExtractTranslation() - return np.array([translation[0], translation[1], translation[2]], dtype=np.float64) - - - - # TODO check if necessary - def get_transform(self, prim_path): - _cube = self.stage.GetPrimAtPath(prim_path) - # Check if it's an Xformable - if not _cube.IsValid() or not UsdGeom.Xformable(_cube): - print(f"Prim at {_cube.GetPath()} is not a valid Xformable.") - else: - xformable = UsdGeom.Xformable(_cube) - # Get the local transformation matrix - transform_matrix = xformable.GetLocalTransformation() - - return transform_matrix - - ''' - - - - def T_inv(self, name, q=None, x=None): - """Get the inverse transform matrix of the specified body. + """Get the inverse transform matrix. Parameters ---------- @@ -431,22 +386,22 @@ def _get_prim_path(self, name): return None def get_joint_positions(self): - all_positions = self.articulation.get_joint_positions() - return all_positions[self.dof_indices] + full_positions = self.articulation_view.get_joint_positions()[0] + return full_positions[self.dof_indices] def get_joint_velocities(self): - all_velocities = self.articulation.get_joint_velocities() - return all_velocities[self.dof_indices] + full_velocities = self.articulation_view.get_joint_velocities()[0] + return full_velocities[self.dof_indices] def set_joint_positions(self, q): - full_positions = self.articulation.get_joint_positions() + full_positions = self.articulation_view.get_joint_positions()[0] full_positions[self.dof_indices] = q - self.articulation.set_joint_positions(full_positions) + self.articulation_view.set_joint_positions(full_positions) def set_joint_velocities(self, dq): - full_velocities = self.articulation.get_joint_velocities() + full_velocities = self.articulation_view.get_joint_velocities()[0] full_velocities[self.dof_indices] = dq - self.articulation.set_joint_velocities(full_velocities) + self.articulation_view.set_joint_velocities(full_velocities) def is_fixed_base(self): num_dof = self.articulation_view.num_dof diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 8e858cd1..88394f17 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -4,14 +4,16 @@ simulation_app = SimulationApp({"headless": False}) import omni import omni.kit.commands # type: ignore -import omni.isaac.core.utils.stage as stage_utils # 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 Articulation, ArticulationView # type: ignore -from omni.isaac.core.robots import Robot # type: ignore +from omni.isaac.core.articulations import ArticulationView # type: ignore +#from isaacsim.core.prims import Articulation # type: ignore +from isaacsim.core.api.robots import Robot # type: ignore + from pxr import UsdGeom, Gf, UsdShade, Sdf # type: ignore -from omni.isaac.core.utils.nucleus import get_assets_root_path # type: ignore +from isaacsim.core.utils.nucleus import get_assets_root_path # type: ignore from isaacsim.robot.policy.examples.robots import H1FlatTerrainPolicy # type: ignore -import omni.isaac.core.utils.numpy.rotations as rot_utils # type: ignore +import isaacsim.core.utils.numpy.rotations as rot_utils # type: ignore class IsaacSim(Interface): @@ -70,24 +72,16 @@ def connect(self, joint_names=None): ) robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.robot_config.robot_type)) - - self.world.reset() - self.articulation = Articulation(prim_path=self.prim_path, name=self.robot_config.robot_type + "_articulation") - self.articulation.initialize() - self.world.scene.add(self.articulation) # Add to scene if not already added by higher-level env - - #TODO remove and replace with articulation + 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 EE if none exists if (self.robot_config.has_EE is False): 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) - + self.add_virtual_ee_link(self.robot_config.EE_parent_link, self.robot_config.ee_link_name, offset=self.robot_config.ee_offset) # Set simulation time step self.world.get_physics_context().set_physics_dt(self.dt) @@ -101,10 +95,10 @@ def connect(self, joint_names=None): self.joint_dyn_addrs = [] if joint_names is None: - print("No joint names provided, using all controllable joints in the articulation.") + print("No joint names provided, using all controllable joints.") joint_names = self.robot_config.controlled_dof - self.all_dof_names = self.articulation.dof_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 @@ -122,7 +116,6 @@ def connect(self, joint_names=None): self.robot_config._connect( self.world, self.stage, - self.articulation, self.articulation_view, self.dof_indices, self.joint_indices, @@ -198,7 +191,6 @@ def create_target_prim(self, prim_path="/World/target", size = .1, color=np.arra 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) @@ -210,10 +202,8 @@ def create_target_prim(self, prim_path="/World/target", size = .1, color=np.arra shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.4) shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0) - # Connect shader to material + # Connect shader to material and bind material to cube material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface") - - # Bind material to cube UsdShade.MaterialBindingAPI(cube_prim).Bind(material) # Disable collision to ensure it's purely visual @@ -222,7 +212,6 @@ def create_target_prim(self, prim_path="/World/target", size = .1, color=np.arra return cube_prim - def set_gains_force_control(self): """Properly set gains for arm joints (DOFs 0-5) and finger joints if present""" # Get current gains or set defaults @@ -244,19 +233,12 @@ def set_gains_force_control(self): print(f"Set gains for force control for arm joints {self.dof_indices}") - - def add_virtual_ee_link(self, EE_parent_link, ee_name, offset=[-0.04, 0, 0]): + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset): """Add virtual end effector link as an Xform under the specified parent link""" - # Full path to parent 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)) - - print(f"Created virtual EE link at {ee_prim_path}") From e6df5fe469890344f80522a7e2fdedb4cd1dfe2f Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Fri, 15 Aug 2025 15:35:08 -0400 Subject: [PATCH 39/42] fixed calc of body_link in J() and removed obsolete h1 setup --- abr_control/arms/isaacsim_config.py | 66 ++++++++++----------------- abr_control/interfaces/nv_isaacsim.py | 31 ++++--------- examples/IsaacSim/force_osc_xyz.py | 24 +++------- 3 files changed, 39 insertions(+), 82 deletions(-) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index cadd6454..10379f77 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -86,6 +86,7 @@ def __init__(self, robot_type): elif self.robot_type == "jaco2": 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" START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" self.target_min = np.array([-0.5, -0.5, 0.5]) @@ -99,12 +100,11 @@ def __init__(self, robot_type): self.EE_parent_link = "right_elbow_link" self.ee_offset=[0.26455, 0.00118, -0.0209] # for H1 START_ANGLES = "0. 0. 0. 0." - # self.target_min = np.array([0.1, -0.5, 1.4]) - self.target_min = np.array([0.12, -0.4, 1.3]) + self.target_min = np.array([0.12, -0.4, 1.4]) self.target_range = np.array([ 0.4 - 0.12, # x range - 0.12 - (-0.4), # y range - 2.2 - 1.3 # z range + 0.05 - (-0.4), # y range + 2.2 - 1.4 # z range ]) 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' @@ -113,15 +113,14 @@ def __init__(self, robot_type): elif self.robot_type == "h1_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_parent_link = "right_elbow_link" self.ee_link_name = "right_hand_link" START_ANGLES = "0. 0. 0. 0. 0." - # self.target_min = np.array([0.1, -0.55, 1.4]) - self.target_min = np.array([0.12, -0.4, 1.3]) + self.target_min = np.array([0.12, -0.4, 1.4]) self.target_range = np.array([ 0.49 - 0.12, # x range - 0.12 - (-0.4), # y range - 2.2 - 1.3 # z range + 0.05 - (-0.4), # y range + 2.2 - 1.4 # z range ]) 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' @@ -158,7 +157,7 @@ def _connect(self, world, stage, articulation_view, dof_indices, joint_indices, self.N_ALL_DOF = self.articulation_view.num_dof self.N_ALL_JOINTS = self.articulation_view.num_joints # offset for non-fixed base (mobile robots), necessary for indexing jacobian matrices etc correctly - self.base_offset = 6 if not self.is_fixed_base() else 0 + self.base_offset = 6 if not self._is_fixed_base() else 0 self.jac_indices = np.hstack( # 6 because position and rotation Jacobians are 3 x N_JOINTS @@ -183,17 +182,14 @@ def _connect(self, world, stage, articulation_view, dof_indices, joint_indices, self._x = np.ones(4) - # works but gets bumpy when out of reach and weird for higher targets def g(self, q=None): - full_gravity = self.articulation_view.get_generalized_gravity_forces()[0] # Drop the first 6 base DOFs for floating-base robots - joint_gravity = full_gravity[6:] - - #return joint_gravity[self.dof_indices] - - return np.zeros(len(self.dof_indices)) # No gravity compensation in this example - + joint_gravity = full_gravity[self.base_offset:] + # for some reason gravity is inverted for fixed_base + sign = -1 if self._is_fixed_base() else 1 + return sign * joint_gravity [self.dof_indices] + def dJ(self, name, q=None, dq=None, x=None): """Returns the derivative of the Jacobian wrt to time @@ -219,22 +215,11 @@ def J(self, name, q=None, x=None): if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments.") - # TODO this could be fixed by attachind virtual EE - if self.robot_type is "ur5": - link_index = 5 - elif self.robot_type is "jaco2": - link_index = 5 - # [6, 10, 14, 18] - elif self.robot_type is "h1": - #link_index = 18 - link_index = 24 - # [6, 10, 14, 18, 20] - elif self.robot_type is "h1_hands": - #link_index = 20 - link_index = 25 - + offset = 0 if not self._is_fixed_base() else -1 + body_index = self.articulation_view.get_body_index(self.EE_parent_link) + offset + env_idx = 0 # Assuming single environment - J_full = jacobians[env_idx, link_index, :, :] + J_full = jacobians[env_idx, body_index, :, :] if len(jacobians.shape) == 4: # apply offset for non-fixed base (mobile robots) indices = self.dof_indices + self.base_offset @@ -243,13 +228,11 @@ def J(self, name, q=None, x=None): else: raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") - # Assign to internal storage # Linear velocity Jacobian (first 3 rows) self._J6N[:3, :] = J[:3, :] # Angular velocity Jacobian (last 3 rows) self._J6N[3:, :] = J[3:, :] - # Additional validation - check if Jacobian makes sense (non-zero values) if np.allclose(J, 0): raise RuntimeError("Jacobian is all zeros - check robot configuration and joint addresses") @@ -376,34 +359,31 @@ def T_inv(self, name, q=None, x=None): raise NotImplementedError - # HELPER FUNCTIONS - # get the prim path for the name of the link, joint, or site def _get_prim_path(self, name): for prim in self.stage.Traverse(): - #TODO could be more general to inlcude TCP etc if str(prim.GetPath()).endswith(name): return prim.GetPath() return None - def get_joint_positions(self): + def _get_joint_positions(self): full_positions = self.articulation_view.get_joint_positions()[0] return full_positions[self.dof_indices] - def get_joint_velocities(self): + def _get_joint_velocities(self): full_velocities = self.articulation_view.get_joint_velocities()[0] return full_velocities[self.dof_indices] - def set_joint_positions(self, q): + def _set_joint_positions(self, q): full_positions = self.articulation_view.get_joint_positions()[0] full_positions[self.dof_indices] = q self.articulation_view.set_joint_positions(full_positions) - def set_joint_velocities(self, dq): + def _set_joint_velocities(self, dq): full_velocities = self.articulation_view.get_joint_velocities()[0] full_velocities[self.dof_indices] = dq self.articulation_view.set_joint_velocities(full_velocities) - def is_fixed_base(self): + def _is_fixed_base(self): num_dof = self.articulation_view.num_dof jacobian_shape = self.articulation_view.get_jacobian_shape()[2] return num_dof == jacobian_shape diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 88394f17..11b4b672 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -9,10 +9,8 @@ from omni.isaac.core.articulations import ArticulationView # type: ignore #from isaacsim.core.prims import Articulation # type: ignore from isaacsim.core.api.robots import Robot # type: ignore - from pxr import UsdGeom, Gf, UsdShade, Sdf # type: ignore from isaacsim.core.utils.nucleus import get_assets_root_path # type: ignore -from isaacsim.robot.policy.examples.robots import H1FlatTerrainPolicy # type: ignore import isaacsim.core.utils.numpy.rotations as rot_utils # type: ignore @@ -56,22 +54,12 @@ def connect(self, joint_names=None): 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}") - if self.robot_config.robot_type.startswith("h1"): - self.h1 = H1FlatTerrainPolicy( - prim_path=self.prim_path, - name=self.robot_config.robot_type, - usd_path=robot_usd_path, - position=np.array([0, 0 , 0]), - orientation=rot_utils.euler_angles_to_quats(np.array([0, 0, 0]), degrees=True), + stage_utils.add_reference_to_stage( + usd_path=robot_usd_path, + prim_path=self.prim_path, ) - - else: - 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)) - + robot = self.world.scene.add(Robot(prim_path=self.prim_path, name=self.robot_config.robot_type)) + self.world.reset() self.articulation_view = ArticulationView(prim_paths_expr=self.prim_path, name=self.robot_config.robot_type + "_view") @@ -92,7 +80,6 @@ def connect(self, joint_names=None): # Get joint information self.dof_indices = [] self.joint_indices = [] - self.joint_dyn_addrs = [] if joint_names is None: print("No joint names provided, using all controllable joints.") @@ -150,7 +137,7 @@ def send_target_angles(self, q): q : numpy.array the target joint angles [radians] """ - self.robot_config.set_joint_positions(q) + self.robot_config._set_joint_positions(q) self.world.step(render=True) @@ -159,8 +146,8 @@ def get_feedback(self): Returns the joint angles and joint velocities in [rad] and [rad/sec], respectively """ - self.q = self.robot_config.get_joint_positions() - self.dq = self.robot_config.get_joint_velocities() + self.q = self.robot_config._get_joint_positions() + self.dq = self.robot_config._get_joint_velocities() return {"q": self.q, "dq": self.dq} @@ -218,7 +205,7 @@ def set_gains_force_control(self): stiffness = np.ones(self.robot_config.N_ALL_DOF) * 100.0 # Default high stiffness damping = np.ones(self.robot_config.N_ALL_DOF) * 10.0 # Default damping - if self.robot_config.is_fixed_base(): + if self.robot_config._is_fixed_base(): controlled_s = 0.0 controlled_d = 0.1 else: diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index 16ba22f0..85037793 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -15,7 +15,7 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "ur5" + arm_model = "h1_hands" # "h1_hands" / "h1" / jaco2 / ur5 robot_config = arm(arm_model) dt = 0.007 # 143 Hz @@ -24,21 +24,15 @@ # create our IsaacSim interface interface = IsaacSim(robot_config, dt=dt) - -#interface.connect(joint_names=robot_config.controlled_dof) interface.connect() - - interface.send_target_angles(robot_config.START_ANGLES) isaac_target = interface.create_target_prim(prim_path=target_prim_path) - interface.set_gains_force_control() # disable gravity -interface.articulation_view.set_body_disable_gravity(True) -gravity = interface.articulation_view.get_body_disable_gravity()[0] -print("Gravity disabled: ", gravity) - +# interface.articulation_view.set_body_disable_gravity(True) +# gravity = interface.articulation_view.get_body_disable_gravity()[0] +# print("Gravity disabled: ", gravity) # damp the movements of the arm damping = Damping(robot_config, kv=30) #kv=10) @@ -62,8 +56,8 @@ def gen_target(interface): target_min = robot_config.target_min - target_range = np.array([1, 1, 0.5]) - target_xyz = (target_min + np.random.rand(3)) * target_range + target_range = robot_config.target_range + target_xyz = target_min + np.random.rand(3) * target_range interface.set_xyz(target_prim_path, target_xyz) @@ -100,7 +94,6 @@ def gen_target(interface): #interface.world.step(render=True) - # calculate end-effector position ee_xyz = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"]) # track data @@ -108,13 +101,10 @@ def gen_target(interface): target_track.append(np.copy(target[:3])) error = np.linalg.norm(ee_xyz - target[:3]) - if error < 0.02: - # interface.model.geom(target_geom).rgba = green + if error < 0.1: #0.02: count += 1 else: count = 0 - # interface.model.geom(target_geom).rgba = red - if count >= 50: print("Generating a new target") gen_target(interface) From 429213d841e600ea072ccbe69a501c5fcfd09032 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Tue, 19 Aug 2025 14:58:14 -0400 Subject: [PATCH 40/42] works with gravity now on all 4 robots (weird fix for h1) --- abr_control/arms/h1/__init__.py | 1 + abr_control/arms/h1/config.py | 269 ++++++++++++++++++++ abr_control/arms/h1/h1.xml | 249 ++++++++++++++++++ abr_control/arms/isaacsim_config.py | 247 +++++++++--------- abr_control/interfaces/nv_isaacsim.py | 71 +++--- examples/IsaacSim/force_floating_control.py | 11 +- examples/IsaacSim/force_osc_xyz.py | 32 +-- 7 files changed, 687 insertions(+), 193 deletions(-) create mode 100644 abr_control/arms/h1/__init__.py create mode 100644 abr_control/arms/h1/config.py create mode 100644 abr_control/arms/h1/h1.xml diff --git a/abr_control/arms/h1/__init__.py b/abr_control/arms/h1/__init__.py new file mode 100644 index 00000000..cca5d9bd --- /dev/null +++ b/abr_control/arms/h1/__init__.py @@ -0,0 +1 @@ +from .config import Config diff --git a/abr_control/arms/h1/config.py b/abr_control/arms/h1/config.py new file mode 100644 index 00000000..5ffc9be7 --- /dev/null +++ b/abr_control/arms/h1/config.py @@ -0,0 +1,269 @@ +import numpy as np +import sympy as sp + +from ..base_config import BaseConfig + + +class Config(BaseConfig): + """Robot config file for the right H1 arm for IsaacSim + + Attributes + ---------- + START_ANGLES : numpy.array + The joint angles for a safe home or rest position + _M_LINKS : sympy.diag + inertia matrix of the links + _M_JOINTS : sympy.diag + inertia matrix of the joints + L : numpy.array + segment lengths of arm [meters] + KZ : sympy.Matrix + z isolation vector in orientational part of Jacobian + + Transform Naming Convention: Tpoint1point2 + ex: Tj1l1 transforms from joint 1 to link 1 + + Transforms are broken up into two matrices for simplification + ex: Tj0l1a and Tj0l1b where the former transform accounts for + joint rotations and the latter accounts for static rotations + and translations + """ + + def __init__(self, **kwargs): + + super().__init__(N_JOINTS=4, N_LINKS=5, ROBOT_NAME="unitree_H1", **kwargs) + + self._T = {} # dictionary for storing calculated transforms + + self.JOINT_NAMES = [f"H1_right_arm_joint{ii}" for ii in range(self.N_JOINTS)] + + # for the null space controller, keep arm near these angles + self.START_ANGLES = np.array( + [0.0, 0.0, 0.0, 0.0], dtype="float32" + ) + + + # Inertia values from URDF links + self._M_LINKS.append(sp.zeros(6, 6)) # virtual base link0 + self._M_LINKS.append(sp.diag(1.032, 1.032, 1.032, 0.005703, 0.005703, 0.000524)) # shoulder_pitch + self._M_LINKS.append(sp.diag(0.918, 0.918, 0.918, 0.004284, 0.004284, 0.000504)) # shoulder_roll + self._M_LINKS.append(sp.diag(0.693, 0.693, 0.693, 0.002196, 0.002196, 0.000486)) # shoulder_yaw + self._M_LINKS.append(sp.diag(0.891, 0.891, 0.891, 0.004158, 0.004158, 0.000486)) # elbow + + + # the joints don't weigh anything + self._M_JOINTS = [sp.zeros(6, 6) for ii in range(self.N_JOINTS)] + + # segment lengths associated with each joint + # [x, y, z], Ignoring lengths < 1e-04 + + + + + self.L = [ + [0.0, 0.0, 0.0], # base offset (torso_link) + [0.0055, -0.15535, 0.42999], # shoulder_pitch joint offset + [0.0, 0.0, 0.0], # shoulder_pitch link offset (assume zero mesh offset) + [-0.0055, -0.0565, -0.0165], # shoulder_roll joint offset + [0.0, 0.0, 0.0], # shoulder_roll link offset (assume zero mesh offset) + [0.0, 0.0, -0.1343], # shoulder_yaw joint offset + [0.0, 0.0, 0.0], # shoulder_yaw link offset + [0.0185, 0.0, -0.198], # elbow joint offset + [0.0, 0.0, 0.0], # elbow link offset (elbow geometry appears centered) + [0.0, 0.0, 0.0], # CoM of hand (to be filled if available) + ] + self.L = np.array(self.L) + + + # Transform matrix : origin -> link 0 + # no change of axes, account for offsets + self.Torgl0 = sp.Matrix( + [ + [1, 0, 0, self.L[0, 0]], + [0, 1, 0, self.L[0, 1]], + [0, 0, 1, self.L[0, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : link 0 -> joint 0 + # no change of axes, account for offsets + self.Tl0j0 = sp.Matrix( + [ + [1, 0, 0, self.L[1, 0]], + [0, 1, 0, self.L[1, 1]], + [0, 0, 1, self.L[1, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : joint 0 -> link 1 + # account for rotation of q + self.Tj0l1a = sp.Matrix( + [ + [sp.cos(self.q[0]), -sp.sin(self.q[0]), 0, 0], + [sp.sin(self.q[0]), sp.cos(self.q[0]), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + # no change of axes, account for offsets + self.Tj0l1b = sp.Matrix( + [ + [1, 0, 0, self.L[2, 0]], + [0, 1, 0, self.L[2, 1]], + [0, 0, 1, self.L[2, 2]], + [0, 0, 0, 1], + ] + ) + self.Tj0l1 = self.Tj0l1a * self.Tj0l1b + + # Transform matrix : link 1 -> joint 1 + # no change of axes, account for offsets + self.Tl1j1 = sp.Matrix( + [ + [1, 0, 0, self.L[3, 0]], + [0, 1, 0, self.L[3, 1]], + [0, 0, 1, self.L[3, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : joint 1 -> link 2 + # account for rotation of q + self.Tj1l2a = sp.Matrix( + [ + [sp.cos(self.q[1]), -sp.sin(self.q[1]), 0, 0], + [sp.sin(self.q[1]), sp.cos(self.q[1]), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + # no change of axes, account for offsets + self.Tj1l2b = sp.Matrix( + [ + [1, 0, 0, self.L[4, 0]], + [0, 1, 0, self.L[4, 1]], + [0, 0, 1, self.L[4, 2]], + [0, 0, 0, 1], + ] + ) + self.Tj1l2 = self.Tj1l2a * self.Tj1l2b + + # Transform matrix : link 2 -> joint 2 + # no change of axes, account for offsets + self.Tl2j2 = sp.Matrix( + [ + [1, 0, 0, self.L[5, 0]], + [0, 1, 0, self.L[5, 1]], + [0, 0, 1, self.L[5, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : joint 2 -> link 3 + # account for rotation of q + self.Tj2l3a = sp.Matrix( + [ + [sp.cos(self.q[2]), -sp.sin(self.q[2]), 0, 0], + [sp.sin(self.q[2]), sp.cos(self.q[2]), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + # no change of axes, account for offsets + self.Tj2l3b = sp.Matrix( + [ + [1, 0, 0, self.L[6, 0]], + [0, 1, 0, self.L[6, 1]], + [0, 0, 1, self.L[6, 2]], + [0, 0, 0, 1], + ] + ) + self.Tj2l3 = self.Tj2l3a * self.Tj2l3b + + # Transform matrix : link 3 -> end-effector + # no change of axes, account for offsets + self.Tl3j3 = sp.Matrix( + [ + [1, 0, 0, self.L[7, 0]], + [0, 1, 0, self.L[7, 1]], + [0, 0, 1, self.L[7, 2]], + [0, 0, 0, 1], + ] + ) + + # Transform matrix : joint 3 -> link 4 + # account for rotation of q + self.Tj3l4a = sp.Matrix( + [ + [sp.cos(self.q[3]), -sp.sin(self.q[3]), 0, 0], + [sp.sin(self.q[3]), sp.cos(self.q[3]), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + # no change of axes, account for offsets + self.Tj3l4b = sp.Matrix( + [ + [1, 0, 0, self.L[8, 0]], + [0, 1, 0, self.L[8, 1]], + [0, 0, 1, self.L[8, 2]], + [0, 0, 0, 1], + ] + ) + self.Tj3l4 = self.Tj3l4a * self.Tj3l4b + + # Transform matrix : link 4 -> end-effector + # no change of axes, account for offsets + self.Tl4ee = sp.Matrix( + [ + [1, 0, 0, self.L[9, 0]], + [0, 1, 0, self.L[9, 1]], + [0, 0, 1, self.L[9, 2]], + [0, 0, 0, 1], + ] + ) + + + # orientation part of the Jacobian (compensating for angular velocity) + self.J_orientation = [ + self._calc_T("joint0")[:3, :3] * self._KZ, # joint 0 orientation + self._calc_T("joint1")[:3, :3] * self._KZ, # joint 1 orientation + self._calc_T("joint2")[:3, :3] * self._KZ, # joint 2 orientation + self._calc_T("joint3")[:3, :3] * self._KZ, + ] # joint 3 orientation + + def _calc_T(self, name): + """Uses Sympy to generate the transform for a joint or link + + name : string + name of the joint, link, or end-effector + """ + + if self._T.get(name, None) is None: + if name == "link0": + self._T[name] = self.Torgl0 + elif name == "joint0": + self._T[name] = self._calc_T("link0") * self.Tl0j0 + elif name == "link1": + self._T[name] = self._calc_T("joint0") * self.Tj0l1 + elif name == "joint1": + self._T[name] = self._calc_T("link1") * self.Tl1j1 + elif name == "link2": + self._T[name] = self._calc_T("joint1") * self.Tj1l2 + elif name == "joint2": + self._T[name] = self._calc_T("link2") * self.Tl2j2 + elif name == "link3": + self._T[name] = self._calc_T("joint2") * self.Tj2l3 + elif name == "joint3": + self._T[name] = self._calc_T("link3") * self.Tl3j3 + elif name == "link4": + self._T[name] = self._calc_T("joint3") * self.Tj3l4 + elif name == "EE": + self._T[name] = self._calc_T("link4") * self.Tl4ee + + else: + raise Exception(f"Invalid transformation name: {name}") + + return self._T[name] diff --git a/abr_control/arms/h1/h1.xml b/abr_control/arms/h1/h1.xml new file mode 100644 index 00000000..9b9bd346 --- /dev/null +++ b/abr_control/arms/h1/h1.xml @@ -0,0 +1,249 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 10379f77..0d0a73f8 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -3,73 +3,21 @@ class IsaacsimConfig: - """A wrapper on the Mujoco simulator to generate all the kinematics and + """A wrapper on the IsaacSim simulator to generate all the kinematics and dynamics calculations necessary for controllers. """ - # https://nvidia-omniverse.github.io/PhysX/physx/5.1.0/docs/Joints.html - JNT_POS_LENGTH_ISAACSIM = { - "free": 7, # 3 (translation) + 4 (quaternion rotation), usually not used in articulated chains - "spherical": 4, # Represented as quaternion in PxArticulationReducedCoordinate - "prismatic": 1, # Linear motion in one axis - "revolute": 1, # Rotational motion in one axis - } - ''' - from omni.isaac.core.articulations import ArticulationJointType - JNT_POS_LENGTH = { - ArticulationJointType.FREE: 7, # 3 for position + 4 for quaternion orientation - ArticulationJointType.BALL: 4, # quaternion orientation - ArticulationJointType.PRISMATIC: 1, # 1 DoF linear - ArticulationJointType.REVOLUTE: 1, # 1 DoF rotational - } - ''' - - JNT_DYN_LENGTH_ISAACSIM = { - "free": 6, # 3 linear + 3 angular velocity (used for root link only, not in articulated chains) - "spherical": 3, # Angular velocity vector (3D) - "prismatic": 1, # Linear velocity along one axis - "revolute": 1, # Angular velocity around one axis - } - - - - def __init__(self, robot_type): """Loads the Isaacsim model from the specified xml file Parameters ---------- - xml_file: string - the name of the arm model to load. If folder remains as None, - the string passed in is parsed such that everything up to the first - underscore is used for the arm directory, and the full string is - used to load the xml within that folder. - - EX: 'myArm' and 'myArm_with_gripper' will both look in the - 'myArm' directory, however they will load myArm.xml and - myArm_with_gripper.xml, respectively - - If a folder is passed in, then folder/xml_file is used - folder: string, Optional (Default: None) - specifies what folder to find the xml_file, if None specified will - checking in abr_control/arms/xml_file (see above for xml_file) - use_sim_state: Boolean, optional (Default: True) - If set False, the state information is provided by the user, which - is then used to calculate the corresponding dynamics values. - The state is then set back to the sim state prior to the user - provided state. - If set true, any q and dq values passed in to the functions are - ignored, and the current state of the simulator is used to - calculate all functions. This can speed up the simulation, because - the step of resetting the state on every call is omitted. - force_download: boolean, Optional (Default: False) - True to force downloading the mesh and texture files, useful when new files - are added that may be missing. - False: if the meshes folder is missing it will ask the user whether they - want to download them + robot_type: string + the name of the arm model to load. """ self.robot_type = robot_type + self.env_idx = 0 # Assuming only one robot self.ee_link_name = "EE" # name used for virtual end-effector, overwritten if one exists already if self.robot_type == "ur5": @@ -78,8 +26,8 @@ def __init__(self, robot_type): self.EE_parent_link = "wrist_3_link" self.ee_offset=[0., 0., 0.] START_ANGLES = "0 -.67 -.67 0 0 0" - self.target_min = np.array([-0.5, -0.5, 0.5]) - self.target_range = [1, 1, 0.5] + self.target_min = np.array([-0.6, -0.5, 0.5]) + self.target_range = np.array([0.9, 1.1, 0.3]) 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 with name '{self.ee_link_name}' is attached, as robot has none.") @@ -131,22 +79,32 @@ def __init__(self, robot_type): def _connect(self, world, stage, articulation_view, dof_indices, joint_indices, prim_path): - - """Called by the interface once the Mujoco simulation is created, - this connects the config to the simulator so it can access the - kinematics and dynamics information calculated by Mujoco. - - Parameters - ---------- - sim: MjSim - The Mujoco Simulator object created by the Mujoco Interface class - dof_indices: np.array of ints - The index of the robot joints in the Mujoco simulation data joint - position array - - """ - - # get access to the Isaac simulation + """Called by the interface once the IsaacSim simulation is created, + this connects the config to the simulator so it can access the + kinematics and dynamics information calculated by IsaacSim. + + Parameters + ---------- + world : omni.isaac.core.World + The Isaac Sim World object that manages the simulation environment + stage : pxr.Usd.Stage + The USD stage containing the scene hierarchy and prims + articulation_view : omni.isaac.core.articulations.ArticulationView + The ArticulationView object that provides access to the robot's + kinematic and dynamic properties in Isaac Sim + dof_indices : np.array of ints + The indices of the controlled robot joints in the articulation's + DOF array. These correspond to the joints that will be controlled + and for which kinematics/dynamics will be computed. + joint_indices : np.array of ints + The indices of the controlled robot joints in the articulation's + joint array. Note that joint_indices and dof_indices may differ + due to fixed joints or different indexing schemes. + prim_path : str + The USD prim path to the robot articulation in the scene hierarchy + (e.g., "/World/Robot") + """ + # get access to IsaacSim self.world = world self.stage = stage self.articulation_view = articulation_view @@ -156,11 +114,12 @@ def _connect(self, world, stage, articulation_view, dof_indices, joint_indices, 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._is_fixed_base() # offset for non-fixed base (mobile robots), necessary for indexing jacobian matrices etc correctly - self.base_offset = 6 if not self._is_fixed_base() else 0 + self.base_offset = 6 if not self._is_fixed_base else 0 self.jac_indices = np.hstack( - # 6 because position and rotation Jacobians are 3 x N_JOINTS + # 6 because position and rotation Jacobians are 3 x N_ALL_DOF [self.dof_indices + (ii * self.N_ALL_DOF) for ii in range(3)] ) @@ -173,23 +132,29 @@ def _connect(self, world, stage, articulation_view, dof_indices, joint_indices, # a place to store data returned from IsaacSim self._g = np.zeros(self.N_JOINTS) - self._J3NP = np.zeros((3, self.N_ALL_JOINTS)) - self._J3NR = np.zeros((3, self.N_ALL_JOINTS)) - self._J6N = np.zeros((6, self.N_JOINTS)) + #self._J3NP = np.zeros((3, self.N_ALL_DOF)) # TODO + #self._J3NR = np.zeros((3, self.N_ALL_DOF)) # TODO + self._J6N = np.zeros((6, self.N_JOINTS)) self._MNN = np.zeros((self.N_ALL_DOF, self.N_ALL_DOF)) - self._R9 = np.zeros(9) + #self._R9 = np.zeros(9) self._R = np.zeros((3, 3)) - self._x = np.ones(4) + #self._x = np.ones(4) def g(self, q=None): - full_gravity = self.articulation_view.get_generalized_gravity_forces()[0] - # Drop the first 6 base DOFs for floating-base robots - joint_gravity = full_gravity[self.base_offset:] - # for some reason gravity is inverted for fixed_base - sign = -1 if self._is_fixed_base() else 1 - return sign * joint_gravity [self.dof_indices] - + """Returns gravitational forces. + + Parameters + ---------- + q: float numpy.array, optional (Default: None) + The joint angles of the robot. + """ + self._g = self.articulation_view.get_generalized_gravity_forces()[self.env_idx] + if self._is_fixed_base: + return -self._g[self.dof_indices] + else: + return np.zeros(len(self.dof_indices)) + def dJ(self, name, q=None, dq=None, x=None): """Returns the derivative of the Jacobian wrt to time @@ -197,13 +162,11 @@ def dJ(self, name, q=None, dq=None, x=None): Parameters ---------- name: string - The name of the Mujoco body to retrieve the Jacobian for + The name of the IsaacSim prim to retrieve the derivative of the Jacobian for q: float numpy.array, optional (Default: None) - The joint angles of the robot. If None the current state is - retrieved from the Mujoco simulator + The joint angles of the robot. dq: float numpy.array, optional (Default: None) - The joint velocities of the robot. If None the current state is - retrieved from the Mujoco simulator + The joint velocities of the robot. x: float numpy.array, optional (Default: None) """ # TODO if ever required @@ -211,15 +174,26 @@ def dJ(self, name, q=None, dq=None, x=None): def J(self, name, q=None, x=None): + """Returns the Jacobian for the controlled DOF. + In case of mobile robots the floating base is accounted for by an offset. + + Parameters + ---------- + name: string + The name of the IsaacSim prim to retrieve the Jacobian for + q: float numpy.array, optional (Default: None) + The joint angles of the robot. + x: float numpy.array, optional (Default: None) + """ jacobians = self.articulation_view.get_jacobians(clone=True) - if jacobians.shape[0] == 0: + if jacobians.shape[self.env_idx] == 0: raise RuntimeError("ArticulationView contains no environments.") - offset = 0 if not self._is_fixed_base() else -1 + offset = 0 if not self._is_fixed_base else -1 body_index = self.articulation_view.get_body_index(self.EE_parent_link) + offset - env_idx = 0 # Assuming single environment - J_full = jacobians[env_idx, body_index, :, :] + + J_full = jacobians[self.env_idx, body_index, :, :] if len(jacobians.shape) == 4: # apply offset for non-fixed base (mobile robots) indices = self.dof_indices + self.base_offset @@ -235,17 +209,21 @@ def J(self, name, q=None, x=None): 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): - """ - Returns the inertia matrix for the controlled arm joints. + """Returns the inertia matrix in task space for the controlled DOF. + In case of mobile robots the floating base is accounted for by an offset. + + Parameters + ---------- + q: float numpy.array, optional (Default: None) + The joint angles of the robot. """ # get_mass_matrices returns (num_envs, joint_count, joint_count) - self._MNN= self.articulation_view.get_mass_matrices()[0] + self._MNN= self.articulation_view.get_mass_matrices()[self.env_idx] # apply offset for non-fixed base (mobile robots) indices = self.dof_indices + self.base_offset M = self._MNN[np.ix_(indices, indices)] @@ -254,7 +232,7 @@ def M(self, q=None): def R(self, name, q=None): """ - Returns the rotation matrix of the specified object in Isaac Sim. + Returns the rotation matrix of the specified object in IsaacSim. Parameters ---------- @@ -263,8 +241,7 @@ def R(self, name, q=None): q : np.ndarray, optional Joint positions (not used here unless you want to simulate a different state). """ - prim_path = self._get_prim_path(name) - prim = self.stage.GetPrimAtPath(prim_path) + prim = self._get_prim(name) if not prim.IsValid(): raise RuntimeError(f"Prim '{prim_path}' not found") @@ -272,31 +249,27 @@ def R(self, name, q=None): matrix = omni.usd.get_world_transform_matrix(prim) # Convert to 3x3 rotation matrix using numpy - R = np.array([ + 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 R - + return self._R def quaternion(self, name, q=None): - """Returns the quaternion + """Returns the quaternion of the specified prim. Parameters ---------- name: string - The name of the Mujoco body to retrieve the Jacobian for + The name of the IsaacSim prim to retrieve the Jacobian for q: float numpy.array, optional (Default: None) - The joint angles of the robot. If None the current state is - retrieved from the Mujoco simulator + The joint angles of the robot. """ if name == "EE": name = self.ee_link_name - - prim_path = self._get_prim_path(name) - prim = self.stage.GetPrimAtPath(prim_path) + prim = self._get_prim(name) # Get 4x4 world transform matrix matrix = omni.usd.get_world_transform_matrix(prim) @@ -309,20 +282,19 @@ def quaternion(self, name, q=None): def C(self): """NOTE: The Coriolis and centrifugal effects are neglected atm, as this method is not called anywhere """ - coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.dof_indices)[0] + coriolis = self.articulation_view.get_coriolis_and_centrifugal_forces(joint_indices=self.dof_indices)[self.env_idx] return coriolis def T(self, name, q=None, x=None): - """Get the transform matrix. + """Returns the transform matrix of the specified prim. Parameters ---------- name: string - The name of the Mujoco body to retrieve the Jacobian for + The name of the prim to retrieve the transform matrix from. q: float numpy.array, optional (Default: None) - The joint angles of the robot. If None the current state is - retrieved from the Mujoco simulator + The joint angles of the robot. x: float numpy.array, optional (Default: None) """ # TODO if ever required @@ -330,29 +302,36 @@ def T(self, name, q=None, x=None): def Tx(self, name, q=None, x=None): - """Simplified version that only gets current position without state changes.""" + """Simplified version of T. + Returns the position without state changes of the specified prim. + + Parameters + ---------- + name: string + The name of the prim to retrieve the position from. + q: float numpy.array, optional (Default: None) + The joint angles of the robot. + x: float numpy.array, optional (Default: None) + """ if name == "EE": name = self.ee_link_name - prim_path = self._get_prim_path(name) - prim = self.stage.GetPrimAtPath(prim_path) + prim = self._get_prim(name) if not prim.IsValid(): raise RuntimeError(f"Invalid prim at path: {prim_path}") 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 the inverse transform matrix. + """Returns the inverse of the transform matrix of the specified prim. Parameters ---------- name: string - The name of the Mujoco body to retrieve the Jacobian for + The name of the prim to retrieve the inverse from. q: float numpy.array, optional (Default: None) - The joint angles of the robot. If None the current state is - retrieved from the Mujoco simulator + The joint angles of the robot. x: float numpy.array, optional (Default: None) """ # TODO if ever required @@ -364,22 +343,28 @@ def _get_prim_path(self, name): if str(prim.GetPath()).endswith(name): return prim.GetPath() return None + + def _get_prim(self, name): + prim_path = self._get_prim_path(name) + prim = self.stage.GetPrimAtPath(prim_path) + return prim + def _get_joint_positions(self): - full_positions = self.articulation_view.get_joint_positions()[0] + full_positions = self.articulation_view.get_joint_positions()[self.env_idx] return full_positions[self.dof_indices] def _get_joint_velocities(self): - full_velocities = self.articulation_view.get_joint_velocities()[0] + full_velocities = self.articulation_view.get_joint_velocities()[self.env_idx] return full_velocities[self.dof_indices] def _set_joint_positions(self, q): - full_positions = self.articulation_view.get_joint_positions()[0] + 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): - full_velocities = self.articulation_view.get_joint_velocities()[0] + 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) diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 11b4b672..1924ef4e 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -7,9 +7,8 @@ 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.prims import Articulation # type: ignore from isaacsim.core.api.robots import Robot # type: ignore -from pxr import UsdGeom, Gf, UsdShade, Sdf # 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 @@ -27,7 +26,6 @@ class IsaacSim(Interface): """ def __init__(self, robot_config, dt=0.001): - super().__init__(robot_config) self.robot_config = robot_config self.dt = dt # time step @@ -44,7 +42,7 @@ def connect(self, joint_names=None): """All initial setup.""" # Initialize the simulation world - self.world = World(stage_units_in_meters=1.0) + 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() @@ -70,9 +68,6 @@ def connect(self, joint_names=None): if (self.robot_config.has_EE is False): 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) - - # Set simulation time step - self.world.get_physics_context().set_physics_dt(self.dt) # Reset the world to initialize physics self.world.reset() @@ -90,14 +85,14 @@ def connect(self, joint_names=None): self.all_body_names = self.articulation_view.body_names for name in joint_names: - if name not in self.all_dof_names and name not in self.all_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) - + # Connect robot config with simulation data print("Connecting to robot config...") self.robot_config._connect( @@ -108,9 +103,13 @@ def connect(self, joint_names=None): self.joint_indices, self.prim_path, ) - - if self.robot_config.robot_type.startswith("h1"): + # additional setup necessary for mobile robots (at least h1) + if not self.robot_config._is_fixed_base: self.world.add_physics_callback("keep_standing", self.keep_standing) + # also works without, but is more reactive if max force is adapted + [self.set_max_force(name=joint_name, value=0.7) for joint_name in self.robot_config.controlled_dof] + else: + self.set_gains() def disconnect(self): @@ -120,7 +119,11 @@ def disconnect(self): def send_forces(self, u): - """Applies the torques u to the joints specified in indices.""" + """Applies the torques u to the DOF specified in dof_indices. + + u : numpy.array + the forces to apply to the controlled DOF. + """ # Create full torque vector for all DOFs full_torques = np.zeros(self.robot_config.N_ALL_DOF) # Apply control torques to the controlled joints @@ -151,15 +154,15 @@ def get_feedback(self): return {"q": self.q, "dq": self.dq} - def set_xyz(self, prim_path, xyz): + def set_xyz(self, name, xyz): """Set the position of an object in the environment. prim_path : string - the prim_path of the object + the prim_path of the prim xyz : np.array the [x,y,z] location of the target [meters] """ - prim = self.stage.GetPrimAtPath(prim_path) + 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) @@ -195,30 +198,36 @@ def create_target_prim(self, prim_path="/World/target", size = .1, color=np.arra # Disable collision to ensure it's purely visual cube_prim.GetPrim().CreateAttribute("physics:collisionEnabled", Sdf.ValueTypeNames.Bool).Set(False) - return cube_prim - - def set_gains_force_control(self): + + def set_target_random(self, name="target"): + target_min = self.robot_config.target_min + target_range = self.robot_config.target_range + target_xyz = target_min + np.random.rand(3) * target_range + self.set_xyz(name, target_xyz) + + + # setting max_force via PhysX DriveAPI + def set_max_force(self, name, value): + prim = self.robot_config._get_prim(name) + # Apply the DriveAPI if not already present + driveAPI = UsdPhysics.DriveAPI.Apply(prim, "angular") # Use "linear" for prismatic joints + # Set the max force + driveAPI.CreateMaxForceAttr(value) + + + def set_gains(self): """Properly set gains for arm joints (DOFs 0-5) and finger joints if present""" # Get current gains or set defaults stiffness = np.ones(self.robot_config.N_ALL_DOF) * 100.0 # Default high stiffness damping = np.ones(self.robot_config.N_ALL_DOF) * 10.0 # Default damping - - if self.robot_config._is_fixed_base(): - controlled_s = 0.0 - controlled_d = 0.1 - else: - controlled_s = 2.0 - controlled_d = 0.5 - for idx in self.dof_indices: - stiffness[idx] = controlled_s - damping[idx] = controlled_d + stiffness[idx] = 0.0 + damping[idx] = 0.1 self.articulation_view.set_gains(stiffness, damping) - print(f"Set gains for force control for arm joints {self.dof_indices}") - + def add_virtual_ee_link(self, EE_parent_link, ee_name, offset): """Add virtual end effector link as an Xform under the specified parent link""" @@ -228,4 +237,4 @@ def add_virtual_ee_link(self, EE_parent_link, ee_name, offset): # 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)) + 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 index ad24d3e3..2bf38cc8 100644 --- a/examples/IsaacSim/force_floating_control.py +++ b/examples/IsaacSim/force_floating_control.py @@ -7,7 +7,6 @@ """ import sys import traceback - import numpy as np from abr_control.arms.isaacsim_config import IsaacsimConfig as arm from abr_control.controllers import Floating @@ -15,16 +14,13 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "h1_hands" - #arm_model = "jaco2" + arm_model = "ur5" robot_config = arm(arm_model) -interface = IsaacSim(robot_config, dt = 0.007) +dt = 0.005 +interface = IsaacSim(robot_config, dt) interface.connect(joint_names=robot_config.controlled_dof) - - interface.send_target_angles(robot_config.START_ANGLES) -interface.set_gains_force_control() # instantiate the controller ctrlr = Floating(robot_config, task_space=False, dynamic=True) @@ -47,7 +43,6 @@ # send forces into Isaacsim interface.send_forces(u) - #interface.world.step(render=True) # calculate the position of the hand hand_xyz = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"]) diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index 85037793..d7de2ae9 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -15,24 +15,17 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "h1_hands" # "h1_hands" / "h1" / jaco2 / ur5 + arm_model = "ur5" # "h1_hands" / "h1" / jaco2 / ur5 robot_config = arm(arm_model) -dt = 0.007 # 143 Hz -#dt = 0.001 # 1000 Hz -target_prim_path="/World/target" +dt = 0.005 +target_name="target" # create our IsaacSim interface -interface = IsaacSim(robot_config, dt=dt) +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) -interface.set_gains_force_control() - -# disable gravity -# interface.articulation_view.set_body_disable_gravity(True) -# gravity = interface.articulation_view.get_body_disable_gravity()[0] -# print("Gravity disabled: ", gravity) +isaac_target = interface.create_target_prim() # damp the movements of the arm damping = Damping(robot_config, kv=30) #kv=10) @@ -54,12 +47,6 @@ np.random.seed(0) -def gen_target(interface): - target_min = robot_config.target_min - target_range = robot_config.target_range - target_xyz = target_min + np.random.rand(3) * target_range - interface.set_xyz(target_prim_path, target_xyz) - try: # get the end-effector's initial position @@ -67,7 +54,7 @@ def gen_target(interface): start = robot_config.Tx(robot_config.ee_link_name, feedback["q"]) # make the target offset from that start position - gen_target(interface) + interface.set_target_random() count = 0 print("\nSimulation starting...\n") @@ -76,9 +63,9 @@ def gen_target(interface): feedback = interface.get_feedback() target = np.hstack( [ - robot_config.Tx(target_prim_path), + robot_config.Tx(target_name), transformations.euler_from_quaternion( - robot_config.quaternion(target_prim_path), "rxyz" + robot_config.quaternion(target_name), "rxyz" ), ] ) @@ -93,7 +80,6 @@ def gen_target(interface): interface.send_forces(u) #interface.world.step(render=True) - # calculate end-effector position ee_xyz = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"]) # track data @@ -107,7 +93,7 @@ def gen_target(interface): count = 0 if count >= 50: print("Generating a new target") - gen_target(interface) + interface.set_target_random() count = 0 except: From be900e39343ee2f5f1f87233639e524eee204b2b Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Wed, 20 Aug 2025 14:59:37 -0400 Subject: [PATCH 41/42] example force_osc_xyz_linear_path_gaussian_velocity works with ur5 and jaco2 --- abr_control/arms/isaacsim_config.py | 21 ++----- abr_control/interfaces/nv_isaacsim.py | 35 +++++++++--- examples/IsaacSim/force_osc_xyz.py | 9 +-- ..._osc_xyz_linear_path_gaussian_velocity.py} | 57 ++++--------------- 4 files changed, 47 insertions(+), 75 deletions(-) rename examples/IsaacSim/{force_osc_xyz_linear_path_gaussian_velocity_WIP.py => force_osc_xyz_linear_path_gaussian_velocity.py} (86%) diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index 0d0a73f8..a8843601 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -15,7 +15,6 @@ def __init__(self, robot_type): robot_type: string the name of the arm model to load. """ - self.robot_type = robot_type self.env_idx = 0 # Assuming only one robot self.ee_link_name = "EE" # name used for virtual end-effector, overwritten if one exists already @@ -26,8 +25,8 @@ def __init__(self, robot_type): self.EE_parent_link = "wrist_3_link" self.ee_offset=[0., 0., 0.] START_ANGLES = "0 -.67 -.67 0 0 0" - self.target_min = np.array([-0.6, -0.5, 0.5]) - self.target_range = np.array([0.9, 1.1, 0.3]) + self.target_min = np.array([-0.4, -0.4, 0.3]) # np.array([-0.6, -0.5, 0.5]) + 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 with name '{self.ee_link_name}' is attached, as robot has none.") @@ -37,8 +36,8 @@ def __init__(self, robot_type): self.EE_parent_link = "j2n6s300_link_6" self.ee_link_name = "j2n6s300_end_effector" START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" - self.target_min = np.array([-0.5, -0.5, 0.5]) - self.target_range = [1, 1, 0.5] + self.target_min = np.array([-0.4, -0.4, 0.3]) # np.array([-0.5, -0.5, 0.5]) + 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 with name '{self.ee_link_name}' specified in UDS, using it ...") @@ -49,11 +48,7 @@ def __init__(self, robot_type): self.ee_offset=[0.26455, 0.00118, -0.0209] # for H1 START_ANGLES = "0. 0. 0. 0." self.target_min = np.array([0.12, -0.4, 1.4]) - self.target_range = np.array([ - 0.4 - 0.12, # x range - 0.05 - (-0.4), # y range - 2.2 - 1.4 # z range - ]) + 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 with name '{self.ee_link_name}' is attached, as robot has none.") @@ -65,11 +60,7 @@ def __init__(self, robot_type): self.ee_link_name = "right_hand_link" START_ANGLES = "0. 0. 0. 0. 0." self.target_min = np.array([0.12, -0.4, 1.4]) - self.target_range = np.array([ - 0.49 - 0.12, # x range - 0.05 - (-0.4), # y range - 2.2 - 1.4 # z range - ]) + 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 with name '{self.ee_link_name}' specified in UDS, using it ...") diff --git a/abr_control/interfaces/nv_isaacsim.py b/abr_control/interfaces/nv_isaacsim.py index 1924ef4e..9553f16f 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -157,7 +157,7 @@ def get_feedback(self): def set_xyz(self, name, xyz): """Set the position of an object in the environment. - prim_path : string + name : string the prim_path of the prim xyz : np.array the [x,y,z] location of the target [meters] @@ -166,6 +166,23 @@ def set_xyz(self, name, xyz): 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 position of an object in the environment. + + name : string + the prim_path of the prim + quat_wxyz : np.array + 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) # method that humanoid does not fall over @@ -200,12 +217,16 @@ def create_target_prim(self, prim_path="/World/target", size = .1, color=np.arra cube_prim.GetPrim().CreateAttribute("physics:collisionEnabled", Sdf.ValueTypeNames.Bool).Set(False) return cube_prim - - def set_target_random(self, name="target"): - target_min = self.robot_config.target_min - target_range = self.robot_config.target_range - target_xyz = target_min + np.random.rand(3) * target_range - self.set_xyz(name, target_xyz) + + def create_random_pos(self): + 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 # setting max_force via PhysX DriveAPI diff --git a/examples/IsaacSim/force_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index d7de2ae9..0b074326 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -15,10 +15,10 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "ur5" # "h1_hands" / "h1" / jaco2 / ur5 + arm_model = "h1" # works with "h1_hands" / "h1" / jaco2 / ur5 robot_config = arm(arm_model) -dt = 0.005 +dt = 0.005 # 200 Hz target_name="target" # create our IsaacSim interface @@ -51,10 +51,7 @@ try: # get the end-effector's initial position feedback = interface.get_feedback() - start = robot_config.Tx(robot_config.ee_link_name, feedback["q"]) - - # make the target offset from that start position - interface.set_target_random() + interface.set_xyz("target", interface.create_random_pos()) count = 0 print("\nSimulation starting...\n") diff --git a/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity_WIP.py b/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity.py similarity index 86% rename from examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity_WIP.py rename to examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity.py index 95afc6df..645b7f37 100644 --- a/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity_WIP.py +++ b/examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity.py @@ -21,8 +21,7 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "jaco2" -# initialize our robot config for the jaco2 + arm_model = "jaco2" # works with jaco2 / ur5 robot_config = arm(arm_model) ctrlr_dof = [True, True, True, False, False, False] @@ -32,16 +31,14 @@ print(stars) print(dof_print) print(stars) -dt = 0.007 # 143 Hz -#dt = 0.001 # 1000 Hz +dt = 0.005 # 200 Hz target_prim_path="/World/target" # create our interface -interface = IsaacSim(robot_config, dt=dt) +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) -interface.fix_arm_joint_gains() # damp the movements of the arm damping = Damping(robot_config, kv=10) @@ -66,35 +63,17 @@ target_track = [] - - - - - - - - - - print("\nSimulation starting...\n") for ii in range(0, n_targets): feedback = interface.get_feedback() - hand_xyz = robot_config.Tx("EE", 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), - ] - ) - + 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) + interface.set_xyz("target", pos_target) + #interface.set_xyz(target_prim_path, pos_target) at_target = 0 count = 0 @@ -103,10 +82,9 @@ 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("EE", feedback["q"]) + hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"]) u = ctrlr.generate( q=feedback["q"], @@ -128,26 +106,11 @@ count += 1 - - - - - - - - - - - - - - - try: print("\nSimulation starting...\n") for ii in range(0, n_targets): feedback = interface.get_feedback() - hand_xyz = robot_config.Tx("EE", feedback["q"]) + hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"]) pos_target = np.array( [ @@ -174,7 +137,7 @@ #interface.set_xyz("target_orientation", filtered_target[:3]) feedback = interface.get_feedback() - hand_xyz = robot_config.Tx("EE", feedback["q"]) + hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"]) u = ctrlr.generate( q=feedback["q"], From c503fbe3c983b5977380f11f75855296a3190bf8 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Thu, 21 Aug 2025 09:51:25 -0400 Subject: [PATCH 42/42] added documentation --- abr_control/arms/h1/__init__.py | 1 - abr_control/arms/h1/config.py | 269 ----------- abr_control/arms/h1/h1.xml | 249 ---------- abr_control/arms/isaacsim_config.py | 644 +++++++++++++++++--------- abr_control/interfaces/nv_isaacsim.py | 418 ++++++++++++----- examples/IsaacSim/force_osc_xyz.py | 8 +- 6 files changed, 733 insertions(+), 856 deletions(-) delete mode 100644 abr_control/arms/h1/__init__.py delete mode 100644 abr_control/arms/h1/config.py delete mode 100644 abr_control/arms/h1/h1.xml diff --git a/abr_control/arms/h1/__init__.py b/abr_control/arms/h1/__init__.py deleted file mode 100644 index cca5d9bd..00000000 --- a/abr_control/arms/h1/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .config import Config diff --git a/abr_control/arms/h1/config.py b/abr_control/arms/h1/config.py deleted file mode 100644 index 5ffc9be7..00000000 --- a/abr_control/arms/h1/config.py +++ /dev/null @@ -1,269 +0,0 @@ -import numpy as np -import sympy as sp - -from ..base_config import BaseConfig - - -class Config(BaseConfig): - """Robot config file for the right H1 arm for IsaacSim - - Attributes - ---------- - START_ANGLES : numpy.array - The joint angles for a safe home or rest position - _M_LINKS : sympy.diag - inertia matrix of the links - _M_JOINTS : sympy.diag - inertia matrix of the joints - L : numpy.array - segment lengths of arm [meters] - KZ : sympy.Matrix - z isolation vector in orientational part of Jacobian - - Transform Naming Convention: Tpoint1point2 - ex: Tj1l1 transforms from joint 1 to link 1 - - Transforms are broken up into two matrices for simplification - ex: Tj0l1a and Tj0l1b where the former transform accounts for - joint rotations and the latter accounts for static rotations - and translations - """ - - def __init__(self, **kwargs): - - super().__init__(N_JOINTS=4, N_LINKS=5, ROBOT_NAME="unitree_H1", **kwargs) - - self._T = {} # dictionary for storing calculated transforms - - self.JOINT_NAMES = [f"H1_right_arm_joint{ii}" for ii in range(self.N_JOINTS)] - - # for the null space controller, keep arm near these angles - self.START_ANGLES = np.array( - [0.0, 0.0, 0.0, 0.0], dtype="float32" - ) - - - # Inertia values from URDF links - self._M_LINKS.append(sp.zeros(6, 6)) # virtual base link0 - self._M_LINKS.append(sp.diag(1.032, 1.032, 1.032, 0.005703, 0.005703, 0.000524)) # shoulder_pitch - self._M_LINKS.append(sp.diag(0.918, 0.918, 0.918, 0.004284, 0.004284, 0.000504)) # shoulder_roll - self._M_LINKS.append(sp.diag(0.693, 0.693, 0.693, 0.002196, 0.002196, 0.000486)) # shoulder_yaw - self._M_LINKS.append(sp.diag(0.891, 0.891, 0.891, 0.004158, 0.004158, 0.000486)) # elbow - - - # the joints don't weigh anything - self._M_JOINTS = [sp.zeros(6, 6) for ii in range(self.N_JOINTS)] - - # segment lengths associated with each joint - # [x, y, z], Ignoring lengths < 1e-04 - - - - - self.L = [ - [0.0, 0.0, 0.0], # base offset (torso_link) - [0.0055, -0.15535, 0.42999], # shoulder_pitch joint offset - [0.0, 0.0, 0.0], # shoulder_pitch link offset (assume zero mesh offset) - [-0.0055, -0.0565, -0.0165], # shoulder_roll joint offset - [0.0, 0.0, 0.0], # shoulder_roll link offset (assume zero mesh offset) - [0.0, 0.0, -0.1343], # shoulder_yaw joint offset - [0.0, 0.0, 0.0], # shoulder_yaw link offset - [0.0185, 0.0, -0.198], # elbow joint offset - [0.0, 0.0, 0.0], # elbow link offset (elbow geometry appears centered) - [0.0, 0.0, 0.0], # CoM of hand (to be filled if available) - ] - self.L = np.array(self.L) - - - # Transform matrix : origin -> link 0 - # no change of axes, account for offsets - self.Torgl0 = sp.Matrix( - [ - [1, 0, 0, self.L[0, 0]], - [0, 1, 0, self.L[0, 1]], - [0, 0, 1, self.L[0, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : link 0 -> joint 0 - # no change of axes, account for offsets - self.Tl0j0 = sp.Matrix( - [ - [1, 0, 0, self.L[1, 0]], - [0, 1, 0, self.L[1, 1]], - [0, 0, 1, self.L[1, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : joint 0 -> link 1 - # account for rotation of q - self.Tj0l1a = sp.Matrix( - [ - [sp.cos(self.q[0]), -sp.sin(self.q[0]), 0, 0], - [sp.sin(self.q[0]), sp.cos(self.q[0]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - ) - # no change of axes, account for offsets - self.Tj0l1b = sp.Matrix( - [ - [1, 0, 0, self.L[2, 0]], - [0, 1, 0, self.L[2, 1]], - [0, 0, 1, self.L[2, 2]], - [0, 0, 0, 1], - ] - ) - self.Tj0l1 = self.Tj0l1a * self.Tj0l1b - - # Transform matrix : link 1 -> joint 1 - # no change of axes, account for offsets - self.Tl1j1 = sp.Matrix( - [ - [1, 0, 0, self.L[3, 0]], - [0, 1, 0, self.L[3, 1]], - [0, 0, 1, self.L[3, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : joint 1 -> link 2 - # account for rotation of q - self.Tj1l2a = sp.Matrix( - [ - [sp.cos(self.q[1]), -sp.sin(self.q[1]), 0, 0], - [sp.sin(self.q[1]), sp.cos(self.q[1]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - ) - # no change of axes, account for offsets - self.Tj1l2b = sp.Matrix( - [ - [1, 0, 0, self.L[4, 0]], - [0, 1, 0, self.L[4, 1]], - [0, 0, 1, self.L[4, 2]], - [0, 0, 0, 1], - ] - ) - self.Tj1l2 = self.Tj1l2a * self.Tj1l2b - - # Transform matrix : link 2 -> joint 2 - # no change of axes, account for offsets - self.Tl2j2 = sp.Matrix( - [ - [1, 0, 0, self.L[5, 0]], - [0, 1, 0, self.L[5, 1]], - [0, 0, 1, self.L[5, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : joint 2 -> link 3 - # account for rotation of q - self.Tj2l3a = sp.Matrix( - [ - [sp.cos(self.q[2]), -sp.sin(self.q[2]), 0, 0], - [sp.sin(self.q[2]), sp.cos(self.q[2]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - ) - # no change of axes, account for offsets - self.Tj2l3b = sp.Matrix( - [ - [1, 0, 0, self.L[6, 0]], - [0, 1, 0, self.L[6, 1]], - [0, 0, 1, self.L[6, 2]], - [0, 0, 0, 1], - ] - ) - self.Tj2l3 = self.Tj2l3a * self.Tj2l3b - - # Transform matrix : link 3 -> end-effector - # no change of axes, account for offsets - self.Tl3j3 = sp.Matrix( - [ - [1, 0, 0, self.L[7, 0]], - [0, 1, 0, self.L[7, 1]], - [0, 0, 1, self.L[7, 2]], - [0, 0, 0, 1], - ] - ) - - # Transform matrix : joint 3 -> link 4 - # account for rotation of q - self.Tj3l4a = sp.Matrix( - [ - [sp.cos(self.q[3]), -sp.sin(self.q[3]), 0, 0], - [sp.sin(self.q[3]), sp.cos(self.q[3]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1], - ] - ) - # no change of axes, account for offsets - self.Tj3l4b = sp.Matrix( - [ - [1, 0, 0, self.L[8, 0]], - [0, 1, 0, self.L[8, 1]], - [0, 0, 1, self.L[8, 2]], - [0, 0, 0, 1], - ] - ) - self.Tj3l4 = self.Tj3l4a * self.Tj3l4b - - # Transform matrix : link 4 -> end-effector - # no change of axes, account for offsets - self.Tl4ee = sp.Matrix( - [ - [1, 0, 0, self.L[9, 0]], - [0, 1, 0, self.L[9, 1]], - [0, 0, 1, self.L[9, 2]], - [0, 0, 0, 1], - ] - ) - - - # orientation part of the Jacobian (compensating for angular velocity) - self.J_orientation = [ - self._calc_T("joint0")[:3, :3] * self._KZ, # joint 0 orientation - self._calc_T("joint1")[:3, :3] * self._KZ, # joint 1 orientation - self._calc_T("joint2")[:3, :3] * self._KZ, # joint 2 orientation - self._calc_T("joint3")[:3, :3] * self._KZ, - ] # joint 3 orientation - - def _calc_T(self, name): - """Uses Sympy to generate the transform for a joint or link - - name : string - name of the joint, link, or end-effector - """ - - if self._T.get(name, None) is None: - if name == "link0": - self._T[name] = self.Torgl0 - elif name == "joint0": - self._T[name] = self._calc_T("link0") * self.Tl0j0 - elif name == "link1": - self._T[name] = self._calc_T("joint0") * self.Tj0l1 - elif name == "joint1": - self._T[name] = self._calc_T("link1") * self.Tl1j1 - elif name == "link2": - self._T[name] = self._calc_T("joint1") * self.Tj1l2 - elif name == "joint2": - self._T[name] = self._calc_T("link2") * self.Tl2j2 - elif name == "link3": - self._T[name] = self._calc_T("joint2") * self.Tj2l3 - elif name == "joint3": - self._T[name] = self._calc_T("link3") * self.Tl3j3 - elif name == "link4": - self._T[name] = self._calc_T("joint3") * self.Tj3l4 - elif name == "EE": - self._T[name] = self._calc_T("link4") * self.Tl4ee - - else: - raise Exception(f"Invalid transformation name: {name}") - - return self._T[name] diff --git a/abr_control/arms/h1/h1.xml b/abr_control/arms/h1/h1.xml deleted file mode 100644 index 9b9bd346..00000000 --- a/abr_control/arms/h1/h1.xml +++ /dev/null @@ -1,249 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/abr_control/arms/isaacsim_config.py b/abr_control/arms/isaacsim_config.py index a8843601..25889efd 100644 --- a/abr_control/arms/isaacsim_config.py +++ b/abr_control/arms/isaacsim_config.py @@ -1,366 +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 on the IsaacSim simulator to generate all the kinematics and - dynamics calculations necessary for controllers. + """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): - """Loads the Isaacsim model from the specified xml file + """Initialize robot configuration for specified robot type. Parameters ---------- - robot_type: string - the name of the arm model to load. + 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 only one robot - self.ee_link_name = "EE" # name used for virtual end-effector, overwritten if one exists already + self.env_idx = 0 # Assuming single robot environment + self.ee_link_name = "EE" # Default name for virtual end-effector - if self.robot_type == "ur5": - 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.] - START_ANGLES = "0 -.67 -.67 0 0 0" - self.target_min = np.array([-0.4, -0.4, 0.3]) # np.array([-0.6, -0.5, 0.5]) - 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 with name '{self.ee_link_name}' is attached, as robot has none.") + # 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.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" - START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04" - self.target_min = np.array([-0.4, -0.4, 0.3]) # np.array([-0.5, -0.5, 0.5]) - 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 with name '{self.ee_link_name}' specified in UDS, using it ...") - - elif self.robot_type == "h1": - 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] # for H1 - START_ANGLES = "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 with name '{self.ee_link_name}' is attached, as robot has none.") - - elif self.robot_type == "h1_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" - START_ANGLES = "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 with name '{self.ee_link_name}' specified in UDS, using it ...") - - self.START_ANGLES = np.array(START_ANGLES.split(), dtype=float) + 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. - - def _connect(self, world, stage, articulation_view, dof_indices, joint_indices, prim_path): - """Called by the interface once the IsaacSim simulation is created, - this connects the config to the simulator so it can access the - kinematics and dynamics information calculated by IsaacSim. - - Parameters - ---------- - world : omni.isaac.core.World - The Isaac Sim World object that manages the simulation environment - stage : pxr.Usd.Stage - The USD stage containing the scene hierarchy and prims - articulation_view : omni.isaac.core.articulations.ArticulationView - The ArticulationView object that provides access to the robot's - kinematic and dynamic properties in Isaac Sim - dof_indices : np.array of ints - The indices of the controlled robot joints in the articulation's - DOF array. These correspond to the joints that will be controlled - and for which kinematics/dynamics will be computed. - joint_indices : np.array of ints - The indices of the controlled robot joints in the articulation's - joint array. Note that joint_indices and dof_indices may differ - due to fixed joints or different indexing schemes. - prim_path : str - The USD prim path to the robot articulation in the scene hierarchy - (e.g., "/World/Robot") - """ - # get access to IsaacSim + 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._is_fixed_base() - # offset for non-fixed base (mobile robots), necessary for indexing jacobian matrices etc correctly - self.base_offset = 6 if not self._is_fixed_base else 0 + 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() - self.jac_indices = np.hstack( - # 6 because position and rotation Jacobians are 3 x N_ALL_DOF - [self.dof_indices + (ii * self.N_ALL_DOF) for ii in range(3)] - ) + 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 - # for the inertia matrix + 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 ] - # a place to store data returned from IsaacSim + def _initialize_data_arrays(self): + """Initialize arrays for storing dynamics data.""" self._g = np.zeros(self.N_JOINTS) - #self._J3NP = np.zeros((3, self.N_ALL_DOF)) # TODO - #self._J3NR = np.zeros((3, self.N_ALL_DOF)) # TODO - self._J6N = np.zeros((6, self.N_JOINTS)) + self._J6N = np.zeros((6, self.N_JOINTS)) self._MNN = np.zeros((self.N_ALL_DOF, self.N_ALL_DOF)) - #self._R9 = np.zeros(9) self._R = np.zeros((3, 3)) - #self._x = np.ones(4) - def g(self, q=None): - """Returns gravitational forces. + """Get gravitational forces for controlled joints. Parameters ---------- - q: float numpy.array, optional (Default: None) - The joint angles of the robot. + q : numpy.ndarray, optional + Joint angles (not used in current implementation) + + Returns + ------- + numpy.ndarray + Gravitational forces for controlled joints """ - self._g = self.articulation_view.get_generalized_gravity_forces()[self.env_idx] + 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): - """Returns the derivative of the Jacobian wrt to time + """Get derivative of Jacobian with respect to time. Parameters ---------- - name: string - The name of the IsaacSim prim to retrieve the derivative of the Jacobian for - q: float numpy.array, optional (Default: None) - The joint angles of the robot. - dq: float numpy.array, optional (Default: None) - The joint velocities of the robot. - x: float numpy.array, optional (Default: None) + 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 """ - # TODO if ever required - raise NotImplementedError - + raise NotImplementedError("Jacobian derivative not implemented yet") def J(self, name, q=None, x=None): - """Returns the Jacobian for the controlled DOF. - In case of mobile robots the floating base is accounted for by an offset. + """Get Jacobian matrix for the specified end-effector. + + For mobile robots, the floating base is accounted for with an offset. Parameters ---------- - name: string - The name of the IsaacSim prim to retrieve the Jacobian for - q: float numpy.array, optional (Default: None) - The joint angles of the robot. - x: float numpy.array, optional (Default: None) + 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[self.env_idx] == 0: + + if jacobians.shape[0] == 0: raise RuntimeError("ArticulationView contains no environments.") - offset = 0 if not self._is_fixed_base else -1 + # 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 non-fixed base (mobile robots) + # Apply offset for mobile robots and extract controlled DOF columns indices = self.dof_indices + self.base_offset - # Extract only the columns for controllable DOFs J = J_full[:, indices] else: raise RuntimeError(f"Unexpected jacobians shape: {jacobians.shape}") - - # Linear velocity Jacobian (first 3 rows) - self._J6N[:3, :] = J[:3, :] - # Angular velocity Jacobian (last 3 rows) - self._J6N[3:, :] = J[3:, :] - - if np.allclose(J, 0): - raise RuntimeError("Jacobian is all zeros - check robot configuration and joint addresses") - return np.copy(self._J6N) + # 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): - """Returns the inertia matrix in task space for the controlled DOF. - In case of mobile robots the floating base is accounted for by an offset. + """Get mass/inertia matrix for controlled joints. + + For mobile robots, the floating base is accounted for with an offset. Parameters ---------- - q: float numpy.array, optional (Default: None) - The joint angles of the robot. + 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_mass_matrices returns (num_envs, joint_count, joint_count) - self._MNN= self.articulation_view.get_mass_matrices()[self.env_idx] - # apply offset for non-fixed base (mobile robots) + # 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): - """ - Returns the rotation matrix of the specified object in IsaacSim. + """Get rotation matrix for the specified object. Parameters ---------- name : str - The name of the object (body, geom, or site) in the USD stage. - q : np.ndarray, optional - Joint positions (not used here unless you want to simulate a different state). + 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 '{prim_path}' not found") - - # Get 4x4 world transform matrix + raise RuntimeError(f"Prim '{name}' not found or invalid") + + # Get 4×4 world transform matrix matrix = omni.usd.get_world_transform_matrix(prim) - # Convert to 3x3 rotation matrix using numpy + # 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 - + return self._R def quaternion(self, name, q=None): - """Returns the quaternion of the specified prim. + """Get quaternion orientation for the specified prim. + Parameters ---------- - - name: string - The name of the IsaacSim prim to retrieve the Jacobian for - q: float numpy.array, optional (Default: None) - The joint angles of the robot. + 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 + if name == "EE": + name = self.ee_link_name + prim = self._get_prim(name) - # Get 4x4 world transform matrix + # Get 4×4 world transform matrix and extract quaternion matrix = omni.usd.get_world_transform_matrix(prim) - quat = matrix.ExtractRotationQuat() + quat = matrix.ExtractRotationQuat() quat_np = np.array([quat.GetReal(), *quat.GetImaginary()]) - - return quat_np + return quat_np - def C(self): - """NOTE: The Coriolis and centrifugal effects are neglected atm, as this method is not called anywhere + 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)[self.env_idx] - return coriolis - + 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): - """Returns the transform matrix of the specified prim. + """Get full transform matrix for the specified prim. Parameters ---------- - name: string - The name of the prim to retrieve the transform matrix from. - q: float numpy.array, optional (Default: None) - The joint angles of the robot. - x: float numpy.array, optional (Default: None) + 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 """ - # TODO if ever required - raise NotImplementedError - + raise NotImplementedError("Full transform matrix not implemented yet") def Tx(self, name, q=None, x=None): - """Simplified version of T. - Returns the position without state changes of the specified prim. + """Get position of the specified prim. Parameters ---------- - name: string - The name of the prim to retrieve the position from. - q: float numpy.array, optional (Default: None) - The joint angles of the robot. - x: float numpy.array, optional (Default: None) + 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 + if name == "EE": + name = self.ee_link_name + prim = self._get_prim(name) if not prim.IsValid(): - raise RuntimeError(f"Invalid prim at path: {prim_path}") - + 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): - """Returns the inverse of the transform matrix of the specified prim. + """Get inverse transform matrix for the specified prim. Parameters ---------- - name: string - The name of the prim to retrieve the inverse from. - q: float numpy.array, optional (Default: None) - The joint angles of the robot. - x: float numpy.array, optional (Default: None) + 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 """ - # TODO if ever required - raise NotImplementedError - + 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) - - def _is_fixed_base(self): - num_dof = self.articulation_view.num_dof - jacobian_shape = self.articulation_view.get_jacobian_shape()[2] - return num_dof == jacobian_shape - + 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 index 9553f16f..76574bf2 100644 --- a/abr_control/interfaces/nv_isaacsim.py +++ b/abr_control/interfaces/nv_isaacsim.py @@ -1,48 +1,106 @@ +"""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 -simulation_app = SimulationApp({"headless": False}) + +# 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 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 IsaacSim. + """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 : class instance - contains all relevant information about the arm - such as: number of joints, number of links, mass information etc. - dt : float, optional (Default: 0.001) - simulation time step in seconds + 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 # time step + 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): - """ - joint_names: list, optional (Default: None) - list of joint names to send control signal to and get feedback from - if None, the joints in the kinematic tree connecting the end-effector - to the world are used - """ + """Connect to the Isaac Sim environment and set up the robot. - """All initial setup.""" + 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 = 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() @@ -51,48 +109,42 @@ def connect(self, joint_names=None): 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)) - + ) + 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() - - self.articulation_view = ArticulationView(prim_paths_expr=self.prim_path, name=self.robot_config.robot_type + "_view") + + # 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 EE if none exists - if (self.robot_config.has_EE is False): + + # 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) - + 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() - - # Get joint information - 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 - 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 + # Set up joint information + self._setup_joint_indices(joint_names) - 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) - - # Connect robot config with simulation data print("Connecting to robot config...") self.robot_config._connect( @@ -103,159 +155,279 @@ def connect(self, joint_names=None): self.joint_indices, self.prim_path, ) - # additional setup necessary for mobile robots (at least h1) + + # Additional setup for mobile robots if not self.robot_config._is_fixed_base: self.world.add_physics_callback("keep_standing", self.keep_standing) - # also works without, but is more reactive if max force is adapted - [self.set_max_force(name=joint_name, value=0.7) for joint_name in self.robot_config.controlled_dof] + # 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): - """Any socket closing etc that must be done to properly shut down""" - self.simulation_app.close() # close Isaac Sim + """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): - """Applies the torques u to the DOF specified in dof_indices. - - u : numpy.array - the forces to apply to the controlled DOF. + """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 + 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): - """Moves the arm to the specified joint angles + """Move the robot to specified joint angles. - q : numpy.array - the target joint angles [radians] + 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): - """Return a dictionary of information needed by the controller. - Returns the joint angles and joint velocities in [rad] and [rad/sec], - respectively + """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. - name : string - the prim_path of the prim - xyz : np.array - the [x,y,z] location of the target [meters] - """ + 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])) + 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 position of an object in the environment. + """Set the orientation of an object in the environment. - name : string - the prim_path of the prim - quat_wxyz : np.array - the [w,x,y,z] quaternion representation of the target orientation - """ + 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])) + 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) - - # method that humanoid does not fall over 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)) - - # Create a visual-only cube (no collision) - def create_target_prim(self, prim_path="/World/target", size = .1, color=np.array([0, 0, 1.0])): + 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) - + 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("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") + 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) + cube_prim.GetPrim().CreateAttribute( + "physics:collisionEnabled", Sdf.ValueTypeNames.Bool + ).Set(False) + return cube_prim - - + def create_random_pos(self): - 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 + """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 - # setting max_force via PhysX DriveAPI 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 - driveAPI = UsdPhysics.DriveAPI.Apply(prim, "angular") # Use "linear" for prismatic joints + # 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): - """Properly set gains for arm joints (DOFs 0-5) and finger joints if present""" - # Get current gains or set defaults - stiffness = np.ones(self.robot_config.N_ALL_DOF) * 100.0 # Default high stiffness - damping = np.ones(self.robot_config.N_ALL_DOF) * 10.0 # Default damping + """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) + self.articulation_view.set_gains(stiffness, damping) def add_virtual_ee_link(self, EE_parent_link, ee_name, offset): - """Add virtual end effector link as an Xform under the specified parent link""" + """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_osc_xyz.py b/examples/IsaacSim/force_osc_xyz.py index 0b074326..b8e8351c 100644 --- a/examples/IsaacSim/force_osc_xyz.py +++ b/examples/IsaacSim/force_osc_xyz.py @@ -15,7 +15,7 @@ if len(sys.argv) > 1: arm_model = sys.argv[1] else: - arm_model = "h1" # works with "h1_hands" / "h1" / jaco2 / ur5 + arm_model = "ur5" # works with "h1_hands" / "h1" / jaco2 / ur5 robot_config = arm(arm_model) dt = 0.005 # 200 Hz @@ -44,10 +44,8 @@ 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() @@ -73,9 +71,7 @@ dq=feedback["dq"], target=target, ) - interface.send_forces(u) - #interface.world.step(render=True) # calculate end-effector position ee_xyz = robot_config.Tx(robot_config.ee_link_name, q=feedback["q"]) @@ -90,7 +86,7 @@ count = 0 if count >= 50: print("Generating a new target") - interface.set_target_random() + interface.set_xyz("target", interface.create_random_pos()) count = 0 except: