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
implementation method quaternion, Tx, R
  • Loading branch information
LeaStef committed Aug 21, 2025
commit 0d70a58ff01e7505993281b33dfba55a0d8c9265
174 changes: 120 additions & 54 deletions abr_control/arms/isaacsim_config.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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
----------
Expand All @@ -293,39 +296,42 @@ 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")

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),
)
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)
Expand Down Expand Up @@ -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):
Expand All @@ -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
Expand Down Expand Up @@ -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)

Expand All @@ -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
Expand Down Expand Up @@ -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

Loading