SIMPACK与Python联合仿真——2. C程序代码编写与编译
继续上个博客《SIMPACK与Python联合仿真——1. 预备知识》内容。本文主要内容为SIMPACK模型搭建、C程序的编写与编译。
1. 实现案例
实现一个简单的案例，对一阶倒立摆SIMPACK模型施加时间历程为正弦曲线的力矩，控制力矩的数值由Python程序生成。
2. SIMPACK模型
1）在SIMPACK中新建General模型，保存为test01.spck。
建立Cylinder模型，杆长0.5m，直径0.01m
2）设置u-Inputs，注意区分各u-Inputs的External index编号互不相同。笔者设置了4个，其中只有第1个u-Inputs输入到倒立摆的旋转轴上作为输入力矩，其他暂时闲置。
3）在杆子的旋转中心与大地原点之间新建93号力元。在4:u-Vector Element torque x处填写u-Inputs生成的u-Vector为$UE_Tq01。即设置为通过外部u-Vector获得的力矩，输入至倒立摆的铰接点，以驱动其旋转。
4）定义y-Outputs，包括由Expression表达式为cos(AX($M_Isys,$M_Body1_J))的旋转角度余弦值$X_cos_theta(Type:19)、Expression表达式为sin(AX($M_Isys,$M_Body1_J))的旋转角度余弦值$X_sin_theta(Type:19)、旋转角速度(Type:5)、旋转角度(Type:3)、（上一个时间步的）施加力矩(Type:11)、运行时间$Y_runnningtime(Type:20)。其中旋转角速度(Type:5)、旋转角度(Type:3)需要自定义Sensor。
此外，如果使用默认SODASRT2求解器，在实时仿真中将发生Warning或Error，应更改为定步长积分器，步长为0.001(可以使用API在程序中调整)，积分器选择FixBDF+Non-iterative projection可以避免很多潜在的因为对于强非线性量与突变量积分带来的错误。
3. SIMPACK Realtime代码变量/宏的定义
Linux的SIMPACK安装目录中，在Simpack-2021x/run/realtime/examples文件夹下可以找到spck_rt_example.c文件。代码给出了使用gcc的编译命令：
To build use:
* gcc spck_rt_example.c -lspck_rt -lrt -lm -L/run/realtime/ -I/run/realtime -o spck_rt_example
上述代码直接使用gcc编译是无法通过的，需要修改代码。
避免污染原程序，将spck_rt_example.c复制一份为test01.c，后续对test01.c进行修改和编译。
在复制生成的test01.c中，自定义量（宏）包括：
-
SPCK_PATH: 指定SIMPACK的安装路径，如果设为NULL，系统会尝试通过PATH环境变量找到SIMPACK的可执行文件。 -
MODEL_FILE: 模型文件的绝对路径。 -
SPCK_MODE: 这个标志位用于控制SIMPACK运行的模式。E_RT_MODE__KEEP_SLV意味着保持simpack-rt-slv运行，这可以减少启动时间，但会一直占用许可证，直到进程被杀死。E_RT_MODE__BUSY_WAIT会使spckRtAdvance()执行忙等待（polling）而不是阻塞等待，确保此程序的优先级低于SIMPACK实时模式或分配给不同CPU。 -
SPCK_CPUS: 指定要使用的CPU的ID，主线程会使用第一个给定的ID，如果为空，则表示可以使用任意的CPU。 -
SPCK_RT_PRIO: 实时优先级，需要确保当前用户有合适的权限。如果设置为0，则表示禁用实时优先级，使用软实时。 -
SPCK_UDP_PORT: 用于UDP通信的端口号，必须大于1024。 -
SPCK_UDP_TIMEOUT: 实时模拟的UDP超时时间，建议设为通信步长的10倍，以确保在SIMPACK实时求解器停止响应时返回超时。 -
SPCK_POSIX_MQ: 如果设置为1，表示使用POSIX消息队列进行通信，而不是UDP。 -
SPCK_VERBOSE: 决定SIMPACK的详细输出级别。例如，0表示不输出任何信息，1表示打印所有接收到的数据包，2表示打印接收和发送的数据包，3还包含远程ip/port，4与3相同，5还包括SIMPACK的十六进制转储（仅在simpack重启时更新）
在多个设置项中，着重注意需要修改以下内容：
- spck_rt.h的include路径
- SIMPACK安装目录
SPCK_PATH -
模型文件的绝对路径MODEL_FILE
参考代码段落定义如下：
#include
#include
#include
#include "/.../Simpack-2021x/run/realtime/spck_rt.h" //spck_rt.h所在目录
// 与Python的TCP通信相关includes
#include
#include
#include
#include
#include
#include
#include
#include
#define DEFAULT_TCP2SPCK_PORT 9999
#define BUFFER_SIZE 1024
#define SPCK_PATH "/.../Simpack-2021x" // 安装目录
#define SPCK_MODE E_RT_MODE__KEEP_SLV
#define SPCK_RT_PRIO 0
#define DEFAULT_SPCK_UDP_PORT 12120
#define SPCK_UDP_TIMEOUT 0.5
#define SPCK_VERBOSE 0
#define DEFAULT_ControlInterval 0.05
#define MODEL_FILE "/.../test01.spck" // 模型目录
#define SPCK_CPUS "" 设置与Python程序之间的TCP通信的代码段落如下：
/* TCP通信设置 START */
int sockfd, new_socket;
struct sockaddr_in serv_addr, cli_addr;
socklen_t addr_size;
char buffer[BUFFER_SIZE] = {0};
// 创建套接字
sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd < 0)
{
perror("socket creation failed");
exit(EXIT_FAILURE);
}
// 设置套接字选项，以便在套接字关闭后立即释放端口号
int opt = 1;
if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0)
{
perror("setsockopt failed");
exit(EXIT_FAILURE);
}
// 设置服务器地址结构
memset(&serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(port);
// 绑定套接字和地址
if (bind(sockfd, (const struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
{
perror("bind failed");
exit(EXIT_FAILURE);
}
// 监听套接字
if (listen(sockfd, 1) < 0)
{
perror("listen failed");
exit(EXIT_FAILURE);
}
// 接受客户端连接
addr_size = sizeof(cli_addr);
new_socket = accept(sockfd, (struct sockaddr *)&cli_addr, &addr_size);
if (new_socket < 0)
{
perror("accept failed");
exit(EXIT_FAILURE);
}
// 设置缓冲区大小
int buffer_size = 512;
setsockopt(sockfd, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size));
setsockopt(sockfd, SOL_SOCKET, SO_RCVBUF, &buffer_size, sizeof(buffer_size));
// 禁用Nagle算法
int flag = 1;
setsockopt(new_socket, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(int));在TCP通信中，由于每次传输数据量较小，因此也设置较小的缓冲区，取512B。
此外设置了禁用Nagle算法与设置套接字选项SO_REUSEADDR，如此设置的原因详见上个博客。
4. SIMPACK Realtime代码运行逻辑
C程序的运行，包括顺序执行如下步骤：
- 初始化参数，包括自定义量（宏）、与Python和SIMPACK通信的参数设定、其他过程变量
- 使用SpckRtInitUDP启动UDP通信，通过SpckRtGetUYDim获得u-Inputs和y-Outputs的维度
- 使用SpckRtStart启动SIMPACK仿真计算，进入仿真循环
- 从SIMPACK中通过调用SpckRtGetY获得该时间步的y-Outputs
- 将y-Outputs通过send函数，由TCP通讯协议发送给Python程序
- 通过recv函数，由TCP通讯协议从Python程序处获取u-Inputs，如果recv_len的值为0，则关闭连接并退出循环
- 通过调用SpckRtSetU函数将Python程序发送来的u-Inputs转发至SIMPACK
- 若循环结束，则关闭Python程序与C语言之间的通信socket，并调用SpckRtFinish结束SIMPACK仿真
C程序完整代码如下：
#include
#include
#include
#include "/home/.../Simpack-2021x/run/realtime/spck_rt.h"
// TCP includes
#include
#include
#include
#include
#include
#include
#include
#include
#define DEFAULT_TCP2SPCK_PORT 9999
#define BUFFER_SIZE 1024
#define SPCK_PATH "/.../Simpack-2021x"
#define SPCK_MODE E_RT_MODE__KEEP_SLV
#define SPCK_RT_PRIO 0
#define DEFAULT_SPCK_UDP_PORT 12120
#define SPCK_UDP_TIMEOUT 0.5
#define SPCK_VERBOSE 0
#define DEFAULT_ControlInterval 0.05
#define MODEL_FILE "/.../test01.spck"
#define SPCK_CPUS ""
int main( int argc, char *argv[] )
{
const char* name;
double tend = 20; // 运行时长
double time;
double value;
double wallTime;
double* u;
double* y;
int i;
int ierr;
int index;
int j;
int nu;
int ny;
struct timespec t0;
struct timespec t1;
int SPCK_UDP_PORT = DEFAULT_SPCK_UDP_PORT;
double ControlInterval = DEFAULT_ControlInterval;
double h = DEFAULT_ControlInterval ;
// 解析命令行参数以配置TCP与UDP通信的端口
int port = DEFAULT_TCP2SPCK_PORT;
if (argc > 1)
{
port = atoi(argv[1]);
}
if (argc > 2)
{
SPCK_UDP_PORT = atoi(argv[2]);
}
if (argc > 3)
{
ControlInterval = atof(argv[3]); // atoi转换为整数,应该使用atof函数
h = ControlInterval ;
}
if (argc > 4)
{
tend = atof(argv[4]);
}
/* TCP通信设置 START */
int sockfd, new_socket;
struct sockaddr_in serv_addr, cli_addr;
socklen_t addr_size;
char buffer[BUFFER_SIZE] = {0};
// 创建套接字
sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd < 0)
{
perror("socket creation failed");
exit(EXIT_FAILURE);
}
// 设置套接字选项，以便在套接字关闭后立即释放端口号
int opt = 1;
if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0)
{
perror("setsockopt failed");
exit(EXIT_FAILURE);
}
// 设置服务器地址结构
memset(&serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(port);
// 绑定套接字和地址
if (bind(sockfd, (const struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
{
perror("bind failed");
exit(EXIT_FAILURE);
}
// 监听套接字
if (listen(sockfd, 1) < 0)
{
perror("listen failed");
exit(EXIT_FAILURE);
}
// 接受客户端连接
addr_size = sizeof(cli_addr);
new_socket = accept(sockfd, (struct sockaddr *)&cli_addr, &addr_size);
if (new_socket < 0)
{
perror("accept failed");
exit(EXIT_FAILURE);
}
// 设置缓冲区大小
int buffer_size = 512; // 655360
setsockopt(sockfd, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size));
setsockopt(sockfd, SOL_SOCKET, SO_RCVBUF, &buffer_size, sizeof(buffer_size));
// 禁用Nagle算法
int flag = 1;
setsockopt(new_socket, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(int));
/* TCP通信设置 END */
ierr = SpckRtInitUDP( SPCK_MODE, SPCK_PATH, MODEL_FILE, SPCK_CPUS, SPCK_RT_PRIO, SPCK_VERBOSE, SPCK_UDP_PORT, SPCK_UDP_TIMEOUT );
if( ierr )
{
SpckRtFinish();
printf( "SpckRtInitUDP failed\n" );
return( 1 );
}
/* Get model dimensions and reserve u/y vector */
SpckRtGetUYDim( &nu, &ny );
u = (double*)calloc( nu, sizeof( double ) );
y = (double*)calloc( ny, sizeof( double ) );
// Print the dimensions of u and y
printf("Dimensions of u: %d\n", nu);
printf("Dimensions of y: %d\n", ny);
if( u == NULL || y == NULL )
{
SpckRtFinish();
printf( "Could not allocate memory for %i inputs and/or %i outputs\n", nu, ny );
return( 1 );
}
/* Start realtime solver */
if( SpckRtStart() )
{
SpckRtFinish();
printf( "Could not start realtime simulation.\n" );
return( 1 );
}
printf( "C:开始calculation loop\n" );
/*calculation loop*/
time = 0;
for( i = 0 ; i <= tend/h ; ++i )
{
time += h;
// y变量
SpckRtGetY( y );
//printf( "C:Rev Y From SPCKrt\n" );
uint32_t ny_network = htonl(ny);
send(new_socket, &ny_network, sizeof(ny_network), 0);
for (int k = 0; k < ny; ++k) {
float y_value = y[k];
uint32_t y_value_network = htonl(*(uint32_t *)&y_value);
send(new_socket, &y_value_network, sizeof(y_value_network), 0);
}
// u变量
double u_num[4];
const double *u = u_num;
ssize_t recv_len;
memset(buffer, 0, BUFFER_SIZE);
recv_len = recv(new_socket, buffer, BUFFER_SIZE, 0);
// 检查recv_len的值，如果为0，则关闭连接并退出循环
if (recv_len == 0) {
printf( "C:因Actor客户端主动关闭TCP连接，提前停止Simpack仿真\n" );
SpckRtFinish();
shutdown(new_socket, SHUT_RDWR); // 可以执行到
close(new_socket);
close(sockfd);
return( 0 );
// break;
} else {
sscanf(buffer, "%lf %lf %lf %lf", &u_num[0], &u_num[1], &u_num[2], &u_num[3]);
}
SpckRtSetU(u);
//printf("C:Sent U to SPCKrt\n");
if( SpckRtAdvance( time ) )
{
SpckRtFinish();
printf( "SpckRtAdvance failed.\n" );
return( 1 );
}
}
//关闭Socket
shutdown(new_socket, SHUT_RDWR);
close(new_socket);
close(sockfd);
/* Close SIMPACK Realtime */
SpckRtFinish();
printf( "Finished.\n" );
return( 0 );
}
5. SIMPACK Realtime代码的编译与调用
test01.c代码使用下列终端命令行编译：
gcc test01.c -lspck_rt -lrt -lm -L /.../Simpack-2021x/run/realtime/linux64 -L /home/yaoyao/Simpack-2021x/run/bin/linux64 -I /.../Simpack-2021x/run/realtime -o test01
编译将形成名称为test01的可执行文件。
特别注意替换代码中以下内容：
- SPCK_PATH
- include spck_rt.h路径
- MODEL_FILE
- .../Simpack-2021x/run/realtime/linux64路径
- .../Simpack-2021x/run/bin/linux64路径
-
.../Simpack-2021x/run/realtime路径
gcc编译各参数含义如下：
-
test01.c: 需要编译的 C 语言源文件。 -
-lspck_rt,-lrt,-lm: 链接选项，用来指定链接器需要链接的库。-l后面的部分就是库的名字。例如，-lspck_rt指定链接spck_rt库，-lrt指定链接rt库，-lm指定链接m库。 -
-L /.../Simpack-2021x/run/realtime/linux64,-L /.../Simpack-2021x/run/bin/linux64:-L选项用来指定链接器搜索库的路径。在这里，链接器会在/.../Simpack-2021x/run/realtime/linux64和/.../Simpack-2021x/run/bin/linux64这两个目录中搜索库。 -
-I /.../Simpack-2021x/run/realtime:-I选项用来指定编译器搜索头文件的路径。在这里，编译器会在/.../Simpack-2021x/run/realtime目录中搜索头文件。 -
-o test01:-o选项用来指定输出文件的名字。在这里，编译后的可执行文件会被命名为test01。
调用时命令示例如下，其中9999为C程序与Python程序之间TCP通信的端口号，12999为C程序与SIMPACK计算核心程序之间UDP的端口号，0.01表示仿真步长间隔。可以根据实际与自定义修改，但需要保证不占用本机其他软件的端口资源，并要保持与Python代码端口号定义之间的一致性。
./test01 9999 12999 0.01以上即完成了C程序的编写与编译。C程序具有两个功能，1）调用SIMPACK APIs与SIMPACK计算核心通信，通信协议为UDP；2）作为Server端与Python程序通过TCP协议进行通信。
Python程序的编写在下一篇博客完成。
6. SIMPACK Realtime模块其他相关特性
SIMPACK Realtime模块是支持并行计算的，即同时进行多个不同模型在不同端口的仿真计算。然而，其首次开启时需要串行启动。如果使用多线程编程并行启动SIMPACK Realtime模型，实测最多启动4个，其他的Realtime模型将会以报错结束，并且启动时间也异常长。
如果需要大量同时进行的SIMPACK并行仿真，务必考虑到以下几点：
1）串行启动SIMPACK Realtime模块
2）在C程序代码中，将SPCK_MODE取值为E_RT_MODE__KEEP_SLV，这样每一轮的并行仿真结束后，下一轮并行仿真的开启时就不会发生同时再次启动Realtime求解器导致的报错
3）在首次启动SIMPACK Realtime求解器并完成与Python程序的仿真后，再次调用相同端口会报错bind failed: Address already in use。其解决方案是，将首次启动与后续循环多轮仿真设置为两组端口，启动时的TCP和UDP端口为A，后续循环多轮仿真的TCP和UDP端口可以设置为B=A+100，即A和B之间要互不相同（当然也需要注意SIMPACK与Python调用的各个端口不可以占用本机其他程序的端口资源）。
4）SIMPACK本身求解器的启动需要一定时间，根据模型复杂程度所需时长在1s至10s不等，建立与Python等其他软件的通信也需要一定的延时，请根据实际调整。下一节的Python代码中也会涉及到并注释显示。