Code as follows:
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})
import carb
from omni.isaac.core import World
from shadow_hand.kinematicsolver import KinematicsSolver
from shadow_hand.RMPflow_Controller import RMPFlowController
from shadow_hand.follow_target import FollowTarget
my_world = World(stage_units_in_meters=1.0)
my_task = FollowTarget(name="follow_target_task")
my_world.add_task(my_task)
my_world.reset()
task_params = my_world.get_task("follow_target_task").get_params()
xarm_name = task_params["robot_name"]["value"]
target_name = task_params["target_name"]["value"]
xarm_shadow = my_world.scene.get_object(xarm_name)
my_controller = RMPFlowController(name= "target_follower_controller",robot_articulation=xarm_shadow)
rmp_flow = my_controller.get_motion_policy()
rmp_flow.set_ignore_state_updates(True)
rmp_flow.visualize_collision_spheres()
articulation_controller = xarm_shadow.get_articulation_controller()
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_controller.reset()
observations = my_world.get_observations()
actions = my_controller.forward(
target_end_effector_position=observations[target_name]["position"],
target_end_effector_orientation=observations[target_name]["orientation"],
)
articulation_controller.apply_action(actions)
simulation_app.close()
import omni.isaac.motion_generation as mg
from omni.isaac.core.articulations import Articulation
class RMPFlowController(mg.MotionPolicyController):
"""[summary]
Args:
name (str): [description]
robot_articulation (Articulation): [description]
physics_dt (float, optional): [description]. Defaults to 1.0/60.0.
"""
def __init__(self, name: str, robot_articulation: Articulation, physics_dt: float = 1.0 / 60.0) -> None:
robot_description_path = "/home/user/Downloads/assets/robot/shadow_hand_description/xarm_shadow/xarm_shadow_robot_descriptor.yaml"
urdf_path = "/home/user/Downloads/assets/robot/shadow_hand_description/xarm_shadow.urdf"
rmpflow_config_path = "/home/user/Downloads/assets/robot/shadow_hand_description/xarm_shadow/xarm_shadow_rmpflow.yaml"
end_effector_frame_name = "wrist"
maximum_substep_size = 0.00334
self.rmp_flow = mg.lula.motion_policies.RmpFlow(robot_description_path,urdf_path,rmpflow_config_path,end_effector_frame_name,maximum_substep_size)
self.articulation_rmp = mg.ArticulationMotionPolicy(robot_articulation, self.rmp_flow, physics_dt)
mg.MotionPolicyController.__init__(self, name=name, articulation_motion_policy=self.articulation_rmp)
(
self._default_position,
self._default_orientation,
) = self._articulation_motion_policy._robot_articulation.get_world_pose()
self._motion_policy.set_robot_base_pose(
robot_position=self._default_position, robot_orientation=self._default_orientation
)
return
def reset(self):
mg.MotionPolicyController.reset(self)
self._motion_policy.set_robot_base_pose(
robot_position=self._default_position, robot_orientation=self._default_orientation
)
from typing import List, Optional
import carb
import numpy as np
from omni.isaac.core.prims.rigid_prim import RigidPrim
from omni.isaac.core.robots.robot import Robot
from omni.isaac.core.utils.prims import get_prim_at_path
from omni.isaac.core.utils.stage import add_reference_to_stage, get_stage_units
class Xarm_Shadow(Robot):
def __init__(
self,
prim_path: str,
name: str = "franka_robot",
usd_path: Optional[str] = None,
position: Optional[np.ndarray] = [0,-0.8,0],
orientation: Optional[np.ndarray] = None,
):
prim = get_prim_at_path(prim_path)
if not prim.IsValid():
if usd_path:
add_reference_to_stage(usd_path=usd_path, prim_path=prim_path)
else:
carb.log_error("usd_path is not existing!")
super().__init__(
prim_path=prim_path + "/link_base", name=name, position=position, orientation=orientation, articulation_controller=None
)
self._end_effector_prim_path = prim_path+ "/wrist"
@property
def end_effector(self) -> RigidPrim:
"""[summary]
Returns:
RigidPrim: [description]
"""
return self._end_effector
def initialize(self, physics_sim_view=None) -> None:
"""[summary]"""
super().initialize(physics_sim_view)
self._end_effector = RigidPrim(prim_path=self._end_effector_prim_path, name=self.name + "_end_effector")
self._end_effector.initialize(physics_sim_view)
return
from typing import Optional
import numpy as np
import omni.isaac.core.tasks as tasks
from omni.isaac.core.utils.prims import is_prim_path_valid
from omni.isaac.core.utils.string import find_unique_string_name
from omni.isaac.core.robots.robot import Robot
from shadow_hand.xarm_shadow import Xarm_Shadow
class FollowTarget(tasks.FollowTarget):
def __init__(
self,
name: str = "xarm_shadow_follow_target",
target_prim_path: Optional[str] = None,
target_name: Optional[str] = None,
target_position: Optional[np.ndarray] = None,
target_orientation: Optional[np.ndarray] = None,
offset: Optional[np.ndarray] = None,
xarm_shadow_prim_path: Optional[str] = None,
xarm_shadow_robot_name: Optional[str] = None,
usd_path : str = "/home/user/Downloads/assets/robot/shadow_hand_description/xarm_shadow/xarm_shadow.usd"
):
tasks.FollowTarget.__init__(
self,
name=name,
target_prim_path=target_prim_path,
target_name=target_name,
target_position=target_position,
target_orientation=target_orientation,
offset=offset,
)
self._xarm_shadow_prim_path = xarm_shadow_prim_path
self._xarm_shadow_robot_name = xarm_shadow_robot_name
self._usd_path = usd_path
return
def set_robot(self) -> Robot:
if self._xarm_shadow_prim_path is None:
self._xarm_shadow_prim_path = find_unique_string_name(
initial_name="/World/xarm_shadow", is_unique_fn=lambda x: not is_prim_path_valid(x)
)
if self._xarm_shadow_robot_name is None:
self._xarm_shadow_robot_name = find_unique_string_name(
initial_name="xarm_shadow", is_unique_fn=lambda x: not self.scene.object_exists(x)
)
return Xarm_Shadow(prim_path=self._xarm_shadow_prim_path, name=self._xarm_shadow_robot_name, usd_path=self._usd_path)
shadow_hand_description.zip (7.0 MB)
These are some config files(URDF,robot_description,rmpflow). Can you help me take a look? or Can you offer me some instrction in detail?
