结合大语言模型的机械臂抓取操作学习

一、 大语言模型的机械臂抓取操作关键步骤

介绍如何基于大语言模型实现机械臂在PyBullet环境中的抓取操作，涵盖机器人运动学、坐标系转换、抓取候选位姿生成、开放词汇检测以及大语言模型代码生成等模块。

1. 机器人正逆运动学基本概念

正运动学: 已知机器人的关节角度，计算机器人末端执行器在空间中的位姿（位置和姿态）。
逆运动学:已知机器人末端执行器在空间中的目标位姿，计算机器人各关节角度使其达到目标位姿。
在PyBullet中，可以使用p.calculateInverseKinematics()函数进行逆运动学计算，p.getLinkState()函数获取机器人连杆状态，包括位置和姿态信息。

正运动学示例：已知关节角度，计算末端执行器位姿。

import pybullet as p# 连接到PyBullet物理引擎
p.connect(p.GUI)# 加载机器人模型
robot_id = p.loadURDF("path/to/robot.urdf")# 设置关节角度
joint_angles = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
for i in range(p.getNumJoints(robot_id)):p.resetJointState(robot_id, i, joint_angles[i])# 获取末端执行器位姿
link_state = p.getLinkState(robot_id, p.getNumJoints(robot_id) - 1)
end_effector_position = link_state[0]
end_effector_orientation = link_state[1]print("末端执行器位置:", end_effector_position)
print("末端执行器姿态:", end_effector_orientation)

逆运动学示例：已知末端执行器位姿，计算关节角度。

import pybullet as p# ... (连接到PyBullet，加载机器人模型)# 设置目标末端执行器位姿
target_position = [0.5, 0.5, 0.5]
target_orientation = [0, 0, 0, 1]# 计算逆运动学解
joint_angles = p.calculateInverseKinematics(robot_id, p.getNumJoints(robot_id) - 1, target_position, targetOrientation=target_orientation)# 设置关节角度
for i in range(p.getNumJoints(robot_id)):p.resetJointState(robot_id, i, joint_angles[i])print("关节角度:", joint_angles)

2. 坐标系转换

机器人系统通常涉及多个坐标系，例如世界坐标系、机器人基坐标系、相机坐标系等。为了实现抓取操作，需要进行坐标系之间的转换。

相机坐标系到世界坐标系:需要相机的外参矩阵（旋转和平移），可以使用p.getCameraImage()函数获取相机姿态信息，并进行转换。
世界坐标系到机器人基坐标系: 需要机器人基坐标系在世界坐标系中的位姿，可以使用p.getBasePositionAndOrientation()函数获取。
机器人基坐标系到末端执行器坐标系: 通过正运动学计算得到。
PyBullet提供了一些函数方便进行坐标系转换，例如p.multiplyTransforms()可以将两个变换矩阵相乘。
坐标系转换示例

import pybullet as p
import numpy as np# ... (连接到PyBullet，加载机器人模型，设置相机)# 获取相机位姿
view_matrix = p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=[0, 0, 0],distance=1.0,yaw=0,pitch=-30,roll=0,upAxisIndex=2)
projection_matrix = p.computeProjectionMatrixFOV(fov=60,aspect=1.0,nearVal=0.1,farVal=100.0)# 获取相机坐标系到世界坐标系的转换矩阵
cam_to_world = np.linalg.inv(np.reshape(view_matrix, (4, 4)))# 获取机器人基座坐标系到世界坐标系的转换矩阵
base_pos, base_orn = p.getBasePositionAndOrientation(robot_id)
base_to_world = np.eye(4)
base_to_world[:3, :3] = p.getMatrixFromQuaternion(base_orn)
base_to_world[:3, 3] = base_pos# 获取相机坐标系到机器人基座坐标系的转换矩阵
cam_to_base = np.dot(np.linalg.inv(base_to_world), cam_to_world)print("相机到世界坐标系:", cam_to_world)
print("机器人基座到世界坐标系:", base_to_world)
print("相机到机器人基座坐标系:", cam_to_base)

3. 基于点云的GraspNet抓取候选生成

GraspNet是一种基于深度学习的抓取姿态估计方法，可以从点云数据中预测多个抓取候选。
获取点云数据:使用RGB-D相机或激光雷达获取场景的点云数据。PyBullet可以使用p.getCameraImage()函数获取深度图，并将其转换为点云。
预处理点云: 对点云进行降采样、滤波等预处理操作。可以使用Open3D或PCL库进行点云处理。
使用GraspNet模型预测抓取候选: 将预处理后的点云输入GraspNet模型，得到多个抓取候选，包括抓取位置、方向和置信度 。
GraspNet抓取候选生成代码举例：

# 假设你已经获取了点云数据 (point_cloud)from graspnetAPI import GraspGroup# 加载GraspNet模型
grasp_net = GraspGroup(model_path="path/to/graspnet_model")# 生成抓取候选
gg = grasp_net.detectGrasp(point_cloud)# 获取抓取姿态和得分
grasps = gg.grasp_group_arrayprint("抓取候选:", grasps)

4. 开放词汇检测对候选进行筛选

为了实现基于自然语言指令的抓取，需要使用开放词汇检测技术识别目标物体。

目标检测: 使用目标检测模型（如YOLOv5）对相机图像进行目标检测，获取目标物体的位置和类别信息。
投影到像素坐标系: 将GraspNet生成的抓取候选从世界坐标系投影到相机像素坐标系。
筛选抓取候选: 判断抓取候选是否位于目标物体的检测框内，并选择置信度最高的抓取候选。

# 假设你已经获取了目标检测结果 (detections)# 遍历抓取候选
filtered_grasps = []
for grasp in grasps:# 将抓取中心点投影到像素坐标系grasp_center_pixel = project_point_to_pixel(grasp.center, view_matrix, projection_matrix)# 检查抓取中心点是否在目标检测框内for detection in detections:if detection.class_name == "red box" and is_point_in_bbox(grasp_center_pixel, detection.bbox):filtered_grasps.append(grasp)breakprint("筛选后的抓取候选:", filtered_grasps)

5. 通过Prompt调用LLM生成规划代码

使用大语言模型（LLM）根据自然语言指令生成抓取规划代码。
DeepSeek v2 API: 可以通过API调用DeepSeek的大语言模型。

Prompt设计: 你是一个机器人，你拥有的技能API如下：
1. get_grasp_by_name(name_text): 输入类别文本，返回检测候选抓取的list
2. execute_grasp(grasp): 输入候选抓取的list，然后执行抓取
现在需要你根据你所拥有的技能API，编写python代码完成给你的任务，只输出plan函数，不要输出其他代码以为的内容。你的任务是“抓取红色的盒子”。

调用LLM API: 将Prompt发送给LLM API (例如DeepSeek v2 API)，获取生成的Python代码。
执行代码:解析LLM生成的代码，并执行plan()函数，完成抓取操作。

本示例代码演示如何使用大语言模型 (LLM) 和 PyBullet 仿真环境实现机械臂抓取红色盒子。

import deepseek# 初始化DeepSeek API
api_key = "YOUR_API_KEY"
client = deepseek.Client(api_key)# 定义API描述和任务
api_description = """
你是一个机器人，你拥有的技能API如下：
1. get_grasp_by_name(name_text): 输入类别文本，返回检测候选抓取的list
2. execute_grasp(grasp): 输入候选抓取的list，然后执行抓取
"""task_description = "抓取红色盒子"# 生成规划代码
prompt = f"{api_description}\n现在需要你根据你所拥有的技能API，编写python代码完成给你的任务，只输出plan函数，不要输出其他代码以为的内容。你的任务是“{task_description}”"
response = client.generate_code(prompt)
plan_code = response.code# 执行规划代码
exec(plan_code)
plan()

二、完整示例代码:

import pybullet as p
import numpy as np
from graspnetAPI import GraspGroup
import deepseek# ... (定义project_point_to_pixel和is_point_in_bbox函数)# 连接到PyBullet物理引擎
p.connect(p.GUI)# 加载机器人模型
robot_id = p.loadURDF("path/to/robot.urdf")# ... (设置相机)# 获取点云数据
point_cloud = get_point_cloud()# 生成抓取候选
grasp_net = GraspGroup(model_path="path/to/graspnet_model")
gg = grasp_net.detectGrasp(point_cloud)
grasps = gg.grasp_group_array# 获取目标检测结果
detections = get_detections()# 筛选抓取候选
filtered_grasps = []
for grasp in grasps:grasp_center_pixel = project_point_to_pixel(grasp.center, view_matrix, projection_matrix)for detection in detections:if detection.class_name == "red box" and is_point_in_bbox(grasp_center_pixel, detection.bbox):filtered_grasps.append(grasp)break# 调用LLM生成规划代码
api_key = "YOUR_API_KEY"
client = deepseek.Client(api_key)
api_description = """
你是一个机器人，你拥有的技能API如下：
1. get_grasp_by_name(name_text): 输入类别文本（注意是英文，要简短），返回检测候选抓取的list
2. execute_grasp(grasp): 输入候选抓取的list，然后执行抓取
"""
task_description = "抓取红色盒子"
prompt = f"{api_description}\n现在需要你根据你所拥有的技能API，编写python代码完成给你的任务，只输出plan函数，不要输出其他代码以为的内容。你的任务是“{task_description}”"
response = client.generate_code(prompt)
plan_code = response.code# 定义API函数
def get_grasp_by_name(name_text):# ... (根据name_text筛选抓取候选，例如返回filtered_grasps)passdef execute_grasp(grasp):# ... (执行抓取操作，例如使用逆运动学控制机器人)pass# 执行规划代码
exec(plan_code)
plan()

注意:

• 以上代码仅为示例，需要根据实际情况进行修改和完善。
• 需要安装PyBullet, GraspNet API, DeepSeek SDK等相关库。
• 需要替换 path/to/robot.urdf, path/to/graspnet_model, YOUR_API_KEY 等占位符。
• 需要实现 project_point_to_pixel, is_point_in_bbox, get_point_cloud, get_detections, get_grasp_by_name, execute_grasp 等函数。

三、总结

结合了机器人学、计算机视觉和自然语言处理技术，实现了基于大语言模型的机械臂抓取操作。通过合理的Prompt设计和API定义，可以利用LLM强大的代码生成能力，实现更加灵活和智能的机器人控制。