Skip to content
Open
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
42 commits
Select commit Hold shift + click to select a range
3672d6e
added basic interface for isaacsim
LeaStef Apr 22, 2025
0a7b3d6
create world with dt
LeaStef Apr 24, 2025
73d07bb
implemented method connect in interface for isaacsim
LeaStef Apr 25, 2025
f34bf78
implemented method get_feedback to provide joint angles and joint ang…
LeaStef Apr 25, 2025
e2fe8ac
implemented sned_target_angles
LeaStef Apr 28, 2025
6578760
add method to provide pose
LeaStef May 1, 2025
b953b7f
example script to make arm follow cube
LeaStef May 1, 2025
a2ca838
added methods getOreintation() and set_xyz to isaac interface
LeaStef May 6, 2025
0362a48
switched urdf import to xml import
LeaStef May 13, 2025
6e1e766
interface adapted to h1, locked robote pelvis orientation so it does …
LeaStef May 16, 2025
8149846
fixed getfeedback to work only for arm DOFs
LeaStef May 20, 2025
4e70b1a
added config file for H1
LeaStef May 21, 2025
25ab5ca
added matrix for segment lengths of arm
LeaStef May 23, 2025
59d3925
added articulation view
LeaStef May 23, 2025
fb9445b
working example with floating robot
LeaStef Jun 17, 2025
0d70a58
implementation method quaternion, Tx, R
LeaStef Jun 20, 2025
adfa03d
floating control now works for ur5 and jaco2
LeaStef Jun 24, 2025
9e412b8
adapted tu num of controlled DOF
LeaStef Jun 24, 2025
bc7a01e
generalized send u
LeaStef Jun 24, 2025
361b35c
new implementation of the method connect, following the structure of …
LeaStef Jun 25, 2025
00471ef
Create a visual-only cube (no collision)
LeaStef Jun 27, 2025
96b2151
fixed gains of arm joints, which were to stiff for force control
LeaStef Jul 7, 2025
04d54a8
fixed def M
LeaStef Jul 7, 2025
823aa6f
Example with force_osc working
LeaStef Jul 8, 2025
c2593a2
added example linear path gaussian velocity
LeaStef Jul 8, 2025
1da42b9
adaptations for no EE
LeaStef Jul 10, 2025
fc6bd28
force control working for UR5 and jaco2
LeaStef Jul 10, 2025
f7bbb1e
adapted mass matrix to extract the correct, controlled joints
LeaStef Jul 14, 2025
4ec172c
added torso joint and used standing method in setup for h1
LeaStef Jul 15, 2025
53067fb
integrated robot H1 with hands
LeaStef Jul 16, 2025
62cdbec
fixed method set_gains_force_control so it adresses the right joints
LeaStef Jul 16, 2025
ff9299a
adapted method send_forces to work with abritary list of joint indices
LeaStef Jul 16, 2025
b1cb9ea
fixed helper methods for arbitrary indices
LeaStef Jul 17, 2025
15dca0a
removed coriolis forces from function for gravity compensation
LeaStef Jul 30, 2025
4d51236
fixed indexing of jacobian due to non-fixed base
LeaStef Aug 6, 2025
0e1a270
fixed method M
LeaStef Aug 7, 2025
24db8ef
force control OSC working for H1 without gravity
LeaStef Aug 11, 2025
090944b
fixed offset for virtual EE
LeaStef Aug 14, 2025
e6df5fe
fixed calc of body_link in J() and removed obsolete h1 setup
LeaStef Aug 15, 2025
429213d
works with gravity now on all 4 robots (weird fix for h1)
LeaStef Aug 19, 2025
be900e3
example force_osc_xyz_linear_path_gaussian_velocity works with ur5 an…
LeaStef Aug 20, 2025
c503fbe
added documentation
LeaStef Aug 21, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
switched urdf import to xml import
  • Loading branch information
LeaStef committed Aug 21, 2025
commit 0362a48f6c3a70c4a17cb8ce6e37d4641f67ecf3
82 changes: 51 additions & 31 deletions abr_control/interfaces/nv_isaacsim.py
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -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

Expand All @@ -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.
Expand All @@ -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)




Expand All @@ -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)
Expand All @@ -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

Expand All @@ -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):
Expand Down
182 changes: 182 additions & 0 deletions abr_control/interfaces/nv_isaacsim_usd_jaco.py
Original file line number Diff line number Diff line change
@@ -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
37 changes: 37 additions & 0 deletions examples/isaacsim/jaco_2.py
Original file line number Diff line number Diff line change
@@ -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...")
Loading