C#调用Python封装的类

我才是汞

850人浏览 · 2025-09-27 18:09:54

我才是汞 · 2025-09-27 18:09:54 发布

由于要计算机械臂动力学的各个矩阵，老是自己手搓又不太行，就试着用Pinocchio库来算。但是Pinocchio的C++版本在Win环境下部署非常麻烦，而且后续机械臂在工控机的win系统下实际运行还是用的C#，就想着用Python版本的Pinocchio然后自己封装一下给C#调用。

1 Pinocchio Python版本安装

官网上给了两种方法，一个是pip：

python -m pip install pin

另一个是conda：

 conda install pinocchio -c conda-forge

我这边试下来发现Ubuntu上用pip没问题，在win上用pip会卡在安装依赖上，一直卡着，也不是网络问题。然后用conda安装就没事了，很顺利。

2 Pinocchio的二次封装

安装好后就是根据自己需求对Pinocchio自带的函数二次封装一下，形成一个自己的类，然后后续调用就行了。需要注意的问题主要是Win环境下Pinocchio读取URDF时，URDF文件的路径不能有任何中文(Ubuntu没有这个问题)。另外，有些URDF文件内部包含了Mesh文件的路径，这个路径能不能有中文我还没实际测试过，我的URDF文件里面没包含Mesh文件。

我的需求是用Pinocchio算机器人动力学方程里的M、C矩阵与G向量，也就是惯性矩阵、科里奥利矩阵与重力向量，相关封装的代码如下，URDF文件的加载是在实例化robot类时完成的。

import pinocchio as pin
import numpy as np
from pathlib import Path
import os

def angles_to_pinocchio(q):
    """
    将关节角向量 q (长度为n) 转换为 Pinocchio 接收的 [cos(q1), sin(q1), ..., cos(qn), sin(qn)] (长度为2n)
    """
    q = np.asarray(q)
    cos_vals = np.cos(q)
    sin_vals = np.sin(q)
    q_pinocchio = np.empty(2 * len(q))
    q_pinocchio[0::2] = cos_vals
    q_pinocchio[1::2] = sin_vals
    return q_pinocchio


class robot:
    """
    封装了 Pinocchio 的机器人模型和常用动力学计算。
    """

    def __init__(self, urdf_path_str: str):
        """
        初始化机器人模型和数据结构。
        """
        urdf_path = Path(urdf_path_str)
        if not urdf_path.exists():
            # 使用 os.path.abspath 可以帮助调试路径问题
            raise FileNotFoundError(f"URDF 文件未找到: {os.path.abspath(urdf_path_str)}")

        # 实例化时执行 model = pin.buildModelFromUrdf(urdf_path) 与 data = model.createData()
        self.model = pin.buildModelFromUrdf(urdf_path)
        self.data = self.model.createData()
        self.nv = self.model.nv  # 广义速度维度

    def crba(self, q: list) -> np.ndarray:
        """
        计算机器人的质量矩阵 (M)。
        """
        q_pin = angles_to_pinocchio(q)
        M = pin.crba(self.model, self.data, q_pin)
        return M

    def computeGeneralizedGravity(self, q: list) -> np.ndarray:
        """
        计算广义重力向量 (g)。
        """
        q_pin = angles_to_pinocchio(q)
        g = pin.computeGeneralizedGravity(self.model, self.data, q_pin)
        return g.reshape(-1)   # 确保返回一维数组

    def computeCoriolisMatrix(self, q: list, v: list) -> np.ndarray:
        """
        计算科里奥利矩阵 (C)。
        """
        q_pin = angles_to_pinocchio(q)
        v_np = np.asarray(v).reshape(-1)

        if len(v_np) != self.nv:
            raise ValueError(f"速度向量 v 的维度必须是 {self.nv}, 实际是 {len(v_np)}。")

        C = pin.computeCoriolisMatrix(self.model, self.data, q_pin, v_np)
        return C

3 C#调用Python封装的类

Python脚本写好了就行了，不需要编译运行之类的操作。

C#使用Python脚本或者混合编程有很多工具可以用，pythonnet是用得比较多的，看NuGet包管理器的下载数量有180万。引用这个包可以用Visual Studio通过NuGet包管理器搜索pythonnet来安装，也可以先pip install pythonnet之后，去对应的环境中找到Python.Runtime.dll，然后在C#项目管理中手动引用这个DLL，如图。

随后是C#代码，写的时候ChatGPT额度满了，让Gemini写的，效果好像比较差，自己调试完后得到能运行的代码如下，我的robot_wrapper.py在【D:\pypinocchiotest】路径下。

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics.LinearAlgebra;
using System.Threading;
using System.Runtime.InteropServices;
using System.IO;
using System.Runtime.CompilerServices;
using System.Numerics;
using Python.Runtime;


namespace ConsoleAppTest
{
	internal class Program
	{
        private const string RobotWrapperPath = @"D:\BaiduNetdisk"; // robot_wrapper.py 所在目录
        //事实上这个路径并不重要，随便改成别的也没事
        private const string UrdfFilePath = @"D:\pypinocchiotest\URDF\SLDASM\urdf\SLDASM.urdf"; // 你的 URDF 路径

        // 请根据您的 Python 安装路径进行修改
        private const string PythonHomePath = @"D:\Anaconda3\envs\pypin\python.exe";
        private const string PythonDllPath = @"D:\Anaconda3\envs\pypin\python312.dll";
		private const string PythonPath = @"D:\Anaconda3\envs\pypin;D:\Anaconda3\envs\pypin\Lib;D:\Anaconda3\envs\pypin\Lib\site-packages;D:\Anaconda3\envs\pypin\DLLs;D:\pypinocchiotest";
		//private const string PythonPath = @"D:\Anaconda3\envs\pypin;D:\Anaconda3\envs\pypin\Lib;D:\Anaconda3\envs\pypin\Lib\site-packages;D:\Anaconda3\envs\pypin\DLLs";
		//关键在于把所有的路径包含进PythonPath，而且不能换行
        
		// 实例化后的 Python 机器人对象
		private static dynamic myRobot;
		private static dynamic np;
        public static void InitializePython()
        {
            Console.WriteLine("--- 1. 初始化 Python 环境 ---");
            try
            {
                // 1. 设置 Python DLL 路径
                Runtime.PythonDLL = PythonDllPath;

                // 2. 设置 Python Home 目录
                PythonEngine.PythonHome = PythonHomePath;
				PythonEngine.PythonPath = PythonPath;

				// 3. 设置 PYTHONPATH，确保能找到 robot_wrapper.py
				//Environment.SetEnvironmentVariable("PYTHONPATH", RobotWrapperPath, EnvironmentVariableTarget.Process);
                //这一步设置环境变量貌似也是不需要的

                // 4. 启动 Python 解释器
                PythonEngine.Initialize();

                Console.WriteLine($"Python version initialized: {PythonEngine.Version}");
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Python 初始化失败！请检查 PythonDllPath 和 PythonHomePath 是否正确。错误: {ex.Message}");
                throw;
            }
        }
        public static void LoadRobot()
        {
            Console.WriteLine("--- 2. 加载 Python 模块和类 ---");
            // 必须在 GIL (Global Interpreter Lock) 块内执行所有 Python 调用
            using (Py.GIL())
            {
                try
                {
					dynamic robot_wrapper = Py.Import("robot_wrapper");
					//之前import test能成功是因为正好有一个模块就叫test，路径在
                    //D:\Anaconda3\envs\pypin\Lib\site-packages\setuptools\command\test.py
					np = Py.Import("numpy");

                    // 获取 robot 类
                    dynamic robot_class = robot_wrapper.robot;

                    // 实例化 robot 类，传入 URDF 路径
                    myRobot = robot_class(UrdfFilePath);

                    Console.WriteLine($"Robot model loaded. nv = {myRobot.nv}");
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"加载机器人失败。请检查 PYTHONPATH 和 URDF 路径。错误: {ex}");
                }
            }
        }
        public static Matrix<double> CallCrba(List<double> q)
        {
            using (Py.GIL())
            {
                // 将 C# 列表转换为 Python 列表 (Pinocchio/Numpy 能够处理)
                // Pinocchio 的函数返回 np.ndarray，Python.NET 可以将其转为 C# double[,]
                dynamic M_py = myRobot.crba(q.ToPython());
                Matrix<double> M = Matrix<double>.Build.DenseOfArray(new double[6,6]);
                for (int i = 0; i < 6; i++)
                {
                    M.SetColumn(i, Vector<double>.Build.DenseOfArray(M_py[i].As<double[]>()));
                }
                
                return M;
            }
        }

        public static double[] CallComputeGeneralizedGravity(List<double> q)
        {
            using (Py.GIL())
            {
                dynamic g_py = myRobot.computeGeneralizedGravity(q.ToPython());
                // 返回的 g 向量是 1D numpy 数组，转为 C# double[]
                return g_py.As<double[]>();
            }
        }

        public static Matrix<double> CallComputeCoriolisMatrix(List<double> q, List<double> v)
        {
            using (Py.GIL())
            {
                dynamic C_py = myRobot.computeCoriolisMatrix(q.ToPython(), v.ToPython());
				// C 矩阵是 2D numpy 数组，转为 C# double[,]
				Matrix<double> C = Matrix<double>.Build.DenseOfArray(new double[6, 6]);
				for (int i = 0; i < 6; i++)
				{
					C.SetColumn(i, Vector<double>.Build.DenseOfArray(C_py[i].As<double[]>()));
				}

				return C;
            }
        }


        public static void Main(string[] args)
        {
            // 1. 初始化 Python 解释器
            InitializePython();

            // 2. 加载机器人模型
            LoadRobot();

            if (myRobot != null)
            {
                // 获取机器人自由度
                int nv = (int)myRobot.nv;

                // 示例输入
                List<double> q_test = Enumerable.Repeat(Math.PI / 4.0, nv).ToList();
                List<double> v_test = Enumerable.Repeat(0.1, nv).ToList();

                // 3. 执行调用
                Console.WriteLine("--- 3. 调用机器人动力学函数 ---");

                int n = 10000;
                Console.WriteLine("执行"+n+"次调用:");
                HighPrecisionTimer htimer = new HighPrecisionTimer();
                Random rand=new Random();
				var q_rand = Vector<double>.Build.DenseOfArray(new double[6]);
				var v_rand = Vector<double>.Build.DenseOfArray(new double[6]);
				htimer.Reset();
                for (int i = 0; i < n; i++)
                {
					for (int j=0;j<6;j++)
                    {
                        q_rand[j] = rand.NextDouble() * 2 * Math.PI;
                        v_rand[j] = rand.NextDouble();
					}

					// CRBA
					var M_matrix = CallCrba(q_rand.ToList<double>());
                    // 重力
                    var g_vector = CallComputeGeneralizedGravity(q_rand.ToList<double>());
                    // 科里奥利矩阵
                    var C_matrix = CallComputeCoriolisMatrix(q_rand.ToList<double>(), v_rand.ToList<double>());
                }
                Console.WriteLine("花费时间：" + htimer.ElapsedSeconds() + "秒");
            }

            // 4. 关闭解释器
            Console.WriteLine("--- 4. 关闭 Python 解释器 ---");
            PythonEngine.Shutdown();
        }
    }
}

需要注意里面的一些地方。一个是Gemini默认生成的代码是：

private const string RobotWrapperPath = @"D:\pypinocchiotest";
private const string PythonPath = @"D:\Anaconda3\envs\pypin;D:\Anaconda3\envs\pypin\Lib;D:\Anaconda3\envs\pypin\Lib\site-packages;D:\Anaconda3\envs\pypin\DLLs";
Environment.SetEnvironmentVariable("PYTHONPATH", RobotWrapperPath, EnvironmentVariableTarget.Process);

然后我发现这样不行，一定要在PythonPath里面带上D:\pypinocchiotest这个路径，然后Environment.SetEnvironmentVariable("PYTHONPATH", RobotWrapperPath, EnvironmentVariableTarget.Process);这一步似乎是不需要的。问了下ChatGPT，得到的答复是：

总的来说大概就是PythonPath一定要全。

最蛋疼的点是，我在用之前说的Gemini生成的默认代码时，C#报错说无法import我的robot_wrapper类。我同路径下还有一个test.py，然后我当时把dynamic robot_wrapper = Py.Import("robot_wrapper")里的robot_wrapper改成test就能正常import，但即使test.py与robot_wrapper.py内容一致，改成test也只是能import，后续 dynamic robot_class = robot_wrapper.robot时就还是报错。

当时这给我弄懵逼了，后来才发现虚拟环境里面也有一个test.py(D:\Anaconda3\envs\pypin\Lib\site-packages\setuptools\command)，然后被PythonPath包含进去了，import的是这个test.py而不是我自己写的那个。

4 测试

测试下来能正常调用，运行结果如图。

运行速度比我想象的快，算一轮M、C、G的平均时间小于100微秒，还是挺可用的。

后续的问题就是如何将Python里的变量转换为C#友好型变量，例如numpy的数组转成double[,]之类的。问了下ChatGPT，说最方便的方案就是“搞清楚Python会返回什么样的变量”，然后自己手动转，例如这个函数：

    def crba(self, q: list) -> np.ndarray:
        """
        计算机器人的质量矩阵 (M)。
        """
        q_pin = angles_to_pinocchio(q)
        M = pin.crba(self.model, self.data, q_pin)
        return M

它返回的M矩阵，在机械臂具有6个轴的情况下就是6x6的矩阵，在C#里面类似于一个double[][]，所以你可以直接double[] a = M_py[0]，这样得到的a是一个长度为6的double数组。

如果不清楚Python会返回啥样的东西，那么要保证能够稳定的转换似乎还是挺困难的。