SIMPACK与Python联合仿真——2. C程序代码编写与编译

继续上个博客《SIMPACK与Python联合仿真——1. 预备知识》内容。本文主要内容为SIMPACK模型搭建、C程序的编写与编译。

1. 实现案例

实现一个简单的案例,对一阶倒立摆SIMPACK模型施加时间历程为正弦曲线的力矩,控制力矩的数值由Python程序生成。

2. SIMPACK模型

1)在SIMPACK中新建General模型,保存为test01.spck。

建立Cylinder模型,杆长0.5m,直径0.01m

SIMPACK与Python联合仿真——2. C程序代码编写与编译_第1张图片

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获得的力矩,输入至倒立摆的铰接点,以驱动其旋转。

SIMPACK与Python联合仿真——2. C程序代码编写与编译_第2张图片

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可以避免很多潜在的因为对于强非线性量与突变量积分带来的错误。

SIMPACK与Python联合仿真——2. C程序代码编写与编译_第3张图片

 

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

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

你可能感兴趣的:(Simpack实时仿真,python)