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)