1.环境构建

1.1 Cuda切换

参考博客：https://www.cnblogs.com/john-mu-wanfeng/p/19071862进行cuda安装切换
cuda官网：https://developer.nvidia.com/cuda-toolkit-archive
cudnn官网：https://developer.nvidia.com/rdp/cudnn-archive

1.2 graspnet环境配置

下载YOLO_World-SAM-GraspNet项目
百度网盘：链接: https://pan.baidu.com/s/1PoDS7odFY6rfxeVfQSn4SQ  提取码: yolo

最近链接实效了，贴另一个佬提供的链接
https://pan.baidu.com/s/1NNeS5C_9irYVFHOGCWR0ZQ    rri4

#1.创建环境
conda create -n mujoco_graspnet python=3.10
conda activate mujoco_graspnet
 
#2.进入项目的graspnet-baseline文件
cd graspnet-baseline
 
#修改requirements.txt
sudo gedit requirements.txt
#这里要指定numpy的安装版本为1.23.0,和scipy版本匹配。删除torch，最后如下所示
#tensorboard
#numpy==1.23.0
#scipy
#open3d>=0.8
#Pillow
#tqdm
#安装graspnet-baseline环境所依赖
pip install -r requirements.txt
 
#4.安装torch等所需的环境包，这里默认直接安装torch就好，选择cuda版本12.x
pip install torch torchvision torchaudio #需指定cuda版本
pip install spatialmath-python==1.1.14
pip install roboticstoolbox-python==1.1.1
pip install modern-robotics==1.1.1
pip install mujoco==3.3.1
 
#5.编译并安装PointNet++自定义算子
cd pointnet2
python setup.py install
cd ../
#6.编译并安装基于PyTorch CUDA实现的k-最近邻（k-NN）算子
cd knn
python setup.py install
cd ../
 
#7.安装graspnetAPI工具包
cd graspnetAPI
#修改setup.py，将sklearn替换为scikit-learn
 
pip install .
cd ../

8.安装KDL运动学解算包
conda install python-orocos-kdl

9.补充额外依赖
pip install ultralytics
pip install lxml
pip install git+https://github.com/openai/CLIP.git

 

1.3 测试运行

直接执行python main_yoloWorld_sam.py

2.工具坐标系切换

由于我在使用该项目时需要控制rm eco65臂，因此仿真环境内需要切换到eco65臂的urdf和scene.xml来代替原本的ur5机械臂。修改manipulator_grasp/env中的env.py文件。

class RMEco65Env:
    def __init__(self):
        pass #同ur5

    def reset(self):
        # 初始化 MuJoCo 模型和数据
        filename = os.path.join(os.path.dirname(os.path.dirname(__file__)), 'assets', 'rm_eco65_scene', 'scene.xml')


        # 创建机械臂实例并设置其基座位置
        self.robot = EM_ECO65()
        # 工具偏移
        robot_tool = sm.SE3.RPY(np.pi/2, -np.pi/2, np.pi)*sm.SE3.Trans(0.142, 0.0, 0.0)
        #其它同ur5


    def close(self):
        pass #同ur5

    def step(self, action=None):
        pass #同ur5

    def render(self):
        pass #同ur5

if __name__ == '__main__':
    env = RMEco65Env()
    env.reset()
    for i in range(1000000):
        env.step()
    imgs = env.render()
    env.close()

由于末端坐标系不同，工具坐标系增加了额外的旋转和平移
 robot_tool = sm.SE3.RPY(np.pi/2, -np.pi/2, np.pi)*sm.SE3.Trans(0.142, 0.0, 0.0)
机器人xml场景：

<mujoco model="rm_eco65_with_gripper">
  <compiler angle="radian"/>

  <default>
    <include file="./rm_eco65_classes.xml"/>
  </default>

  <asset>
    <!-- 机械臂网格 -->
    <mesh name="baselink" content_type="model/stl" file="./mesh/rm_eco65_arm/baselink.STL"/>
    <mesh name="Link1" content_type="model/stl" file="./mesh/rm_eco65_arm/Link1.STL"/>
    <mesh name="Link2" content_type="model/stl" file="./mesh/rm_eco65_arm/Link2.STL"/>
    <mesh name="Link3" content_type="model/stl" file="./mesh/rm_eco65_arm/Link3.STL"/>
    <mesh name="Link4" content_type="model/stl" file="./mesh/rm_eco65_arm/Link4.STL"/>
    <mesh name="Link5" content_type="model/stl" file="./mesh/rm_eco65_arm/Link5.STL"/>
    <mesh name="Link6" content_type="model/stl" file="./mesh/rm_eco65_arm/Link6.STL"/>
    
    <!-- 夹爪网格 -->
    <mesh name="base_mount" file="./assets/base_mount.stl" scale="0.001 0.001 0.001"/>
    <mesh name="base" file="./assets/base.stl" scale="0.001 0.001 0.001"/>
    <mesh name="driver" file="./assets/driver.stl" scale="0.001 0.001 0.001"/>
    <mesh name="coupler" file="./assets/coupler.stl" scale="0.001 0.001 0.001"/>
    <mesh name="follower" file="./assets/follower.stl" scale="0.001 0.001 0.001"/>
    <mesh name="pad" file="./assets/pad.stl" scale="0.001 0.001 0.001"/>
    <mesh name="silicone_pad" file="./assets/silicone_pad.stl" scale="0.001 0.001 0.001"/>
    <mesh name="spring_link" file="./assets/spring_link.stl" scale="0.001 0.001 0.001"/>
    
    <!-- 材质定义 -->
    <material name="metal" rgba="0.58 0.58 0.58 1"/>
    <material name="silicone" rgba="0.1882 0.1882 0.1882 1"/>
    <material name="gray" rgba="0.4627 0.4627 0.4627 1"/>
    <material name="black" rgba="0.149 0.149 0.149 1"/>
  </asset>

  <default>
    <default class="2f85">
      <mesh scale="0.001 0.001 0.001"/>
      <general biastype="affine"/>

      <joint axis="1 0 0"/>
      <default class="driver">
        <joint range="0 0.8" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
      </default>
      <default class="follower">
        <joint range="-0.872664 0.872664" armature="0.001" pos="0 -0.018 0.0065" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
      </default>
      <default class="spring_link">
        <joint range="-0.29670597283 0.8" armature="0.001" stiffness="0.05" springref="2.62" damping="0.00125"/>
      </default>
      <default class="coupler">
        <joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
      </default>

      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
      </default>
      <default class="collision">
        <geom type="mesh" group="3"/>
        <default class="pad_box1">
          <geom mass="0" type="box" pos="0 -0.0026 0.028125" size="0.011 0.004 0.009375" friction="0.7"
            solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.55 0.55 0.55 1"/>
        </default>
        <default class="pad_box2">
          <geom mass="0" type="box" pos="0 -0.0026 0.009375" size="0.011 0.004 0.009375" friction="0.6"
            solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.45 0.45 0.45 1"/>
        </default>
      </default>
    </default>
  </default>

  <worldbody>
    <!-- 机械臂主体 -->
    <body name="eco65_base" pos="0.80 0.6 0.745" quat="0 0 0 1">
      <geom type="mesh" rgba="1 1 0.9451 1" mesh="baselink"/>
      <body name="Link1" pos="0 0 0.1625">
        <inertial pos="-0.022858 -2.0784e-06 -0.013658" quat="0.422359 0.558164 0.572915 0.426419" mass="0.74247" diaginertia="0.00184837 0.00159481 0.000747401"/>
        <joint name="joint1" class="size3_limited" pos="0 0 0" axis="0 0 1" armature="0.0005" range="-3.1067 3.1067"/>
        <geom type="mesh" rgba="1 1 0.9451 1" mesh="Link1"/>
        <body name="Link2" pos="-0.086 0 0" quat="0.707105 0.707108 0 0">
          <inertial pos="-1.6049e-06 0.10336 0.067332" quat="0.49284 0.507114 -0.50706 0.492782" mass="1.3502" diaginertia="0.0129426 0.012821 0.00111406"/>
          <joint name="joint2" class="size3_limited" pos="0 0 0" axis="0 0 -1" armature="0.0005" range="-3.1067 2.3562"/>
          <geom type="mesh" rgba="1 1 0.9451 1" mesh="Link2"/>
          <body name="Link3" pos="0 0.26 0" quat="-2.59734e-06 0.707105 0.707108 -2.59735e-06">
            <inertial pos="0.10374 7.0495e-07 0.018599" quat="0.519814 0.479367 0.479376 0.519807" mass="1.0422" diaginertia="0.0094046 0.00930383 0.000785521"/>
            <joint name="joint3" class="size3_limited" pos="0 0 0" axis="0 0 1" armature="0.0005" range="-2.7925 2.5307"/>
            <geom type="mesh" rgba="1 1 0.9451 1" mesh="Link3"/>
            <body name="Link4" pos="0.24 0 -0.05888" quat="0.707105 0 0 0.707108">
              <inertial pos="-0.0069819 -0.016328 0.0067416" quat="0.446544 0.894762 -6.23183e-05 5.65167e-06" mass="0.27383" diaginertia="0.00024751 0.000214431 0.000154999"/>
              <joint name="joint4" class="size3_limited" pos="0 0 0" axis="0 0 1" armature="0.0005" range="-3.1067 3.1067"/>
              <geom type="mesh" rgba="1 1 0.9451 1" mesh="Link4"/>
              <body name="Link5" pos="0.00040486 -0.10983 0" quat="0.707105 0.707108 0 0">
                <inertial pos="-0.00040425 0.0066328 -0.012475" quat="0.446544 0.894762 -6.23183e-05 5.65167e-06" mass="0.27794" diaginertia="0.00024751 0.000214431 0.000154999"/>
                <joint name="joint5" class="size3_limited" pos="0 0 0" axis="0 0 1" armature="0.0005" range="-3.1067 3.1067"/>
                <geom type="mesh" rgba="0.29804 0.28627 0.28235 1" mesh="Link5"/>
                <body name="Link6" pos="0 0.0795 0" quat="0.707105 -0.707108 0 0">
                  <inertial pos="0.00095587 -0.0013555 -0.010297" quat="0.594994 0.390437 -0.592979 0.37672" mass="0.18" diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
                  <joint name="joint6" class="size1" pos="0 0 0" axis="0 0 1" armature="0.01" range="-6.2832 6.2832" damping="0.3"/>
                  <geom type="mesh" rgba="1 1 0.9451 1" mesh="Link6"/>

                  <site name="tcp" pos="0 0 0" type="box" size="0.01 0.01 0.01"  rgba="0 1 0 0.5" group="2" />
                  

                  <!-- 在这里连接夹爪到机械臂末端 -->
                      <body name="2f85_base" pos="0 0 0" quat="0 0 0 1" childclass="2f85">
                    <geom class="visual" mesh="base_mount" material="black"/>
                    <geom class="collision" mesh="base_mount"/>
                    <body name="base" pos="0 0 0.0038" quat="1 0 0 -1">
                      <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0"
                        diaginertia="0.000260285 0.000225381 0.000152708"/>
                      <geom class="visual" mesh="base" material="black"/>
                      <geom class="collision" mesh="base"/>
                      <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
                      <!-- 右侧4杆机构 -->
                      <body name="right_driver" pos="0 0.0306011 0.054904">
                        <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
                          diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
                        <joint name="right_driver_joint" class="driver"/>
                        <geom class="visual" mesh="driver" material="gray"/>
                        <geom class="collision" mesh="driver"/>
                        <body name="right_coupler" pos="0 0.0315 -0.0041">
                          <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
                            diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
                          <joint name="right_coupler_joint" class="coupler"/>
                          <geom class="visual" mesh="coupler" material="black"/>
                          <geom class="collision" mesh="coupler"/>
                        </body>
                      </body>
                      <body name="right_spring_link" pos="0 0.0132 0.0609">
                        <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
                          diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
                        <joint name="right_spring_link_joint" class="spring_link"/>
                        <geom class="visual" mesh="spring_link" material="black"/>
                        <geom class="collision" mesh="spring_link"/>
                        <body name="right_follower" pos="0 0.055 0.0375">
                          <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
                            diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
                          <joint name="right_follower_joint" class="follower"/>
                          <geom class="visual" mesh="follower" material="black"/>
                          <geom class="collision" mesh="follower"/>
                          <body name="right_pad" pos="0 -0.0189 0.01352">
                            <geom class="pad_box1" name="right_pad1"/>
                            <geom class="pad_box2" name="right_pad2"/>
                            <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="0.707107 0 0 0.707107"
                              diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
                            <geom class="visual" mesh="pad"/>
                            <body name="right_silicone_pad">
                              <geom class="visual" mesh="silicone_pad" material="black"/>
                            </body>
                          </body>
                        </body>
                      </body>
                      <!-- 左侧4杆机构 -->
                      <body name="left_driver" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
                        <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
                          diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
                        <joint name="left_driver_joint" class="driver"/>
                        <geom class="visual" mesh="driver" material="gray"/>
                        <geom class="collision" mesh="driver"/>
                        <body name="left_coupler" pos="0 0.0315 -0.0041">
                          <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
                            diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
                          <joint name="left_coupler_joint" class="coupler"/>
                          <geom class="visual" mesh="coupler" material="black"/>
                          <geom class="collision" mesh="coupler"/>
                        </body>
                      </body>
                      <body name="left_spring_link" pos="0 -0.0132 0.0609" quat="0 0 0 1">
                        <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
                          diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
                        <joint name="left_spring_link_joint" class="spring_link"/>
                        <geom class="visual" mesh="spring_link" material="black"/>
                        <geom class="collision" mesh="spring_link"/>
                        <body name="left_follower" pos="0 0.055 0.0375">
                          <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
                            diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
                          <joint name="left_follower_joint" class="follower"/>
                          <geom class="visual" mesh="follower" material="black"/>
                          <geom class="collision" mesh="follower"/>
                          <body name="left_pad" pos="0 -0.0189 0.01352">
                            <geom class="pad_box1" name="left_pad1"/>
                            <geom class="pad_box2" name="left_pad2"/>
                            <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1"
                              diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
                            <geom class="visual" mesh="pad"/>
                            <body name="left_silicone_pad">
                              <geom class="visual" mesh="silicone_pad" material="black"/>
                            </body>
                          </body>
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <contact>
    <exclude body1="base" body2="left_driver"/>
    <exclude body1="base" body2="right_driver"/>
    <exclude body1="base" body2="left_spring_link"/>
    <exclude body1="base" body2="right_spring_link"/>
    <exclude body1="right_coupler" body2="right_follower"/>
    <exclude body1="left_coupler" body2="left_follower"/>
  </contact>

  <tendon>
    <fixed name="split">
      <joint joint="right_driver_joint" coef="0.5"/>
      <joint joint="left_driver_joint" coef="0.5"/>
    </fixed>
  </tendon>

  <equality>
    <connect anchor="0 0 0" body1="right_follower" body2="right_coupler" solimp="0.95 0.99 0.001" solref="0.005 1"/>
    <connect anchor="0 0 0" body1="left_follower" body2="left_coupler" solimp="0.95 0.99 0.001" solref="0.005 1"/>
    <joint joint1="right_driver_joint" joint2="left_driver_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001"
      solref="0.005 1"/>
  </equality>

  <actuator>
    <!-- 机械臂关节执行器 -->
    <general class="size3" name="joint1_act" joint="joint1"/>
    <general class="size3" name="joint2_act" joint="joint2"/>
    <general class="size3" name="joint3_act" joint="joint3"/>
    <general class="size1" name="joint4_act" joint="joint4"/>
    <general class="size1" name="joint5_act" joint="joint5"/>
    <general class="size1" name="joint6_act" joint="joint6"/>
    
    <!-- 夹爪执行器 -->
    <general class="2f85" name="fingers_actuator" tendon="split" forcerange="-5 5" ctrlrange="0 255"
      gainprm="0.3137255 0 0" biasprm="0 -100 -10"/>
  </actuator>
</mujoco>

class,scene可根据需要模仿ur5自行调整。

3.逆运动学

ur5采用其特有的解算方法，对于其它的机械臂并不通用，为了仿真时准确解算出目标关节角度，需要引入通用的逆运动学解算方法，这里使用kdl算法代替。创建EM_ECO65类（类似UR5）继承Robot，修改ik，fk和move_cartesian函数

import os
import sys
# 自动补充项目根目录和manipulator_grasp到sys.path，兼容直接运行和包方式
ROOT_DIR = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
sys.path.append(ROOT_DIR)
sys.path.append(os.path.join(ROOT_DIR, 'manipulator_grasp'))

from typing import List

import numpy as np
import roboticstoolbox as rtb
from spatialmath import SE3
import modern_robotics as mr

from arm.geometry import Geometry3D, Capsule
from arm.utils import MathUtils
from .robot_config import RobotConfig
from .robot import Robot

from kdl_parser.urdf_parser_py.urdf import URDF
from kdl_parser.urdf import treeFromFile, treeFromUrdfModel
import PyKDL

class EM_ECO65(Robot):
    def __init__(self) -> None:
        super().__init__()
        # RM_ECO65 机械臂参数
        success, tree = self.load_urdf_to_kdl('eco65 urdf路径')
        base_link = "baselink"
        tip_link = "Link6"
        chain = self.create_chain_from_tree(tree, base_link, tip_link)
        self.fk_solver, self.ik_vel_solver, self.ik_pos_solver, self.chain = self.setup_kinematics(chain)
        self.q0 = [0.2, 0.2, -2.5, 0.4, 0.5, 0.6]

    def load_urdf_to_kdl(self, urdf_file_path: str):
        """
        从URDF文件加载KDL树结构
        
        参数:
            urdf_file_path: URDF文件的路径
        
        返回:
            success: 加载是否成功
            tree: KDL树对象
        """
    
        if not os.path.exists(urdf_file_path):
            print(f"urdf not exists - {urdf_file_path}")
            return False, None
        
        try:
            success, tree = treeFromFile(urdf_file_path)
            if success:
                print(f"load sucess: {urdf_file_path}")
                print(f"chain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints")
                return success, tree
        except Exception as e:
            print(f"methods1 failed: {e}")
        
        # 使用二进制模式读取文件并移除编码声明
        try:
            with open(urdf_file_path, 'rb') as f:
                # 读取二进制数据
                xml_content = f.read()
                # 尝试解码为字符串
                try:
                    xml_str = xml_content.decode('utf-8')
                except UnicodeDecodeError:
                    # 尝试其他编码
                    xml_str = xml_content.decode('latin-1')
                # 移除XML编码声明
                if xml_str.startswith('<?xml'):
                    xml_str = xml_str[xml_str.find('?>') + 2:]
                # 从字符串创建URDF模型
                robot_model = URDF.from_xml_string(xml_str)
                # 从URDF模型创建KDL树
                success, tree = treeFromUrdfModel(robot_model)
                if success:
                    print(f"method2 sucess: {urdf_file_path}")
                    print(f"chain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints")
                    return success, tree
                else:
                    print("method2 failed")
        except Exception as e:
            print(f"method2 failed: {e}")
        
        try:
            robot_model = URDF.from_xml_file(urdf_file_path)
            success, tree = treeFromUrdfModel(robot_model)
            
            if success:
                print(f"method3 sucess: {urdf_file_path}")
                print(f"chain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints")
                return success, tree
            else:
                print("method3 failed")
        except Exception as e:
            print(f"method3 failed: {e}")
        
        print(f"can not load: {urdf_file_path}")
        return False, None

    def create_chain_from_tree(self, tree, base_link, tip_link):
        """
        从KDL树创建运动链
        
        参数:
            tree: KDL树对象
            base_link: 基链接名称
            tip_link: 末端执行器链接名称
        
        返回:
            chain: KDL链对象
        """
        try:
            chain = tree.getChain(base_link, tip_link)
            print(f"create chain from {base_link} to {tip_link}")
            print(f"chain has {chain.getNrOfSegments()} segs and {chain.getNrOfJoints()} joints")
            return chain
        except Exception as e:
            print(f"create chain failed: {e}")
            return None

    def setup_kinematics(self, chain):
        """
        设置正逆运动学求解器
        
        参数:
            chain: KDL链对象
        
        返回:
            fk_solver: 正向运动学求解器
            ik_vel_solver: 逆速度运动学求解器
            ik_pos_solver: 逆位置运动学求解器
            chain: 运动链对象 (添加此返回值以便在其他函数中使用)
        """
        fk_solver = PyKDL.ChainFkSolverPos_recursive(chain)
        ik_vel_solver = PyKDL.ChainIkSolverVel_pinv(chain)
        max_iterations = 5000
        eps = 3*1e-2
        ik_pos_solver = PyKDL.ChainIkSolverPos_NR(
            chain, fk_solver, ik_vel_solver, max_iterations, eps
        )
        
        return fk_solver, ik_vel_solver, ik_pos_solver, chain


    def perform_forward_kinematics(self, fk_solver, joint_positions):
        """
        执行正向运动学计算
        
        参数:
            fk_solver: 正向运动学求解器
            joint_positions: 关节位置列表
        
        返回:
            frame: KDL Frame对象，表示末端执行器的位姿
        """
        q = PyKDL.JntArray(len(joint_positions))
        for i in range(len(joint_positions)):
            q[i] = joint_positions[i]
        
        frame = PyKDL.Frame()
        status = fk_solver.JntToCart(q, frame)
        
        if status >= 0:
            position = [frame.p.x(), frame.p.y(), frame.p.z()]
            rotation = frame.M.GetQuaternion() 
            # print(f"fk sucess:")
            # print(f"pos: {position}")
            # print(f"quat: {rotation}")
            return position, rotation, frame
        else:
            # print("fk failed")
            return None

    def fkine(self, q) -> SE3:
        """
        使用KDL正向运动学求解器，输入关节角q，输出SE3位姿
        """
        if not hasattr(self, 'fk_solver'):
            raise RuntimeError('KDL求解器未初始化，请先调用setup_kdl_kinematics')
        position, quat, frame = self.perform_forward_kinematics(self.fk_solver, q)
        if position is None:
            return SE3()
        # KDL四元数为(x, y, z, w)，spatialmath为(w, x, y, z)
        from spatialmath import UnitQuaternion
        rot = UnitQuaternion([quat[3], quat[0], quat[1], quat[2]]).R
        se3_pose = SE3.Rt(rot, position)
        return se3_pose

    def perform_inverse_kinematics(self, ik_pos_solver, chain, target_frame, initial_joints=None):
        """
        执行逆运动学计算
        """
        num_joints = chain.getNrOfJoints()
        if initial_joints is None:
            q_init = PyKDL.JntArray(num_joints)
        else:
            q_init = PyKDL.JntArray(num_joints)
            joints = list(initial_joints)[:num_joints]
            if len(joints) < num_joints:
                joints += [0.0] * (num_joints - len(joints))
            for i in range(num_joints):
                q_init[i] = joints[i]
        q_out = PyKDL.JntArray(num_joints)
        status = ik_pos_solver.CartToJnt(q_init, target_frame, q_out)
        if status >= 0:
            joint_positions = [q_out[i] for i in range(num_joints)]
            # 角度归一化到[-pi, pi]
            joint_positions = [((x + np.pi) % (2 * np.pi)) - np.pi for x in joint_positions]
            # print(f"ik sucess:")
            # print(f"Joints: {joint_positions}")
            return joint_positions
        else:
            print("ik failed")
            return None

    def ikine(self, Tep: SE3) -> np.ndarray:
        """
        使用KDL逆运动学求解器，输入为SE3，输出为关节角度数组（与原格式一致）
        补偿机械臂末端和夹爪末端的差异
        """
        # print('##################',self._base)
        # T06 = self._base.inv() * Tep# * self._tool.inv()
        T06 = Tep * self._tool.inv()
        # T06 = Tep * self._tool.inv()
        pos = T06.t
        rot = T06.R
        # PyKDL.Rotation() 可直接接受9个元素
        frame = PyKDL.Frame(
            PyKDL.Rotation(
                rot[0,0], rot[0,1], rot[0,2],
                rot[1,0], rot[1,1], rot[1,2],
                rot[2,0], rot[2,1], rot[2,2]
            ),
            PyKDL.Vector(pos[0], pos[1], pos[2])
        )
        if not hasattr(self, 'ik_pos_solver'):
            raise RuntimeError('KDL求解器未初始化，请先调用setup_kdl_kinematics')
        joints = self.perform_inverse_kinematics(self.ik_pos_solver, self.chain, frame, self.q0)
        if joints is None:
            return np.array([])
        return np.array(joints)
    

    def move_cartesian(self, T: SE3):
        """
        移动机器人到指定的笛卡尔空间位姿。
        参数:
        T: 目标位姿，表示为SE3对象
        异常:
        如果逆运动学求解失败，将抛出断言异常。
        """
        q = self.ikine(T)
        if len(q) == 0:
            import traceback
            print("[IK FAILED] move_cartesian: SE3=", T)
            # print(self.q0)
            traceback.print_stack()
        assert len(q), "inverse kinematics failure"
        self.q0 = q[:]
    

##剩余类函数不变

4.main函数修改

根据KDL，可以根据每一个移动步骤的目标位置姿态逆解出角度q
 

        Two_in_base = T_wb.inv() * T_wo#T_wb在world下的旋转矩阵
        new_t = Two_in_base.t.copy()
        new_t[2] += 0.2
        T2_base = sm.SE3.Rt(Two_in_base.R, new_t)
        # 用平移后的T2_base_offset做逆解
        self.q2 = robot.ikine(T2_base)
        if isinstance(self.q2, tuple):
            self.q2 = self.q2[0]
    

        time2 = 2
        parameter2 = JointParameter(self.q1, self.q2)
        velocity_parameter2 = QuinticVelocityParameter(time2)
        trajectory_parameter2 = TrajectoryParameter(parameter2, velocity_parameter2)
        planner2 = TrajectoryPlanner(trajectory_parameter2)
        # 执行planner_array = [planner2]
        time_array = [0.0, time2]
        planner_array = [planner2]
        total_time = np.sum(time_array)
        time_step_num = round(total_time / 0.002) + 1
        times = np.linspace(0.0, total_time, time_step_num)
        time_cumsum = np.cumsum(time_array)
        for timei in times:
            for j in range(len(time_cumsum)):
                if timei == 0.0:
                    break
                if timei <= time_cumsum[j]:
                    planner_interpolate = planner_array[j - 1].interpolate(timei - time_cumsum[j - 1])
                    if isinstance(planner_interpolate, np.ndarray):
                        joint = planner_interpolate
                        robot.move_joint(joint)
                    else:
                        robot.move_cartesian(planner_interpolate)
                        joint = robot.get_joint()
                    action[:6] = joint
                    env.step(action)
                    break