1
2

cd ~/isaacsim
./isaac-sim.sh

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

import sys
import numpy as np
import argparse
from typing import Optional


from isaacsim import SimulationApp
simulation_app = SimulationApp({"headless": False})  
from isaacsim.core.api import World
from isaacsim.core.prims import Articulation
from isaacsim.core.utils.stage import add_reference_to_stage, get_stage_units
from isaacsim.core.utils.viewports import set_camera_view
from isaacsim.storage.native import get_assets_root_path
from isaacsim.robot_motion.motion_generation import ArticulationKinematicsSolver, LulaKinematicsSolver
from isaacsim.core.utils.numpy.rotations import euler_angles_to_quats

from isaacsim.core.utils.types import ArticulationAction
from isaacsim.robot.manipulators.manipulators import SingleManipulator
import isaacsim.core.api.tasks as tasks

class KinematicsSolver(ArticulationKinematicsSolver):
    def __init__(self, robot_articulation: Articulation, end_effector_frame_name: Optional[str] = None) -> None:
        
        self._kinematics = LulaKinematicsSolver(robot_description_path="asset/isaac_sim/ur5e_hand/ur5e_hand.yaml",
                                                urdf_path="asset/urdf_ros2/ur5e_hand/urdf/ur5e_hand.urdf")
        if end_effector_frame_name is None:
            end_effector_frame_name = "wrist_3_link"
        ArticulationKinematicsSolver.__init__(self, robot_articulation, self._kinematics, end_effector_frame_name)
        return


world = World(stage_units_in_meters=1.0)
world.initialize_physics()
world.scene.add_default_ground_plane()
set_camera_view(
    eye=[2.0, 1.5, 1.8], target=[0.00, 0.00, 0.81], camera_prim_path="/OmniverseKit_Persp"
)


add_reference_to_stage(usd_path="asset/isaac_sim/ur5e_hand/ur5e_hand.usd", prim_path="/World/ur5e_hand")
ur5e_hand = SingleManipulator(
    prim_path="/World/ur5e_hand", 
    name="ur5e_hand", 
    end_effector_prim_path="/World/ur5e_hand/wrist_3_link")
init_joint_positions=np.array([0.0, -np.pi/2, np.pi/2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
ur5e_hand.set_joints_default_state([init_joint_positions])
ur5e_hand.initialize()
ur5e_hand.post_reset()
world.scene.add(ur5e_hand)


controller = KinematicsSolver(ur5e_hand)
articulation_controller = ur5e_hand.get_articulation_controller()


while simulation_app.is_running():
    world.step(render=True)
    if world.is_playing():
        if world.current_time_step_index == 0:
            world.reset()
        actions, succ = controller.compute_inverse_kinematics(
            target_position=np.array([0.5, 0, 0.5]),
            target_orientation=euler_angles_to_quats(np.array([0, 0, 0])),
        )
        if succ:
            articulation_controller.apply_action(actions)
        else:
            print("IK did not converge to a solution.  No action is being taken.")
simulation_app.close()