Twincat3与c++通过ADS通信，实现电机力矩控制

weixin_45832444

1548人浏览 · 2025-01-26 11:44:07

weixin_45832444 · 2025-01-26 11:44:07 发布

前言

主要介绍一下简单使用c++实现前馈控制，然后通过twincat3实现对机械臂电机的控制。写的不是很全面，把我遇到的困难大概写了一下，希望对你有帮助。

开发环境准备

首先安装vs，Visual Studio 2022 IDE - 适用于软件开发人员的编程工具，从官网的安装下载即可。安装过程中务必勾选C++编译器，否则将无法使用twincat3的C++功能。

然后根据官网安装twincat3，和之前安装 full 版本的唯一区别是在安装过程中多了一个选项，TC3 软件会自动识别你当前电脑所安装的 VS 版本，之后勾选 Twincat3 的 runtime 嵌入到哪一个 VS 中，其他步骤可以参考：

https://tr.beckhoff.com.cn/pluginfile.php/80808/mod_resource/content/1/TwinCAT3_C%2B%2B_Simulink%E6%95%99%E7%A8%8BV3.11.pdf

首先启动Visual Studio，新建一个项目。点击窗口左上方菜单栏的文件->新建->项目。出现窗口如下图所示。选择Visual C++中的Windows控制台应用程序选项。输入项目名称并选择为解决方案创建目录。点击确定。

此时需要添加链接库，以便实现通讯中用到的各种函数的直接调用。在左侧找到所建立的项目处，点击右键->属性。

选择配置属性->C/C++->常规->附加库目录->编辑。如下图所示，添加的程序的所在目录，点击确定。

选择配置属性->C/C++->预处理器->预处理器定义->编辑，如下图所示。

选择配置属性->链接器->常规->附加库目录->编辑，进入附加包含目录的编辑界面。如下图所示，添加的程序的所在目录，点击确定。（本路径是以安装TwinCAT3时的默认路径配置的）

再选择链接器－>输入－>附加依赖项，点击右边的按钮选择编辑进入附加依赖项界面。如下图所示添加的.lib文件的文件名（TcAdsDll.lib），点击确定。再次点击确定，链接库添加完成。

准备工作就完成了。

测试电机力矩模式

我用的是一个erob电机，可以通过力矩控制，先用一个电机进行测试，将电机和工控机连接，打开打开Twincat3软件进行扫描设备设别电机

电机扫描后需要在AXIS1中需改分辨率都一些参数

根据自己用的电机修改其他参数，将一些限制关掉

新建项目进行编程，其中一些模块的引用可以查官网文档

PROGRAM MAIN
VAR
   MCPower : MC_POWER; //使能模块
   moder_of_operation AT %Q* : SINT;//操作模式转换
   position AT %Q* : DINT;
target_Torque AT %Q* : INT; // 目标扭矩 (%)
   t: LREAL ;
   i: INT;
END_VAR

begin := TRUE;
moder_of_operation := 10;
MCPower(Enable:=begin,
Enable_Positive:= begin,
Enable_Negative:= begin,
Override:=100,
Axis:= Joint);
target_Torque := (5 + 3*SIN(PI*t))*2;
t := t +0.00001;

我用的电机操作字8是位置控制，9是速度控制，10是力矩控制。将操作的状态控制与电机相连接。

运行程序测试，就可以看到，电机的力矩随着给定的变换而变化。

Twincat3编程

打开Twincat3软件，我这里是用到了六个关节的控制，因此我在这里简单写一下六个关节的编程，以实现c++通过ADS与Twincat3通信，获得c++计算反馈到的力矩值。

PROGRAM MAIN
VAR
   dyn : Dynamics;//定义的参数类型
   initSuccess_2 : BOOL;//判断是否为第二关节，如果是第二关节，就调用第二关节的参数，需要自己辨识出来，这里不多写了
   inertia_matrices_2 : ARRAY [1..6,1..3,1..3] OF LREAL;//惯性参数
   comResult_2 : ARRAY [1..6] OF Vector3D;//质心参数
   masses_2 : ARRAY [1..6] OF LREAL;//质量参数
   initSuccess_3 : BOOL;
   inertia_matrices_3 : ARRAY [1..6,1..3,1..3] OF LREAL;
   comResult_3 : ARRAY [1..6] OF Vector3D;
   masses_3 : ARRAY [1..6] OF LREAL;
   initSuccess_4 : BOOL;
   inertia_matrices_4 : ARRAY [1..6,1..3,1..3] OF LREAL;
   comResult_4 : ARRAY [1..6] OF Vector3D;
   masses_4 : ARRAY [1..6] OF LREAL;
   initSuccess_5 : BOOL;
   inertia_matrices_5 : ARRAY [1..6,1..3,1..3] OF LREAL;
   comResult_5 : ARRAY [1..6] OF Vector3D;
   masses_5 : ARRAY [1..6] OF LREAL;
   initSuccess_6 : BOOL;
   inertia_matrices_6 : ARRAY [1..6,1..3,1..3] OF LREAL;
   comResult_6 : ARRAY [1..6] OF Vector3D;
   masses_6 : ARRAY [1..6] OF LREAL;
   i : INT;
   t : LREAL := 0.0 ;
   tao :ARRAY[1..6] OF LREAL;//力矩信息，用于接收c++计算得到的数据

   positions : ARRAY [1..6] OF LREAL;
   velocities : ARRAY [1..6] OF LREAL;
   accelerations : ARRAY [1..6] OF LREAL;
   force_2 :UINT;
   force_3 :UINT;
   force_4 :UINT;


END_VAR

//简单写一下控制的轨迹，速度和加速度

positions[1] := 0.0;
positions[2] := 0.5*SIN(0.5* pi * t);
positions[3] := 0.5*COS(0.5* pi * t);
positions[4] := -positions[3];
positions[5] := positions[3];
positions[6] := 0.0;
velocities[1] := 0.0;
velocities[2] := 0.025 * COS(0.5* pi * t);
velocities[3] := -0.025*SIN(0.5* pi * t);
velocities[4] := -velocities[3];
velocities[5] := velocities[3];
velocities[6] := 0.0;
accelerations[1] := 0.0;
accelerations[2] := -0.5*0.025 * SIN(0.5* pi * t);
accelerations[3] := -0.5*0.025 * COS(0.5* pi * t);
accelerations[4] := -accelerations[3];
accelerations[5] := accelerations[3];
accelerations[6] := 0.0;
t := t + 0.01;
initSuccess_2 := InitializeParameters(2, dyn := dyn);
IF initSuccess_2 THEN
   inertia_matrices_2 := dyn.inertia_matrices;
   comResult_2 := dyn.center_of_mass;
   masses_2 := dyn.masses;
END_IF
initSuccess_3 := InitializeParameters(3, dyn := dyn);
IF initSuccess_3 THEN
   inertia_matrices_3 := dyn.inertia_matrices;
   comResult_3 := dyn.center_of_mass;
   masses_3 := dyn.masses;
END_IF
initSuccess_4 := InitializeParameters(4, dyn := dyn);
IF initSuccess_4 THEN
   inertia_matrices_4 := dyn.inertia_matrices;
   comResult_4 := dyn.center_of_mass;
   masses_4 := dyn.masses;
END_IF
initSuccess_5 := InitializeParameters(5, dyn := dyn);
IF initSuccess_5 THEN
   inertia_matrices_5 := dyn.inertia_matrices;
   comResult_5 := dyn.center_of_mass;
   masses_5 := dyn.masses;
END_IF
initSuccess_6 := InitializeParameters(6, dyn := dyn);
IF initSuccess_6 THEN
   inertia_matrices_6 := dyn.inertia_matrices;
   comResult_6 := dyn.center_of_mass;
   masses_6 := dyn.masses;
END_IF
force_2 := TO_UINT(tao[2]*10);
force_3 := TO_UINT(tao[3]*10);
force_4 := TO_UINT(tao[6]*10);

VS编程

// ADS_Server.cpp
#include <windows.h>
#include "C:\TwinCAT\ADSApi\TcAdsDll\Include\TcAdsDef.h" // 结构体和常量的声明
#include "C:\TwinCAT\ADSApi\TcAdsDll\Include\TcAdsAPI.h" // ADS函数的声明
#include "C:\Users\Twincat3\Desktop\c\CompensationLibrary\eigen\Eigen\Dense"
#include <time.h>
#include <iostream>
#include <thread>
#include <chrono>
#include "compensation.h" // 导入DLL接口
#pragma comment(lib,"ws2_32.lib")
#pragma comment(lib,"TcAdsDll.lib") // 链接 TwinCAT ADS 库
#pragma comment(lib,"CompensationLibrary.lib") // 链接 Compensation DLL 的导入库
using namespace std;

int main() {
	long      nErr, nPort;	    // 定义端口变量
	AmsAddr   Addr;		        // 定义AMS地址变量
	PAmsAddr  pAddr = &Addr;	    // 定义端口地址变量

	// 打开ADS端口
	nPort = AdsPortOpen();
	if (nPort < 0) {
		cerr << "Failed to open ADS port.\n";
		return -1;
	}

	// 获取本地地址
	nErr = AdsGetLocalAddress(pAddr); // 自动获取本地地址
	if (nErr < 0) {
		cerr << "Failed to AdsGetLocalAddress. Error: " << nErr << '\n';
		AdsPortClose();
		return -1;
	}

	pAddr->port = 851;			// TwinCAT 3 的通讯端口为851
	int         nIndex, i, j;	// 定义循环变量

	unsigned long lHdlVar100; // Handle for positions
	unsigned long lHdlVar200; // Handle for velocities
	unsigned long lHdlVar300; // Handle for accelerations
	unsigned long lHdlVar400; // Handle for TAO
	double		positions[6];	    // 定义数组float
	double		velocities[6];
	double		accelerations[6];
	double		tao[6];
	char p[] = "MAIN.positions";
	char v[] = "MAIN.velocities";
	char a[] = "MAIN.accelerations";
	char T[] = "MAIN.tao";


	// 关节二
	unsigned long lHdlVar2;   	// Handle for masses_2
	unsigned long lHdlVar3;      // Handle for center_of_mass_2
	unsigned long lHdlVar4;      // Handle for inertia_matrices_2
	double		masses_2[6];	    // 定义数组float
	double		comResult_2[6][3];
	double		inertia_matrices_2[6][3][3];
	char szVar2[] = "MAIN.masses_2";
	char cmass_2[] = "MAIN.comResult_2";
	char im_2[] = "MAIN.inertia_matrices_2";
	// 关节三
	unsigned long lHdlVar5;   	// Handle for masses_2
	unsigned long lHdlVar6;      // Handle for center_of_mass_2
	unsigned long lHdlVar7;      // Handle for inertia_matrices_2
	double		masses_3[6];	    // 定义数组float
	double		comResult_3[6][3];
	double		inertia_matrices_3[6][3][3];
	char szVar3[] = "MAIN.masses_3";
	char cmass_3[] = "MAIN.comResult_3";
	char im_3[] = "MAIN.inertia_matrices_3";
	// 关节四
	unsigned long lHdlVar8;   	// Handle for masses_2
	unsigned long lHdlVar9;      // Handle for center_of_mass_2
	unsigned long lHdlVar10;      // Handle for inertia_matrices_2
	double		masses_4[6];	    // 定义数组float
	double		comResult_4[6][3];
	double		inertia_matrices_4[6][3][3];
	char szVar4[] = "MAIN.masses_4";
	char cmass_4[] = "MAIN.comResult_4";
	char im_4[] = "MAIN.inertia_matrices_4";
	// 关节五
	unsigned long lHdlVar11;   	// Handle for masses_2
	unsigned long lHdlVar12;      // Handle for center_of_mass_2
	unsigned long lHdlVar13;      // Handle for inertia_matrices_2
	double		masses_5[6];	    // 定义数组float
	double		comResult_5[6][3];
	double		inertia_matrices_5[6][3][3];
	char szVar5[] = "MAIN.masses_5";
	char cmass_5[] = "MAIN.comResult_5";
	char im_5[] = "MAIN.inertia_matrices_5";
	// 关节六
	unsigned long lHdlVar14;   	// Handle for masses_2
	unsigned long lHdlVar15;      // Handle for center_of_mass_2
	unsigned long lHdlVar16;      // Handle for inertia_matrices_2
	double		masses_6[6];	    // 定义数组float
	double		comResult_6[6][3];
	double		inertia_matrices_6[6][3][3];
	char szVar6[] = "MAIN.masses_6";
	char cmass_6[] = "MAIN.comResult_6";
	char im_6[] = "MAIN.inertia_matrices_6";

	do {
		//positions
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar100), &lHdlVar100, sizeof(p), p);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for masses_2: " << nErr << '\n';
		}
		else {
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar100, sizeof(positions), &positions[0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for masses_2: " << nErr << '\n';
			}
			else {
				for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
					cout << "positions[" << nIndex << "]: " << positions[nIndex] << '\n';
			}
		}
		//velocities
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar200), &lHdlVar200, sizeof(v), v);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for masses_2: " << nErr << '\n';
		}
		else {
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar200, sizeof(velocities), &velocities[0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for masses_2: " << nErr << '\n';
			}
			else {
				for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
					cout << "velocities[" << nIndex << "]: " << velocities[nIndex] << '\n';
			}
		}
		//accelerations
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar300), &lHdlVar300, sizeof(a), a);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for masses_2: " << nErr << '\n';
		}
		else {
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar300, sizeof(accelerations), &accelerations[0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for masses_2: " << nErr << '\n';
			}
			else {
				for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
					cout << "accelerations[" << nIndex << "]: " << accelerations[nIndex] << '\n';
			}
		}


		// 读取 masses_2
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar2), &lHdlVar2, sizeof(szVar2), szVar2);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for masses_2: " << nErr << '\n';
		}
		else {
			// 读取 masses_2 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar2, sizeof(masses_2), &masses_2[0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for masses_2: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//		cout << "masses_2[" << nIndex << "]: " << masses_2[nIndex] << '\n';
			//}
		}
		// 读取comResult_2
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar3), &lHdlVar3, sizeof(cmass_2), cmass_2);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for center_of_mass_2: " << nErr << '\n';
		}
		else {
			// 读取 center_of_mass_2 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar3, sizeof(comResult_2), &comResult_2[0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for center_of_mass_2: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			cout << "comResult_2[" << nIndex << "][" << i << "]: " << comResult_2[nIndex][i] << '\n';
			//		}
			//	}
			//}
		}
		// 读取 inertia_matrices_2
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar4), &lHdlVar4, sizeof(im_2), im_2);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for inertia_matrices_2: " << nErr << '\n';
		}
		else {
			// 读取 inertia_matrices_2 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar4, sizeof(inertia_matrices_2), &inertia_matrices_2[0][0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for inertia_matrices_2: " << nErr << '\n';
			}
				//else {
				//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
				//	{
				//		for (i = 0; i < 3; i++)
				//		{
				//			for (j = 0; j < 3; j++)
				//			{
				//				cout << "inertia_matrices_2[" << nIndex << "][" << i << "][" << j << "]: " << inertia_matrices_2[nIndex][i][j] << '\n';
				//			}
				//		}
				//	}
				//}
		}


		// 读取 masses_3
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar5), &lHdlVar5, sizeof(szVar3), szVar3);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for masses_3: " << nErr << '\n';
		}
		else {
			// 读取 masses_3 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar5, sizeof(masses_3), &masses_3[0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for masses_3: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//		cout << "masses_3[" << nIndex << "]: " << masses_3[nIndex] << '\n';
			//}
		}
		// 读取comResult_3
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar6), &lHdlVar6, sizeof(cmass_3), cmass_3);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for center_of_mass_3: " << nErr << '\n';
		}
		else {
			// 读取 center_of_mass_3 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar6, sizeof(comResult_3), &comResult_3[0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for center_of_mass_3: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			cout << "comResult_3[" << nIndex << "][" << i << "]: " << comResult_3[nIndex][i] << '\n';
			//		}
			//	}
			//}
		}
		// 读取 inertia_matrices_3
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar7), &lHdlVar7, sizeof(im_3), im_3);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for inertia_matrices_3: " << nErr << '\n';
		}
		else {
			// 读取 inertia_matrices_2 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar7, sizeof(inertia_matrices_3), &inertia_matrices_3[0][0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for inertia_matrices_2: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			for (j = 0; j < 3; j++)
			//			{
			//				cout << "inertia_matrices_3[" << nIndex << "][" << i << "][" << j << "]: " << inertia_matrices_3[nIndex][i][j] << '\n';
			//			}
			//		}
			//	}
			//}
		}




		// 读取 masses_4
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar8), &lHdlVar8, sizeof(szVar4), szVar4);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for masses_4: " << nErr << '\n';
		}
		else {
			// 读取 masses_4 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar8, sizeof(masses_4), &masses_4[0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for masses_4: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//		cout << "masses_4[" << nIndex << "]: " << masses_4[nIndex] << '\n';
			//}
		}
		// 读取comResult_4
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar9), &lHdlVar9, sizeof(cmass_4), cmass_4);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for center_of_mass_4: " << nErr << '\n';
		}
		else {
			// 读取 center_of_mass_4 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar9, sizeof(comResult_4), &comResult_4[0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for center_of_mass_4: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			cout << "comResult_4[" << nIndex << "][" << i << "]: " << comResult_4[nIndex][i] << '\n';
			//		}
			//	}
			//}
		}
		// 读取 inertia_matrices_4
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar10), &lHdlVar10, sizeof(im_4), im_4);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for inertia_matrices_4: " << nErr << '\n';
		}
		else {
			// 读取 inertia_matrices_4 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar10, sizeof(inertia_matrices_4), &inertia_matrices_4[0][0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for inertia_matrices_4: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			for (j = 0; j < 3; j++)
			//			{
			//				cout << "inertia_matrices_4[" << nIndex << "][" << i << "][" << j << "]: " << inertia_matrices_4[nIndex][i][j] << '\n';
			//			}
			//		}
			//	}
			//}
		}



		// 读取 masses_5
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar11), &lHdlVar11, sizeof(szVar5), szVar5);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for masses_5: " << nErr << '\n';
		}
		else {
			// 读取 masses_5 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar11, sizeof(masses_5), &masses_5[0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for masses_5: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//		cout << "masses_5[" << nIndex << "]: " << masses_5[nIndex] << '\n';
			//}
		}
		// 读取comResult_5
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar12), &lHdlVar12, sizeof(cmass_5), cmass_5);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for center_of_mass_5: " << nErr << '\n';
		}
		else {
			// 读取 center_of_mass_5 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar12, sizeof(comResult_5), &comResult_5[0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for center_of_mass_5: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			cout << "comResult_5[" << nIndex << "][" << i << "]: " << comResult_5[nIndex][i] << '\n';
			//		}
			//	}
			//}
		}
		// 读取 inertia_matrices_5
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar13), &lHdlVar13, sizeof(im_5), im_5);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for inertia_matrices_5: " << nErr << '\n';
		}
		else {
			// 读取 inertia_matrices_5 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar13, sizeof(inertia_matrices_5), &inertia_matrices_5[0][0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for inertia_matrices_5: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			for (j = 0; j < 3; j++)
			//			{
			//				cout << "inertia_matrices_5[" << nIndex << "][" << i << "][" << j << "]: " << inertia_matrices_5[nIndex][i][j] << '\n';
			//			}
			//		}
			//	}
			//}
		}






		// 读取 masses_6
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar14), &lHdlVar14, sizeof(szVar6), szVar6);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for masses_6: " << nErr << '\n';
		}
		else {
			// 读取 masses_6 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar14, sizeof(masses_6), &masses_6[0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for masses_6: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//		cout << "masses_6[" << nIndex << "]: " << masses_6[nIndex] << '\n';
			//}
		}
		// 读取comResult_6
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar15), &lHdlVar15, sizeof(cmass_6), cmass_6);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for center_of_mass_6: " << nErr << '\n';
		}
		else {
			// 读取 center_of_mass_6 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar15, sizeof(comResult_6), &comResult_6[0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for center_of_mass_6: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			cout << "comResult_6[" << nIndex << "][" << i << "]: " << comResult_6[nIndex][i] << '\n';
			//		}
			//	}
			//}
		}
		// 读取 inertia_matrices_6
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar16), &lHdlVar16, sizeof(im_6), im_6);
		if (nErr) {
			cerr << "Error: AdsSyncReadWriteReq for inertia_matrices_6: " << nErr << '\n';
		}
		else {
			// 读取 inertia_matrices_6 值
			nErr = AdsSyncReadReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar16, sizeof(inertia_matrices_6), &inertia_matrices_6[0][0][0]);
			if (nErr) {
				cerr << "Error: AdsSyncReadReq for inertia_matrices_6: " << nErr << '\n';
			}
			//else {
			//	for (nIndex = 0; nIndex < 6; nIndex++) // 用for循环语句来实现读取数组中的元素
			//	{
			//		for (i = 0; i < 3; i++)
			//		{
			//			for (j = 0; j < 3; j++)
			//			{
			//				cout << "inertia_matrices_6[" << nIndex << "][" << i << "][" << j << "]: " << inertia_matrices_6[nIndex][i][j] << '\n';
			//			}
			//		}
			//	}
			//}
		}


		double q2 = JointStatesCallback(positions,velocities,accelerations,masses_2,comResult_2,inertia_matrices_2,2);
		double q3 = JointStatesCallback(positions, velocities, accelerations, masses_3, comResult_3, inertia_matrices_3, 3);
		double q4 = JointStatesCallback(positions, velocities, accelerations, masses_4, comResult_4, inertia_matrices_4, 4);
		double q5 = JointStatesCallback(positions, velocities, accelerations, masses_5, comResult_5, inertia_matrices_5, 5);
		double q6 = JointStatesCallback(positions, velocities, accelerations, masses_6, comResult_6, inertia_matrices_6, 6);
		


		tao[0] = 0.0;
		tao[1] = q2;
		tao[2] = q3;
		tao[3] = q4;
		tao[4] = q5;
		tao[5] = q6;
		
		for (nIndex = 0; nIndex < 6; nIndex++) 
		{cout << "tao[" << nIndex << "]: " << tao[nIndex] << '\n';}
		nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar400), &lHdlVar400, sizeof(T), T);
		if (nErr) cerr << "Error: AdsSyncReadWriteReq: " << nErr << '\n';
		//通过句柄向PLC写入数组
		nErr = AdsSyncWriteReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar400, sizeof(tao), &tao[0]);
		if (nErr) cerr << "Error: AdsSyncReadReq: " << nErr << '\n';



		Sleep(50);
	} while (1);




	// 关闭ADS端口
	AdsPortClose();

	return 0;
}

// compensation.cpp
#include "pch.h"
#include "compensation.h"
#include <cmath>
#include <iostream>

Eigen::Matrix4d compensation::Ti_li(double theta, double d, double a, double alpha)
{
	Eigen::Matrix4d T;
	T << cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta),
		sin(theta), cos(theta)*cos(alpha), -cos(alpha)*sin(alpha), a*sin(theta),
		0, sin(alpha), cos(alpha), d,
		0, 0, 0, 1;
	return T;
}

// Implement friction method
double compensation::friction(double theta, double dtheta, const Eigen::VectorXd &f, const Eigen::VectorXd &f1)
{
	double fvi = f(0);    // 粘性摩擦系数
	double fci = f(1);    // 静摩擦系数
	double Bi = f(2);     // Stribeck摩擦参数
	double alpha_i = f(3); // αi
	double beta_s = f(4);  // βs
	double qs = f(5);     // Stribeck速度
	// term1: |q̇i|^αi sign(q̇i) Fvi
	double term1 = std::pow(std::abs(dtheta), alpha_i) * std::copysign(1.0, dtheta) * fvi;

	// term2: (1 - e^(-(|q̇i|/|qs|)^βs)) sign(q̇i) Fci
	double exp_term2 = std::exp(-std::pow(std::abs(dtheta / qs), beta_s));
	double term2 = (1 - exp_term2) * std::copysign(1.0, dtheta) * fci;

	// term3: e^(-(|q̇i|/|qs|)^βs) sign(q̇i) Bi
	double exp_term3 = std::exp(-std::pow(std::abs(dtheta / qs), beta_s));
	double term3 = exp_term3 * std::copysign(1.0, dtheta) * Bi;

	// 总的摩擦力
	double linear_friction = term1 + term2 + term3;

	// 非线性摩擦
	double fc = f1(0); // 静摩擦系数
	double fv = f1(1); // 粘性摩擦系数
	double nonlinear_friction = fv + fc * std::abs(dtheta);

	// 总摩擦力
	double total_friction = linear_friction + nonlinear_friction;

	// 检测 NaN 值
	if (std::isnan(total_friction)) {
		printf("Friction calculation returned NaN. Check input parameters: theta=%f, dtheta=%f, qs=%f", theta, dtheta, qs);
	}

	return total_friction;
}

// Implement joint_states_callback
double compensation::joint_states_callback(
	const Eigen::VectorXd &joint_positions,
	const Eigen::VectorXd &joint_velocities,
	const Eigen::VectorXd &joint_accelerations,
	const std::vector<double> &masses,
	const std::vector<Eigen::Vector3d> &center_of_mass,
	const std::vector<Eigen::Matrix3d> &inertia_matrices,
	int num
) {
	// Extract masses
	double m1 = masses[0];
	double m2 = masses[1];
	double m3 = masses[2];
	double m4 = masses[3];
	double m5 = masses[4];
	double m6 = masses[5];

	// Extract center of mass
	Eigen::Vector3d pc1 = center_of_mass[0];
	Eigen::Vector3d pc2 = center_of_mass[1];
	Eigen::Vector3d pc3 = center_of_mass[2];
	Eigen::Vector3d pc4 = center_of_mass[3];
	Eigen::Vector3d pc5 = center_of_mass[4];
	Eigen::Vector3d pc6 = center_of_mass[5];

	// Extract inertia matrices
	Eigen::Matrix3d I1 = inertia_matrices[0];
	Eigen::Matrix3d I2 = inertia_matrices[1];
	Eigen::Matrix3d I3 = inertia_matrices[2];
	Eigen::Matrix3d I4 = inertia_matrices[3];
	Eigen::Matrix3d I5 = inertia_matrices[4];
	Eigen::Matrix3d I6 = inertia_matrices[5];

	// Extract joint positions, velocities, accelerations
	double theta_1 = joint_positions[0];
	double theta_2 = joint_positions[1];
	double theta_3 = joint_positions[2];
	double theta_4 = joint_positions[3];
	double theta_5 = joint_positions[4];
	double d6 = joint_positions[5];
	double dq1 = joint_velocities[0];
	double dq2 = joint_velocities[1];
	double dq3 = joint_velocities[2];
	double dq4 = joint_velocities[3];
	double dq5 = joint_velocities[4];
	double dq6 = joint_velocities[5];
	double ddq1 = joint_accelerations[0];
	double ddq2 = joint_accelerations[1];
	double ddq3 = joint_accelerations[2];
	double ddq4 = joint_accelerations[3];
	double ddq5 = joint_accelerations[4];
	double ddq6 = joint_accelerations[5];
	double M_PI = 3.1415926;

	// Calculate transformation matrices
	Eigen::Matrix4d T01 = Ti_li(theta_1, 0.81411, 0, 0);
	Eigen::Matrix4d T12 = Ti_li(theta_2 + M_PI / 2, -0.363951, 0, -M_PI / 18);
	Eigen::Matrix4d T23 = Ti_li(theta_3 - 140.66 * M_PI / 180, 0, 0.0384988, M_PI / 2);
	Eigen::Matrix4d T34 = Ti_li(theta_4 - 123.3 * M_PI / 180, 0, 0.266, 0);
	Eigen::Matrix4d T45 = Ti_li(theta_5 + 3.96 * M_PI / 180, 0, 0.366, 0);
	Eigen::Matrix4d T56 = Ti_li(0, d6, 0.130331, M_PI / 2);
	Eigen::Matrix4d T = T01 * T12 * T23 * T34 * T45 * T56;

	Eigen::Vector3d positionxyz_current = T.block<3, 1>(0, 3);
	// printf("Current position: x=%f, y=%f, z=%f", positionxyz_current[0], positionxyz_current[1], positionxyz_current[2]);

	std::vector<Eigen::Matrix4d> transformations;
	transformations.emplace_back(T01);
	transformations.emplace_back(T12);
	transformations.emplace_back(T23);
	transformations.emplace_back(T34);
	transformations.emplace_back(T45);
	transformations.emplace_back(T56);

	// Rotation matrices
	Eigen::Matrix3d R01 = T01.block<3, 3>(0, 0);
	Eigen::Matrix3d R12 = T12.block<3, 3>(0, 0);
	Eigen::Matrix3d R23 = T23.block<3, 3>(0, 0);
	Eigen::Matrix3d R34 = T34.block<3, 3>(0, 0);
	Eigen::Matrix3d R45 = T45.block<3, 3>(0, 0);
	Eigen::Matrix3d R56 = T56.block<3, 3>(0, 0);
	Eigen::Matrix3d R67;
	R67 << 1, 0, 0,
		0, 0, 1,
		0, 0, 1;

	Eigen::Vector3d p76(0, 0, 0);
	// Joint axis
	Eigen::Vector3d z(0, 0, 1);
	Eigen::Vector3d g(0, -9.8 * 0.866, -9.8 * 0.5);
	Eigen::Vector3d w0 = Eigen::Vector3d::Zero();
	Eigen::Vector3d w0d = Eigen::Vector3d::Zero();
	Eigen::Vector3d v0d = g;

	// Compute w1, w1d, v1d, vc1d, F1, N1
	Eigen::Vector3d w1 = R01.transpose() * w0 + dq1 * z;
	Eigen::Vector3d w1d = R01.transpose() * w0d + R01.transpose() * (w0.cross(dq1 * z)) + ddq1 * z;
	Eigen::Vector3d v1d = R01.transpose() * (v0d + w0d.cross(T01.block<3, 1>(0, 3)) + w0.cross(w0.cross(T01.block<3, 1>(0, 3))));
	Eigen::Vector3d vc1d = w1.cross(w1.cross(pc1)) + w1d.cross(pc1) + v1d;
	Eigen::Vector3d F1 = m1 * vc1d;
	Eigen::Vector3d N1 = I1 * w1d + w1.cross(I1 * w1);

	// Compute w2, w2d, v2d, vc2d, F2, N2
	Eigen::Vector3d w2 = R12.transpose() * w1 + dq2 * z;
	Eigen::Vector3d w2d = R12.transpose() * w1d + R12.transpose() * (w1.cross(dq2 * z)) + ddq2 * z;
	Eigen::Vector3d v2d = R12.transpose() * (v1d + w1d.cross(T12.block<3, 1>(0, 3)) + w1.cross(w1.cross(T12.block<3, 1>(0, 3))));
	Eigen::Vector3d vc2d = w2.cross(w2.cross(pc2)) + w2d.cross(pc2) + v2d;
	Eigen::Vector3d F2 = m2 * vc2d;
	Eigen::Vector3d N2 = I2 * w2d + w2.cross(I2 * w2);

	// Compute w3, w3d, v3d, vc3d, F3, N3
	Eigen::Vector3d w3 = R23.transpose() * w2 + dq3 * z;
	Eigen::Vector3d w3d = R23.transpose() * w2d + R23.transpose() * (w2.cross(dq3 * z)) + ddq3 * z;
	Eigen::Vector3d v3d = R23.transpose() * (w2d.cross(T23.block<3, 1>(0, 3)) + w2.cross(w2.cross(T23.block<3, 1>(0, 3))) + v2d);
	Eigen::Vector3d vc3d = w3d.cross(pc3) + w3.cross(w3.cross(pc3) + v3d);
	Eigen::Vector3d F3 = m3 * vc3d;
	Eigen::Vector3d N3 = I3 * w3d + w3.cross(I3 * w3);

	// Compute w4, w4d, v4d, vc4d, F4, N4
	Eigen::Vector3d w4 = R34.transpose() * w3 + dq4 * z;
	Eigen::Vector3d w4d = R34.transpose() * w3d + R34.transpose() * (w3.cross(dq4 * z)) + ddq4 * z;
	Eigen::Vector3d v4d = R34.transpose() * (v3d + w3d.cross(T34.block<3, 1>(0, 3)) + w3.cross(w3.cross(T34.block<3, 1>(0, 3))));
	Eigen::Vector3d vc4d = w4.cross(w4.cross(pc4)) + w4d.cross(pc4) + v4d;
	Eigen::Vector3d F4 = m4 * vc4d;
	Eigen::Vector3d N4 = I4 * w4d + w4.cross(I4 * w4);

	// Compute w5, w5d, v5d, vc5d, F5, N5
	Eigen::Vector3d w5 = R45.transpose() * w4 + dq5 * z;
	Eigen::Vector3d w5d = R45.transpose() * w4d + R45.transpose() * (w4.cross(dq5 * z)) + ddq5 * z;
	Eigen::Vector3d v5d = R45.transpose() * (v4d + w4d.cross(T45.block<3, 1>(0, 3)) + w4.cross(w4.cross(T45.block<3, 1>(0, 3))));
	Eigen::Vector3d vc5d = w5.cross(w5.cross(pc5)) + w5d.cross(pc5) + v5d;
	Eigen::Vector3d F5 = m5 * vc5d;
	Eigen::Vector3d N5 = I5 * w5d + w5.cross(I5 * w5);

	// Compute w6, w6d, v6d, vc6d, F6, N6
	Eigen::Vector3d w6 = R56.transpose() * w5;
	Eigen::Vector3d w6d = R56.transpose() * w5d;
	Eigen::Vector3d v6d = R56.transpose() * (w5d.cross(T56.block<3, 1>(0, 3)) + w5.cross(w5.cross(T56.block<3, 1>(0, 3))) + ddq6 * z);
	Eigen::Vector3d vc6d = v6d;
	Eigen::Vector3d F6 = m6 * vc6d;
	Eigen::Vector3d N6 = I6 * w6d + w6.cross(I6 * w6);

	// Rotational joint
	/*
	Eigen::Vector3d w6 = R56.transpose() * w5 + dq6 * z;
	Eigen::Vector3d w6d = R56.transpose() * w5d + R56.transpose() * (w5.cross(dq6 * z)) + ddq6 * z;
	Eigen::Vector3d v6d = R56.transpose() * (v5d + w5d.cross(T56.block<3, 1>(0, 3)) + w5.cross(w5.cross(T56.block<3, 1>(0, 3))));
	Eigen::Vector3d vc6d = w6.cross(w6.cross(pc6)) + w6d.cross(pc6) + v6d;
	Eigen::Vector3d F6 = m6 * vc6d;
	Eigen::Vector3d N6 = I6 * w6d + w6.cross(I6 * w6);
	*/

	// Final forces and moments
	Eigen::Vector3d f7 = Eigen::Vector3d::Zero();
	Eigen::Vector3d n7 = Eigen::Vector3d::Zero();

	Eigen::Vector3d f6 = R67 * f7 + F6;
	Eigen::Vector3d n6 = N6 + R67 * n7 + pc6.cross(F6) + p76.cross(R67 * f7);
	double t6 = f6.dot(z);
	// double t6 = n6.dot(z);

	Eigen::Vector3d f5 = R56 * f6 + F5;
	Eigen::Vector3d n5 = N5 + R56 * n6 + pc5.cross(F5) + T56.block<3, 1>(0, 3).cross(R56 * f6);
	double t5 = n5.dot(z);

	Eigen::Vector3d f4 = R45 * f5 + F4;
	Eigen::Vector3d n4 = N4 + R45 * n5 + pc4.cross(F4) + T45.block<3, 1>(0, 3).cross(R45 * f5);
	double t4 = n4.dot(z);

	Eigen::Vector3d f3 = R34 * f4 + F3;
	Eigen::Vector3d n3 = N3 + R34 * n4 + pc3.cross(F3) + T34.block<3, 1>(0, 3).cross(R34 * f4);
	double t3 = n3.dot(z);

	Eigen::Vector3d f2 = R23 * f3 + F2;
	Eigen::Vector3d n2 = N2 + R23 * n3 + pc2.cross(F2) + T23.block<3, 1>(0, 3).cross(R23 * f3);
	double t2 = n2.dot(z);

	Eigen::Vector3d f1 = R12 * f2 + F1;
	Eigen::Vector3d n1 = N1 + R12 * n2 + pc1.cross(F1) + T12.block<3, 1>(0, 3).cross(R12 * f2);
	double t1 = n1.dot(z);

	// Torque vector
	// Eigen::VectorXd torque = Eigen::VectorXd::Zero(6);
	// torque << t1,t2,t3,t4,t5,t6;
	// ROS_INFO("torque: %f %f %f %f %f %f",torque(0),torque(1),torque(2),torque(3),torque(4),torque(5));
	double friction2, q2, tao;
	double friction3, q3;
	double friction4, q4;
	double friction5, q5;
	double friction6, q6;
	Eigen::VectorXd q = Eigen::VectorXd::Zero(6);
	Eigen::VectorXd q_0 = Eigen::VectorXd::Zero(6);
	//printf("T2 :%f \n", t2);
	switch (num)
	{
	case 2:
		friction2 = get_friction(2, theta_2, dq2);
		q2 = t2 + friction2;
		tao = q2;
		//printf("friction2:%f \n", friction2);
		//printf("tao2: %f \n", tao);
		//q(1) = q2; // Joint 2 torque
		break;
	case 3:
		friction3 = get_friction(3, theta_3, dq3);
		q3 = t3 + friction3;
		tao = q3;
		//printf("tao3: %f ", tao);
		//q(2) = q3; // Joint 3 torque
		break;
	case 4:
		friction4 = get_friction(4, theta_4, dq4);
		q4 = t4 + friction4;
		tao = q4;
		//printf("tao4: %f ", tao);
		//q(3) = q4; // Joint 4 torque
		break;
	case 5:
		friction5 = get_friction(5, theta_5, dq5);
		q5 = t5 + friction5;
		tao = q5;
		//printf("tao5: %f ", tao);
		//q(4) = q5; // Joint 5 torque
		break;
	case 6:
		friction6 = get_friction(6, d6, dq6);
		q6 = t6 + friction6;
		tao = q6;
		//printf("tao6: %f ", tao);
		//q(5) = q6; // Joint 6 torque
		break;
	default:
		printf("Invalid joint number. Please select a valid joint 2-6.");
	}
	return tao;
}

// Implement get_friction
double compensation::get_friction(int num, double theta, double dtheta) {
	Eigen::VectorXd f(6);
	Eigen::VectorXd f1(2);
	switch (num)
	{
	case 2: {
		f << -1.8157018, 0.31180483, -0.779818, 0.70042086, 18.232029, 0.1;
		f1 << 11.897971153259277, 1.3998908996582031;
		double friction_value = friction(theta, dtheta, f, f1);
		return friction_value;
	}
	case 3: {
		f << -5.4534173, 0.888715, -3.9374301, 0.7200519, 37.840214, 0.1;
		f1 << 2.160109281539917, 4.213362216949463;
		double friction_value = friction(theta, dtheta, f, f1);
		return friction_value;
	}
	case 4: {
		f << 9.524408, -10.3272, -5.953079, 0.1, 3.2415874, 0.7954815;
		f1 << 2.2083520889282227, 4.218172550201416;
		double friction_value = friction(theta, dtheta, f, f1);
		return friction_value;
	}
	case 5: {
		f << -1.6657009, 0.4856454, -4.3181567, 0.84907013, 38.90394, 0.1;
		f1 << 2.493135452270508, 4.444770336151123;
		double friction_value = friction(theta, dtheta, f, f1);
		return friction_value;
	}
	case 6: {
		f << 4.66212, -3.874223, -1.3778862, 0.22896422, 0.70151824, 0.1;
		f1 << 0.1655563861131668, 1.9374377727508545;
		double friction_value = friction(theta, dtheta, f, f1);
		return friction_value;
	}
	default:
		printf("Please select a valid joint number.");
		return 0.0;
	}
}

// Implement the exported JointStatesCallback function
extern "C" COMPENSATION_API double JointStatesCallback(
    const double joint_positions[6],
    const double joint_velocities[6],
    const double joint_accelerations[6],
    const double masses[6],
    const double center_of_mass[][3],
    const double inertia_matrices[][3][3],
    int num
) {
    // 将C数组转换为Eigen类型
    Eigen::VectorXd joint_pos(6);
    Eigen::VectorXd joint_vel(6);
    Eigen::VectorXd joint_acc(6);
    std::vector<double> mass(masses, masses + 6);
    std::vector<Eigen::Vector3d> com;
    std::vector<Eigen::Matrix3d> inertia;

    for (int i = 0; i < 6; ++i) {
        joint_pos(i) = joint_positions[i];
        joint_vel(i) = joint_velocities[i];
        joint_acc(i) = joint_accelerations[i];
        Eigen::Vector3d pc(center_of_mass[i][0], center_of_mass[i][1], center_of_mass[i][2]);
        com.push_back(pc);
        Eigen::Matrix3d I;
        for (int j = 0; j < 3; ++j) {
            for (int k = 0; k < 3; ++k) {
                I(j, k) = inertia_matrices[i][j][k];
            }
        }
        inertia.push_back(I);
    }
    compensation comp;
    double result = comp.joint_states_callback(joint_pos, joint_vel, joint_acc, mass, com, inertia, num);
    return result;
}

// Implement the exported GetFriction function
extern "C" COMPENSATION_API double GetFriction(int num, double theta, double dtheta) {
    compensation comp;
    return comp.get_friction(num, theta, dtheta);
}

compensation.cpp文件是将辨识得到的参数通过牛顿-欧拉的公式计算出动力学力矩。

运行代码前，Twincat3中的tao的值都是0的：

运行ADS_Server.cpp代码，可以在Twincat3中看到tao的值是实时变化的，说明通信正常

注：这个代码中没有包含控制电机的部分，将测试电机力矩模式的代码写入就能实现了，这里就不过多的写了，如果有需要的可以联系我，发送完整的代码。

腾讯云开发者社区

腾讯云面向开发者汇聚海量精品云计算使用和开发经验，营造开放的云计算技术生态圈。

更多推荐

Elasticsearch复杂数据类型终极指南：从入门到精通

Elasticsearch作为功能强大的搜索引擎，支持多种复杂数据类型，让开发者能够灵活处理各种结构化和非结构化数据。本文将带你全面了解Elasticsearch中的复杂数据类型，从基础概念到实际应用，助你轻松掌握数据建模的核心技巧。## 内部对象：构建层级化数据结构在Elasticsearch中，对象类型（Object）是最基础的复杂数据类型之一，用于表示具有嵌套关系的数据。例如，我们可

腾讯云开发者社区

终极指南：Flink SQL连接器版本管理从混乱到有序的升级之路

Apache Flink作为流处理领域的佼佼者，其SQL连接器的版本管理一直是开发者面临的核心挑战。本文将系统讲解Flink SQL连接器版本管理的最佳实践，帮助你轻松应对版本兼容性问题，实现从混乱到有序的升级之旅。## 连接器版本管理的常见痛点 😫在Flink应用开发中，连接器版本管理常常让开发者头疼不已。不同版本的连接器可能导致各种兼容性问题，例如API变更、功能差异甚至运行时错误。

腾讯云开发者社区

如何快速搭建Neon无服务器PostgreSQL：面向初学者的完整指南

Neon是一款革命性的无服务器PostgreSQL解决方案，它通过分离存储和计算层，实现了自动扩缩容、类代码式数据库分支以及零级扩展能力。本指南将帮助你从零开始搭建Neon开发环境，体验这款创新数据库的强大功能。## 准备工作：环境要求与依赖项在开始搭建Neon环境前，请确保你的系统满足以下要求：- Linux操作系统（推荐Ubuntu 20.04+或Debian 11+）- Git