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<SPCK_INSTALLDIR>/run/realtime/<SIMPACK_OS> -I<SPCK_INSTALLDIR>/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 <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "/.../Simpack-2021x/run/realtime/spck_rt.h" //spck_rt.h所在目录
// 与Python的TCP通信相关includes
#include <arpa/inet.h>
#include <netinet/in.h>
#include <stdbool.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include <netinet/tcp.h>

#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程序的运行，包括顺序执行如下步骤：

1. 初始化参数，包括自定义量（宏）、与Python和SIMPACK通信的参数设定、其他过程变量
2. 使用SpckRtInitUDP启动UDP通信，通过SpckRtGetUYDim获得u-Inputs和y-Outputs的维度
3. 使用SpckRtStart启动SIMPACK仿真计算，进入仿真循环
   1. 从SIMPACK中通过调用SpckRtGetY获得该时间步的y-Outputs
   2. 将y-Outputs通过send函数，由TCP通讯协议发送给Python程序
   3. 通过recv函数，由TCP通讯协议从Python程序处获取u-Inputs，如果recv_len的值为0，则关闭连接并退出循环
   4. 通过调用SpckRtSetU函数将Python程序发送来的u-Inputs转发至SIMPACK
4. 若循环结束，则关闭Python程序与C语言之间的通信socket，并调用SpckRtFinish结束SIMPACK仿真

C程序完整代码如下：

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "/home/.../Simpack-2021x/run/realtime/spck_rt.h"
// TCP includes
#include <arpa/inet.h>
#include <netinet/in.h>
#include <stdbool.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include <netinet/tcp.h>

#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代码中也会涉及到并注释显示。