Changed Robot orientation while spawning from USD in Isaac Sim

I am spawning this custom robot using the given script below.

from omni.isaac.kit import SimulationApp




simulation_app = SimulationApp({"headless": False})

from omni.isaac.core import World
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.robots import Robot
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.utils.transformations import tf_matrix_from_pose,pose_from_tf_matrix
from husky_ur3_robot_2 import Husky_UR3_Robot
from husky_wheel_controller import HuskyController
from omni.isaac.core.utils.stage import add_reference_to_stage, get_stage_units
from spline_utils import generate_joint_angle_trajectory_space_quinctic
import numpy as np



'''
Kp = 10*np.array([5.729578e+08, 5.729578e+08, 5.729578e+08, 5.729578e+08, 5.729578e+08, 5.729578e+08, 0, 0, 0, 0])
Kd = np.array([5729578., 5729578., 5729578., 5729578., 5729578., 5729578., 5.729578e+08, 5.729578e+08, 5.729578e+08, 5.729578e+08])
'''
final_posture = np.array([0,0,0,0,0,0])

my_world = World(stage_units_in_meters=1.0)



my_robot = my_world.scene.add(
      Husky_UR3_Robot(prim_path="/Robot", 
                      usd_path="./husky_ur3_robotiq2f85.usd",
                      name = "Robot",
                      end_effector_name = "ur_arm_flange",
                      position = np.array([0,0,0]), 
                      orientation=np.array([1, 0, 0, 0])
                  )
)

# "/home/bikram/husky_ur3_robotiq2f85.usd"


# my_robot.get_articulation_controller().set_gains(kps = Kp, kds = Kd)

my_world.reset()

my_robot.initialize()
my_world.scene.add_default_ground_plane()

# my_robot.get_articulation_controller().set_gains(kps = Kp, kds = Kd)


while simulation_app.is_running():
      my_world.step(render = True)
      if my_world.is_playing():
            if my_world.current_time_step_index == 0:
                  my_world.reset()
      

      my_robot.set_joint_positions(positions = final_posture, joint_indices = my_robot.ur_arm_dof_indices)
      print(my_robot.base_link.get_world_pose())

            




simulation_app.close()

In my usd

./husky_ur3_robotiq2f85.usd

the robot position is at (0,0,0) and orientation is (1,0,0,0 ) but the robot is changing its orientation while it is being spawned. Can anyone help me out here ? This is the robot class.

# Copyright (c) 2021, NVIDIA CORPORATION.  All rights reserved.
#
# NVIDIA CORPORATION and its licensors retain all intellectual property
# and proprietary rights in and to this software, related documentation
# and any modifications thereto.  Any use, reproduction, disclosure or
# distribution of this software and related documentation without an express
# license agreement from NVIDIA CORPORATION is strictly prohibited.
#





import carb
from typing import Optional
import numpy as np


from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.robots.robot import Robot
from omni.isaac.core.utils.prims import get_prim_at_path, define_prim
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.prims.rigid_prim import RigidPrim


class Husky_UR3_Robot(Robot):
    def __init__(self,
                 prim_path : str,
                 usd_path: str,
                 name : Optional[str],
                 end_effector_name: str,
                 position: Optional[np.ndarray] = None,
                 orientation: Optional[np.ndarray] = None,          
    ) -> None:
        add_reference_to_stage(usd_path, prim_path)
        Robot.__init__(self,prim_path=prim_path, name=name, position=position, orientation=orientation, articulation_controller=None)
        self._husky_base_joint_names = ["front_left_wheel",
                                      "front_right_wheel",
                                      "rear_left_wheel",
                                      "rear_right_wheel"]
        self._husky_dof_idxs = []
        self._ur_joint_names = ["ur_arm_shoulder_pan_joint",
                                    "ur_arm_shoulder_lift_joint",
                                    "ur_arm_elbow_joint",
                                    "ur_arm_wrist_1_joint",
                                    "ur_arm_wrist_2_joint",
                                    "ur_arm_wrist_3_joint"]

        self._ur_arm_dof_idxs = []
        self._num_dof = len(self._husky_base_joint_names) + len(self._ur_joint_names)
        self._end_effector_prim_path = prim_path + '/' + end_effector_name
        self._base_link_prim_path = prim_path + '/' + 'base_link'
        self._base_link = None
        





    @property
    def husky_wheel_dof_indices(self):
        return self._husky_dof_idxs
    
    @property
    def ur_arm_dof_indices(self):
        return self._ur_arm_dof_idxs
    








    def apply_wheel_actions(self, actions : ArticulationAction) -> None:

        actions_length = actions.get_length()
        if actions_length is not None and actions_length != self._num_wheel_dof:
            raise Exception("ArticulationAction passed should be the same length as the number of wheels")
        joint_actions = ArticulationAction()

        if actions.joint_positions is not None:
            joint_actions.joint_positions = np.zeros(self._num_dof)
            for i in range(self._num_wheel_dof):
                joint_actions.joint_positions[self._husky_dof_idxs[i]] = actions.joint_postions[i]

        if actions.joint_velocities is not None:
            joint_actions.joint_velocities = np.zeros(self._num_dof)
            for i in range(self._num_wheel_dof):
                joint_actions.joint_velocities[self._husky_dof_idxs[i]] = actions.joint_velocities[i]
        
        if actions.joint_efforts is not None:
            joint_actions.joint_efforts = np.zeros(self._num_dof)
            for i in range(self._num_wheel_dof):
                joint_actions.joint_efforts[self._husky_dof_idxs[i]] = actions.joint_efforts[i]
        self.apply_action(control_actions = joint_actions)
        return
    



    def apply_arm_actions(self, actions:ArticulationAction) -> None:
        actions_length = actions.get_length()
        if actions_length is not None and actions_length != self._num_dof_ur:
            raise Exception("ArticulationAction passed should be the same length as the number of UR joints")
        joint_actions = ArticulationAction()
        
        if actions.joint_positions is not None:
            joint_actions.joint_positions = np.zeros(self._num_dof)
            for i in range(self._num_dof_ur):
                joint_actions.joint_positions[self._ur_arm_dof_idxs[i]] = actions.joint_positions[i]

        if actions.joint_velocities is not None:
            joint_actions.joint_velocities = np.zeros(self._num_dof)
            for i in range(self._num_dof_ur):
                joint_actions.joint_velocities[self._ur_arm_dof_idxs[i]] = actions.joint_velocities[i]


        if actions.joint_efforts is not None:
            joint_actions.joint_efforts = np.zeros(self._num_dof)
            for i in range(self._num_dof_ur):
                joint_actions.joint_efforts[self._ur_arm_dof_idxs[i]] = actions.joint_efforts[i]

        self.apply_action(control_actions = joint_actions)
        return
    





    def set_ur_arm_joint_positions(self, positions) -> None:
        full_dof_positions = [None] * self._num_dof
        for i in range(self._num_dof_ur):
            full_dof_positions[self._ur_arm_dof_idxs[i]] = positions[i]
        self.set_joint_positions(positions = np.array(full_dof_positions))







    def initialize(self, physics_sim_view=None) -> None:


        super().initialize(physics_sim_view=physics_sim_view)

        if self._ur_joint_names is not None:
            self._ur_arm_dof_idxs = [
                self.get_dof_index(self._ur_joint_names[i]) for i in range(len(self._ur_joint_names))
            ]
        elif self._ur_arm_dof_idxs is None:
            carb.log_error("need to have either arm joint names or joint indices")
        if self._husky_base_joint_names is not None:
            self._husky_dof_idxs = [
                self.get_dof_index(self._husky_base_joint_names[i]) for i in range(len(self._husky_base_joint_names))
            ]
        elif self._husky_dof_idxs is None:
            carb.log_error("need to have either wheel names or wheel indices")
        self._end_effector = RigidPrim(prim_path = self._end_effector_prim_path, name = self.name + "_end_effector")
        self._end_effector.initialize(physics_sim_view)

        self._base_link = RigidPrim(prim_path = self._base_link_prim_path, name = self.name + "_base_link")
        self._base_link.initialize(physics_sim_view)
        

        self._num_dof_ur = len(self._ur_arm_dof_idxs)
        self._num_wheel_dof = len(self._husky_dof_idxs)


        return




    def post_reset(self) -> None:

        Robot.post_reset(self)
        #if self._articulation_controller is not None:
        self._articulation_controller.switch_control_mode(mode="position")
        return



    def set_gains(self,kps, kds):
        self.kps = kps
        self.kds = kds
        #this function is incomplete. It needs some attention....



    @property
    def end_effector(self):
        return self._end_effector
    
    @property
    def base_link(self):
        return self._base_link
    

this is the orientation of the base_link of the robot.

array([-0.7039336 , 0.7039338 , -0.06690642, 0.06692077]

whereas it is supposed to be

array([1,0,0,0])

in quaternion.

The usd and the urdf files are attached below.

usd_urdf.zip (3.8 MB)

Hi,
Is this happening also in latest IsaacSim 2023.1? This does ring a bell and I think it should be fixed right @kellyg ?
Regards,
Ales

This should be fixed in Isaac Sim 2023.1.0. Please try with the latest isaac sim release.

Okay @kellyg, I will try with the Isaac Sim 2023.1.0.

I should let you know when I am using the robot usd without the gripper (which is attached to the robot arm as shown in the image) the robot is spawning at the correct orientation but when I am using the robot with the gripper attached to the arm this issue is coming up. I have two different sets of USD generated from 2 URDFs.

  1. with gripper ( facing this issue )
  2. without gripper (spawning correctly )

Thanks and regards,
Tribikram

Dear @kellyg, @AlesBorovicka,
Thank you for your support. I installed the 2023.1.0 hotfix release of Isaac Sim and the spawning issue is no more happening.

Thanks and regards,
Tribikram.

1 Like

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.