Skip to content

fix format #18

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
wilsonchenghy merged 1 commit into from
Jul 19, 2025
Merged

fix format #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
79 changes: 49 additions & 30 deletions autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,30 +1,30 @@
from isaaclab_assets import UR10_CFG
from isaaclab.utils.math import subtract_frame_transforms
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab.utils import configclass
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.markers import VisualizationMarkers
from isaaclab.managers import SceneEntityCfg
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
from isaaclab.assets import AssetBaseCfg
import isaaclab.sim as sim_utils
import torch
import argparse
from isaaclab.app import AppLauncher

parser = argparse.ArgumentParser(description="Robot Arm with Joint Space Control")
parser = argparse.ArgumentParser(
description="Robot Arm with Joint Space Control")
AppLauncher.add_app_launcher_args(parser)
args_cli = parser.parse_args()

app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

import torch

import isaaclab.sim as sim_utils
from isaaclab.assets import AssetBaseCfg
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
from isaaclab.managers import SceneEntityCfg
from isaaclab.markers import VisualizationMarkers
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab.utils.math import subtract_frame_transforms

##
# Pre-defined configs
##
from isaaclab_assets import UR10_CFG


@configclass
Expand All @@ -41,41 +41,55 @@ class TableTopSceneCfg(InteractiveSceneCfg):
# lights
dome_light = AssetBaseCfg(
prim_path="/World/Light",
spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
)
spawn=sim_utils.DomeLightCfg(
intensity=3000.0,
color=(
0.75,
0.75,
0.75)))

# mount
table = AssetBaseCfg(
prim_path="{ENV_REGEX_NS}/Table",
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0)
),
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd",
scale=(
2.0,
2.0,
2.0)),
)

# articulation
robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")


def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):

robot = scene["robot"]

# Create controller
diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls")
diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
diff_ik_cfg = DifferentialIKControllerCfg(
command_type="pose", use_relative_mode=False, ik_method="dls")
diff_ik_controller = DifferentialIKController(
diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)

# Markers
frame_marker_cfg = FRAME_MARKER_CFG.copy()
frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current"))
ee_marker = VisualizationMarkers(
frame_marker_cfg.replace(
prim_path="/Visuals/ee_current"))

# Specify robot-specific parameters
robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
robot_entity_cfg = SceneEntityCfg(
"robot", joint_names=[".*"], body_names=["ee_link"])

# Resolving the scene entities
robot_entity_cfg.resolve(scene)

# Obtain the frame index of the end-effector ; For a fixed base robot, the frame index is one less than the body index. This is because ; the root body is not included in the returned Jacobians.
# Obtain the frame index of the end-effector ; For a fixed base robot, the
# frame index is one less than the body index. This is because ; the root
# body is not included in the returned Jacobians.
if robot.is_fixed_base:
ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
else:
Expand All @@ -99,20 +113,24 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
[1.0, -1.712, 1.712, 0.0, 0.0, 0.0],
[0.0, 1.712, 1.712, 0.0, 0.0, 0.0],
]
joint_position = torch.tensor(joint_position_list[joint_position_index], device=sim.device)
joint_position = torch.tensor(
joint_position_list[joint_position_index],
device=sim.device)
joint_vel = robot.data.default_joint_vel.clone()

if joint_position_index >= len(joint_position_list) - 1:
joint_position_index = 0
else:
joint_position_index += 1

robot.reset()

joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone()
joint_pos_des = joint_position.unsqueeze(
0)[:, robot_entity_cfg.joint_ids].clone()

# apply actions (Smooth movement)
robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
robot.set_joint_position_target(
joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
scene.write_data_to_sim()

# perform step
Expand All @@ -125,7 +143,8 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
scene.update(sim_dt)

# obtain quantities from simulation
ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
ee_pose_w = robot.data.body_state_w[:,
robot_entity_cfg.body_ids[0], 0:7]
# update marker positions
ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])

Expand Down Expand Up @@ -153,4 +172,4 @@ def main():

if __name__ == "__main__":
main()
simulation_app.close()
simulation_app.close()
116 changes: 76 additions & 40 deletions autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,31 +1,35 @@
from isaaclab_assets import UR10_CFG, FRANKA_PANDA_HIGH_PD_CFG
from isaaclab.utils.math import subtract_frame_transforms
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab.utils import configclass
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.markers import VisualizationMarkers
from isaaclab.managers import SceneEntityCfg
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
from isaaclab.assets import AssetBaseCfg
import isaaclab.sim as sim_utils
import torch
import argparse
from isaaclab.app import AppLauncher

parser = argparse.ArgumentParser(description="Robot Arm with Task Space IK Control")
parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.")
parser = argparse.ArgumentParser(
description="Robot Arm with Task Space IK Control")
parser.add_argument(
"--robot",
type=str,
default="franka_panda",
help="Name of the robot.")
AppLauncher.add_app_launcher_args(parser)
args_cli = parser.parse_args()

app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

import torch

import isaaclab.sim as sim_utils
from isaaclab.assets import AssetBaseCfg
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
from isaaclab.managers import SceneEntityCfg
from isaaclab.markers import VisualizationMarkers
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab.utils.math import subtract_frame_transforms

##
# Pre-defined configs
##
from isaaclab_assets import UR10_CFG, FRANKA_PANDA_HIGH_PD_CFG


@configclass
Expand All @@ -42,15 +46,22 @@ class TableTopSceneCfg(InteractiveSceneCfg):
# lights
dome_light = AssetBaseCfg(
prim_path="/World/Light",
spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
)
spawn=sim_utils.DomeLightCfg(
intensity=3000.0,
color=(
0.75,
0.75,
0.75)))

# mount
table = AssetBaseCfg(
prim_path="{ENV_REGEX_NS}/Table",
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0)
),
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd",
scale=(
2.0,
2.0,
2.0)),
)

# Cube
Expand All @@ -64,41 +75,56 @@ class TableTopSceneCfg(InteractiveSceneCfg):

# articulation
if args_cli.robot == "franka_panda":
robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
robot = FRANKA_PANDA_HIGH_PD_CFG.replace(
prim_path="{ENV_REGEX_NS}/Robot")
elif args_cli.robot == "ur10":
robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
else:
raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
raise ValueError(
f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
# robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")


def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):

robot = scene["robot"]

# Create controller
diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls")
diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
diff_ik_cfg = DifferentialIKControllerCfg(
command_type="pose", use_relative_mode=False, ik_method="dls")
diff_ik_controller = DifferentialIKController(
diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)

# Markers
frame_marker_cfg = FRAME_MARKER_CFG.copy()
frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current"))
goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal"))
ee_marker = VisualizationMarkers(
frame_marker_cfg.replace(
prim_path="/Visuals/ee_current"))
goal_marker = VisualizationMarkers(
frame_marker_cfg.replace(
prim_path="/Visuals/ee_goal"))

# Specify robot-specific parameters
if args_cli.robot == "franka_panda":
robot_entity_cfg = SceneEntityCfg("robot", joint_names=["panda_joint.*"], body_names=["panda_hand"])
robot_entity_cfg = SceneEntityCfg(
"robot",
joint_names=["panda_joint.*"],
body_names=["panda_hand"])
elif args_cli.robot == "ur10":
robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
robot_entity_cfg = SceneEntityCfg(
"robot", joint_names=[".*"], body_names=["ee_link"])
else:
raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
raise ValueError(
f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
# robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])

# Resolving the scene entities
robot_entity_cfg.resolve(scene)

# Obtain the frame index of the end-effector ; For a fixed base robot, the frame index is one less than the body index. This is because ; the root body is not included in the returned Jacobians.
# Obtain the frame index of the end-effector ; For a fixed base robot, the
# frame index is one less than the body index. This is because ; the root
# body is not included in the returned Jacobians.
if robot.is_fixed_base:
ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
else:
Expand All @@ -117,21 +143,27 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
joint_position_list = [
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
]
joint_position = torch.tensor(joint_position_list[joint_position_index], device=sim.device)
joint_position = torch.tensor(
joint_position_list[joint_position_index],
device=sim.device)
joint_vel = robot.data.default_joint_vel.clone()
joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone()
joint_pos_des = joint_position.unsqueeze(
0)[:, robot_entity_cfg.joint_ids].clone()
robot.write_joint_state_to_sim(joint_pos_des, joint_vel)

while simulation_app.is_running():
# Get cube/target_point coordinates
position, quaternion = scene["cube"].get_world_poses()
ik_commands = torch.cat([position, quaternion], dim=1) # Quaternion is in (w, x, y, z)
# Quaternion is in (w, x, y, z)
ik_commands = torch.cat([position, quaternion], dim=1)

diff_ik_controller.set_command(ik_commands)

# obtain quantities from simulation
jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
jacobian = robot.root_physx_view.get_jacobians()[
:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
ee_pose_w = robot.data.body_state_w[:,
robot_entity_cfg.body_ids[0], 0:7]
root_pose_w = robot.data.root_state_w[:, 0:7]
joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]

Expand All @@ -141,10 +173,12 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
)

# compute the joint commands
joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
joint_pos_des = diff_ik_controller.compute(
ee_pos_b, ee_quat_b, jacobian, joint_pos)

# apply actions (Smooth movement)
robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
robot.set_joint_position_target(
joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
scene.write_data_to_sim()

# perform step
Expand All @@ -154,11 +188,13 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
scene.update(sim_dt)

# obtain quantities from simulation
ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
ee_pose_w = robot.data.body_state_w[:,
robot_entity_cfg.body_ids[0], 0:7]

# update marker positions
ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
goal_marker.visualize(ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7])
goal_marker.visualize(
ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7])


def main():
Expand All @@ -184,4 +220,4 @@ def main():

if __name__ == "__main__":
main()
simulation_app.close()
simulation_app.close()
Loading
Loading