diff --git a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py index bc81a7b..06bc1b8 100644 --- a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py +++ b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py @@ -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 @@ -41,22 +41,15 @@ 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 @@ -64,32 +57,25 @@ class TableTopSceneCfg(InteractiveSceneCfg): 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: @@ -113,24 +99,20 @@ 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 @@ -143,8 +125,7 @@ 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]) @@ -172,4 +153,4 @@ def main(): if __name__ == "__main__": main() - simulation_app.close() + simulation_app.close() \ No newline at end of file diff --git a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py index 29bb172..79161cd 100644 --- a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py +++ b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py @@ -1,35 +1,31 @@ -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 @@ -46,22 +42,15 @@ 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 @@ -75,56 +64,41 @@ 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: @@ -143,27 +117,21 @@ 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() - # Quaternion is in (w, x, y, z) - ik_commands = torch.cat([position, quaternion], dim=1) + ik_commands = torch.cat([position, quaternion], dim=1) # Quaternion is in (w, x, y, z) 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] @@ -173,12 +141,10 @@ 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 @@ -188,13 +154,11 @@ 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(): @@ -220,4 +184,4 @@ def main(): if __name__ == "__main__": main() - simulation_app.close() + simulation_app.close() \ No newline at end of file diff --git a/autonomy/simulation/Wato_additional_scripts/shadow_hand/shadow_hand_joint_space_test.py b/autonomy/simulation/Wato_additional_scripts/shadow_hand/shadow_hand_joint_space_test.py index bd107d5..200207c 100644 --- a/autonomy/simulation/Wato_additional_scripts/shadow_hand/shadow_hand_joint_space_test.py +++ b/autonomy/simulation/Wato_additional_scripts/shadow_hand/shadow_hand_joint_space_test.py @@ -1,26 +1,27 @@ -from isaaclab_assets import SHADOW_HAND_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 + +from isaaclab_assets import SHADOW_HAND_CFG # Shadow hand has 24 DOF - found from print(robot_entity_cfg.joint_ids) # Print joint position - robot.data.default_joint_pos @@ -40,12 +41,8 @@ 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( @@ -60,19 +57,16 @@ class TableTopSceneCfg(InteractiveSceneCfg): def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): - + robot = scene["robot"] # Specify robot-specific parameters - robot_entity_cfg = SceneEntityCfg( - "robot", joint_names=[".*"], body_names=[".*"]) + robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=[".*"]) # 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: @@ -89,95 +83,24 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): count = 0 # cycle between joint state - joint_position_list = [[0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0.], - [1., - 1., - 1., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0.], - [0., - 0., - 1., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 0., - 1., - 1., - 0., - 0., - 0., - 0., - 0.]] - joint_position = torch.tensor( - joint_position_list[joint_position_index], - device=sim.device) + joint_position_list = [ + [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], + [1., 1., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], + [0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 1., 0., 0., 0., 0., 0.] + ] + joint_position = torch.tensor(joint_position_list[joint_position_index], device=sim.device) 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 @@ -213,4 +136,4 @@ def main(): if __name__ == "__main__": main() - simulation_app.close() + simulation_app.close() \ No newline at end of file