用Python控制Isaac Sim机器人

Isaac Sim提供了强大的Python API来控制仿真中的机器人。本文深入讲解运动控制、轨迹规划和传感器数据读取。

关节空间控制

import numpy as np
from isaacsim.core import World
from isaacsim.assets import FrankaRobot

world = World()
franka = FrankaRobot(prim_path="/World/Franka", name="franka")
world.scene.add(franka)
world.reset()

# 获取当前关节角度
current_joints = franka.get_joint_positions()
print(f"当前关节角度: {current_joints}")

# 设置目标关节角度
target_joints = np.array([0.0, -0.5, 0.0, -1.5, 0.0, 1.0, 0.0])
franka.set_joint_positions(target_joints)

# 等待运动完成
while not franka.reached_target():
    world.step(render=True)

笛卡尔空间控制

# 获取末端执行器位姿
end_effector_pose = franka.get_end_effector_pose()
position = end_effector_pose[:3]  # [x, y, z]
orientation = end_effector_pose[3:]  # [qx, qy, qz, qw]

# 移动到目标位置
target_position = np.array([0.5, 0.0, 0.3])
target_orientation = np.array([0.0, 0.0, 0.0, 1.0])  # 单位四元数

franka.move_to_position(target_position)
franka.move_to_orientation(target_orientation)

# 同时移动位置和方向
franka.move_to_pose(position=target_position, orientation=target_orientation)

轨迹规划

class TrajectoryPlanner:
    def __init__(self, robot):
        self.robot = robot

    def plan_linear_path(self, start, end, steps=100):
        """规划直线路径"""
        trajectory = np.linspace(start, end, steps)
        return trajectory

    def plan_circular_path(self, center, radius, steps=100):
        """规划圆形路径"""
        angles = np.linspace(0, 2*np.pi, steps)
        trajectory = []
        for angle in angles:
            x = center[0] + radius * np.cos(angle)
            y = center[1] + radius * np.sin(angle)
            z = center[2]
            trajectory.append([x, y, z])
        return np.array(trajectory)

    def execute_trajectory(self, trajectory, speed=0.1):
        """执行轨迹"""
        for point in trajectory:
            self.robot.move_to_position(point)
            # 等待到达
            while np.linalg.norm(
                self.robot.get_end_effector_pose()[:3] - point
            ) > 0.01:
                world.step(render=True)

# 使用示例
planner = TrajectoryPlanner(franka)

# 直线运动
start = np.array([0.3, 0.0, 0.2])
end = np.array([0.5, 0.2, 0.3])
trajectory = planner.plan_linear_path(start, end)
planner.execute_trajectory(trajectory)

# 圆形运动
center = np.array([0.4, 0.0, 0.25])
radius = 0.1
trajectory = planner.plan_circular_path(center, radius)
planner.execute_trajectory(trajectory)

读取传感器数据

class SensorReader:
    def __init__(self, camera, lidar=None):
        self.camera = camera
        self.lidar = lidar

    def get_camera_data(self):
        """获取相机数据"""
        rgb = self.camera.get_rgb()          # RGB图像
        depth = self.camera.get_depth()      # 深度图
        normals = self.camera.get_normals()  # 法线图

        # 获取相机内参
        intrinsics = self.camera.get_intrinsics()
        fx, fy = intrinsics[0, 0], intrinsics[1, 1]
        cx, cy = intrinsics[0, 2], intrinsics[1, 2]

        return {
            "rgb": rgb,
            "depth": depth,
            "normals": normals,
            "intrinsics": {"fx": fx, "fy": fy, "cx": cx, "cy": cy}
        }

    def get_lidar_data(self):
        """获取激光雷达数据"""
        if self.lidar is None:
            return None

        points = self.lidar.get_point_cloud()  # 点云数据
        intensities = self.lidar.get_intensities()  # 反射强度

        return {
            "points": points,
            "intensities": intensities
        }

    def depth_to_pointcloud(self, depth, intrinsics):
        """将深度图转换为点云"""
        h, w = depth.shape
        fx, fy = intrinsics["fx"], intrinsics["fy"]
        cx, cy = intrinsics["cx"], intrinsics["cy"]

        # 创建像素坐标网格
        u, v = np.meshgrid(np.arange(w), np.arange(h))

        # 反投影到3D空间
        z = depth
        x = (u - cx) * z / fx
        y = (v - cy) * z / fy

        # 组合成点云
        points = np.stack([x, y, z], axis=-1)
        points = points.reshape(-1, 3)

        # 过滤无效点
        valid = (z > 0) & (z < 5.0)  # 0-5米范围
        points = points[valid.flatten()]

        return points

# 使用示例
sensor = SensorReader(camera)
data = sensor.get_camera_data()

# 深度图转点云
point_cloud = sensor.depth_to_pointcloud(data["depth"], data["intrinsics"])
print(f"点云点数: {len(point_cloud)}")

力/力矩控制

class ForceController:
    def __init__(self, robot):
        self.robot = robot

    def apply_force(self, force_vector):
        """在末端执行器施加力"""
        self.robot.apply_end_effector_force(force_vector)

    def compliant_control(self, target_position, stiffness=100, damping=10):
        """柔顺控制"""
        current_pos = self.robot.get_end_effector_pose()[:3]
        current_vel = self.robot.get_end_effector_velocity()[:3]

        # 计算力
        position_error = target_position - current_pos
        force = stiffness * position_error - damping * current_vel

        # 施加力
        self.apply_force(force)

        return force

# 使用示例
controller = ForceController(franka)

# 柔顺移动到目标位置
target = np.array([0.5, 0.0, 0.2])
for _ in range(1000):
    world.step(render=True)
    force = controller.compliant_control(target)
    if np.linalg.norm(force) < 0.1:
        break

FAQ

如何控制机械臂的速度？

使用set_joint_velocities()设置关节速度，或在轨迹规划中调整时间参数。

传感器数据有延迟怎么办？

Isaac Sim的传感器数据是同步的。如果感觉有延迟，检查仿真步长设置。

如何实现多机器人协同控制？

每个机器人作为独立的Prim对象，通过共享的状态变量或ROS2话题进行协调。