使用任务空间控制器

使用任务空间控制器#

在以前的教程中，我们使用关节空间控制器来控制机器人。然而，在许多情况下，使用任务空间控制器更直观。例如，如果我们想对机器人进行远程操作，更容易指定期望的末端执行器姿势，而不是期望的关节位置。

在本教程中，我们将学习如何使用任务空间控制器来控制机器人。我们将使用 controllers.DifferentialIKController 类来跟踪期望的末端执行器姿势指令。

代码#

该教程对应于 source/standalone/tutorials/05_controllers 目录中的 run_diff_ik.py 脚本。

run_diff_ik.py 的代码
  1# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
  2# All rights reserved.
  3#
  4# SPDX-License-Identifier: BSD-3-Clause
  5
  6"""
  7This script demonstrates how to use the differential inverse kinematics controller with the simulator.
  8
  9The differential IK controller can be configured in different modes. It uses the Jacobians computed by
 10PhysX. This helps perform parallelized computation of the inverse kinematics.
 11
 12.. code-block:: bash
 13
 14    # Usage
 15    ./isaaclab.sh -p source/standalone/tutorials/05_controllers/ik_control.py
 16
 17"""
 18
 19"""Launch Isaac Sim Simulator first."""
 20
 21import argparse
 22
 23from omni.isaac.lab.app import AppLauncher
 24
 25# add argparse arguments
 26parser = argparse.ArgumentParser(description="Tutorial on using the differential IK controller.")
 27parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.")
 28parser.add_argument("--num_envs", type=int, default=128, help="Number of environments to spawn.")
 29# append AppLauncher cli args
 30AppLauncher.add_app_launcher_args(parser)
 31# parse the arguments
 32args_cli = parser.parse_args()
 33
 34# launch omniverse app
 35app_launcher = AppLauncher(args_cli)
 36simulation_app = app_launcher.app
 37
 38"""Rest everything follows."""
 39
 40import torch
 41
 42import omni.isaac.lab.sim as sim_utils
 43from omni.isaac.lab.assets import AssetBaseCfg
 44from omni.isaac.lab.controllers import DifferentialIKController, DifferentialIKControllerCfg
 45from omni.isaac.lab.managers import SceneEntityCfg
 46from omni.isaac.lab.markers import VisualizationMarkers
 47from omni.isaac.lab.markers.config import FRAME_MARKER_CFG
 48from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg
 49from omni.isaac.lab.utils import configclass
 50from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR
 51from omni.isaac.lab.utils.math import subtract_frame_transforms
 52
 53##
 54# Pre-defined configs
 55##
 56from omni.isaac.lab_assets import FRANKA_PANDA_HIGH_PD_CFG, UR10_CFG  # isort:skip
 57
 58
 59@configclass
 60class TableTopSceneCfg(InteractiveSceneCfg):
 61    """Configuration for a cart-pole scene."""
 62
 63    # ground plane
 64    ground = AssetBaseCfg(
 65        prim_path="/World/defaultGroundPlane",
 66        spawn=sim_utils.GroundPlaneCfg(),
 67        init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)),
 68    )
 69
 70    # lights
 71    dome_light = AssetBaseCfg(
 72        prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
 73    )
 74
 75    # mount
 76    table = AssetBaseCfg(
 77        prim_path="{ENV_REGEX_NS}/Table",
 78        spawn=sim_utils.UsdFileCfg(
 79            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0)
 80        ),
 81    )
 82
 83    # articulation
 84    if args_cli.robot == "franka_panda":
 85        robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
 86    elif args_cli.robot == "ur10":
 87        robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
 88    else:
 89        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
 90
 91
 92def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
 93    """Runs the simulation loop."""
 94    # Extract scene entities
 95    # note: we only do this here for readability.
 96    robot = scene["robot"]
 97
 98    # Create controller
 99    diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls")
100    diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
101
102    # Markers
103    frame_marker_cfg = FRAME_MARKER_CFG.copy()
104    frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
105    ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current"))
106    goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal"))
107
108    # Define goals for the arm
109    ee_goals = [
110        [0.5, 0.5, 0.7, 0.707, 0, 0.707, 0],
111        [0.5, -0.4, 0.6, 0.707, 0.707, 0.0, 0.0],
112        [0.5, 0, 0.5, 0.0, 1.0, 0.0, 0.0],
113    ]
114    ee_goals = torch.tensor(ee_goals, device=sim.device)
115    # Track the given command
116    current_goal_idx = 0
117    # Create buffers to store actions
118    ik_commands = torch.zeros(scene.num_envs, diff_ik_controller.action_dim, device=robot.device)
119    ik_commands[:] = ee_goals[current_goal_idx]
120
121    # Specify robot-specific parameters
122    if args_cli.robot == "franka_panda":
123        robot_entity_cfg = SceneEntityCfg("robot", joint_names=["panda_joint.*"], body_names=["panda_hand"])
124    elif args_cli.robot == "ur10":
125        robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
126    else:
127        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
128    # Resolving the scene entities
129    robot_entity_cfg.resolve(scene)
130    # Obtain the frame index of the end-effector
131    # For a fixed base robot, the frame index is one less than the body index. This is because
132    # the root body is not included in the returned Jacobians.
133    if robot.is_fixed_base:
134        ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
135    else:
136        ee_jacobi_idx = robot_entity_cfg.body_ids[0]
137
138    # Define simulation stepping
139    sim_dt = sim.get_physics_dt()
140    count = 0
141    # Simulation loop
142    while simulation_app.is_running():
143        # reset
144        if count % 150 == 0:
145            # reset time
146            count = 0
147            # reset joint state
148            joint_pos = robot.data.default_joint_pos.clone()
149            joint_vel = robot.data.default_joint_vel.clone()
150            robot.write_joint_state_to_sim(joint_pos, joint_vel)
151            robot.reset()
152            # reset actions
153            ik_commands[:] = ee_goals[current_goal_idx]
154            joint_pos_des = joint_pos[:, robot_entity_cfg.joint_ids].clone()
155            # reset controller
156            diff_ik_controller.reset()
157            diff_ik_controller.set_command(ik_commands)
158            # change goal
159            current_goal_idx = (current_goal_idx + 1) % len(ee_goals)
160        else:
161            # obtain quantities from simulation
162            jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
163            ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
164            root_pose_w = robot.data.root_state_w[:, 0:7]
165            joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]
166            # compute frame in root frame
167            ee_pos_b, ee_quat_b = subtract_frame_transforms(
168                root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
169            )
170            # compute the joint commands
171            joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
172
173        # apply actions
174        robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
175        scene.write_data_to_sim()
176        # perform step
177        sim.step()
178        # update sim-time
179        count += 1
180        # update buffers
181        scene.update(sim_dt)
182
183        # obtain quantities from simulation
184        ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
185        # update marker positions
186        ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
187        goal_marker.visualize(ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7])
188
189
190def main():
191    """Main function."""
192    # Load kit helper
193    sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)
194    sim = sim_utils.SimulationContext(sim_cfg)
195    # Set main camera
196    sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0])
197    # Design scene
198    scene_cfg = TableTopSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)
199    scene = InteractiveScene(scene_cfg)
200    # Play the simulator
201    sim.reset()
202    # Now we are ready!
203    print("[INFO]: Setup complete...")
204    # Run the simulator
205    run_simulator(sim, scene)
206
207
208if __name__ == "__main__":
209    # run the main function
210    main()
211    # close sim app
212    simulation_app.close()

代码解释#

在使用任何任务空间控制器时，确保所提供的量在正确的框架中是很重要的。当并行化环境实例时，它们都存在于同一个独特的模拟世界框架中。然而，通常，我们希望每个环境本身都有自己的本地框架。这可以通过 scene.InteractiveScene.env_origins 属性访问。

在我们的 API 中，我们使用以下符号来表示框架:

  • 模拟世界框架（表示为 w ），这是整个模拟的框架。

  • 本地环境框架（表示为 e ），这是本地环境的框架。

  • 机器人的基础框架（表示为 b ），这是机器人基座链接的框架。

由于资产实例不 “认识” 本地环境框架，它们返回其状态在模拟世界框架中。因此，我们需要将获取的量转换为本地环境框架。这是通过从获取的量中减去本地环境原点来完成的。

创建一个 IK 控制器#

DifferentialIKController 类计算机器人达到期望末端执行器姿势所需的关节位置。所包含的实现以批量格式执行计算，并使用 PyTorch 操作。支持不同类型的逆运动学求解器，包括阻尼最小二乘法和伪逆方法。这些求解器可以使用 ik_method 参数进行指定。此外，该控制器可以处理相对和绝对姿势命令。

在本教程中，我们将使用阻尼最小二乘法来计算期望的关节位置。此外，由于我们希望跟踪期望的末端执行器姿势，我们将使用绝对姿势命令模式。

    # 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)

获取机器人的关节和体指数#

IK 控制器的实现是一个仅计算类。因此，它期望用户提供有关机器人的必要信息。这包括机器人的关节位置、当前末端执行器姿势和雅可比矩阵。

虽然属性 assets.ArticulationData.joint_pos 提供了关节位置，但我们只想要机器人手臂的关节位置，而不是夹具。类似地，虽然属性 assets.ArticulationData.body_state_w 提供了所有机器人身体的状态，但我们只想要机器人末端执行器的状态。因此，我们需要对这些数组进行索引以获取所需的量。

为此，关节类提供了方法 find_joints() 和 find_bodies() 。这些方法接受关节和体的名称，并返回它们对应的索引。

虽然您可以直接使用这些方法来获取索引，但我们建议使用 SceneEntityCfg 类解析索引。此类在各个 API 中用于从场景实体中提取某些信息。内部调用上述方法以获取索引。但是，它还执行一些额外的检查以确保提供的名称是有效的。因此，使用此类是一个更安全的选择。

    # Specify robot-specific parameters
    if args_cli.robot == "franka_panda":
        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"])
    else:
        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
    # 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.
    if robot.is_fixed_base:
        ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
    else:
        ee_jacobi_idx = robot_entity_cfg.body_ids[0]

计算机器人命令#

IK 控制器将设置所需命令和计算所需关节位置的操作分开。这样做是为了允许用户以不同的频率运行 IK 控制器而非机器人的控制频率。

set_command() 方法将期望的末端执行器姿势作为单个批量数组输入。姿势在机器人的基座框架中指定。

            # reset controller
            diff_ik_controller.reset()
            diff_ik_controller.set_command(ik_commands)

然后，我们可以使用 compute() 方法计算所需的关节位置。该方法接受当前末端执行器姿势（在基座框架中）、雅可比矩阵和当前关节位置为输入。我们从机器人数据中读取雅可比矩阵，该数据使用其从物理引擎计算的价值。

            # 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]
            root_pose_w = robot.data.root_state_w[:, 0:7]
            joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]
            # compute frame in root frame
            ee_pos_b, ee_quat_b = subtract_frame_transforms(
                root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
            )
            # compute the joint commands
            joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)

计算得到的关节位置目标然后可以应用在机器人上，就像在以前的教程中所做的那样。

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

代码执行#

现在，我们已经查看了代码，请运行脚本并查看结果:

./isaaclab.sh -p source/standalone/tutorials/05_controllers/run_diff_ik.py --robot franka_panda --num_envs 128

该脚本将启动一个具有 128 个机器人的模拟。使用 IK 控制器来控制这些机器人。当前和期望的末端执行器姿势应该使用框架标记显示。当机器人达到期望姿势时，命令应该循环到脚本中指定的下一个姿势。

run_diff_ik.py 的结果

要停止模拟，您可以关闭窗口，或在终端中按下 Ctrl+C 。