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

时间:2024-10-14 07:37:43

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

介绍如何基于大语言模型实现机械臂在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_array

print("抓取候选:", 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)
      break

print("筛选后的抓取候选:", 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)
  pass

def 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强大的代码生成能力,实现更加灵活和智能的机器人控制。