Hello everyone,
I wanted to move a UR3 arm attached to a mobile robot base from one set of joint positions to another set of joint positions. But the arm movement is happening in a weird way. Can anyone check the demo given in the video here. Video here !
The script running is given here :
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_whole_body_handler import Husky_UR3_Whole_Body_Handler
from tiago_dual_WB_handler import TiagoDualWBHandler
from husky_ur3_robot_2 import Husky_UR3_Robot
from husky_wheel_controller import HuskyController
import numpy as np
# husky_ur_handler = Husky_UR3_Whole_Body_Handler()
# robot = husky_ur_handler.get_robot()
my_world = World(stage_units_in_meters=1.0)
initial_posture = np.array([-3.4930844,-1.7593288, -1.2764046, -1.676637, 1.5707291, -1.9221054])
vel = np.array([0.5,-0.5,-0.5,-0.5,0.5,0.5])
final_posture = np.array([1.0528518, -1.8536792, -0.02006058, -0.7816168, 1.2877516, 1.8681929])
delta = (final_posture-initial_posture)/100
my_robot = my_world.scene.add(
Husky_UR3_Robot(prim_path="/World/Husky_UR3",
usd_path="/home/bikram/husky_ur3.usd",
name = "Husky_UR3")
)
my_world.scene.add_default_ground_plane()
#my_controller = HuskyController(name="simple_control", wheel_radius=0.03, wheel_base=0.1125)
my_world.reset()
my_robot.initialize()
while simulation_app.is_running():
if my_world.is_playing():
if my_world.current_time_step_index == 0: # in this line if we add "simulation_context.current_time_step_index == 0:" it would still work because Simulation Context() has the same function as World() has...
my_world.reset()
#my_controller.reset()
my_robot.set_joint_positions(positions = initial_posture, joint_indices = my_robot.ur_arm_dof_indices)
for i in range(10000):
my_world.step(render=True)
my_robot.apply_arm_actions(ArticulationAction(joint_positions = initial_posture + i*delta, joint_velocities = vel, joint_efforts = vel))
print("ok")
simulation_app.close()
The Robot class that I used is :
'''
This is another trial of writing the Husky_UR3_robot class.
'''
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
class Husky_UR3_Robot(Robot):
def __init__(self,
prim_path : str,
usd_path: str,
name : Optional[str],
position: Optional[np.ndarray] = None,
orientation: Optional[np.ndarray] = None,
) -> None:
# prim = get_prim_at_path(prim_path)
# if not prim.IsValid():
# prim = define_prim(prim_path, "Xform")
# prim.GetReferences().AddReference(usd_path)
add_reference_to_stage(usd_path, prim_path)
super().__init__(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)
@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 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._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
Please can anyone tell me, how can I make a good movement of the robot arm.