KUKA iiwa 机器人实时控制 -- 4. 客户端FRI C++程序

HIT.roboT

已于 2024-04-30 14:43:34 修改

阅读量975

点赞数 20

分类专栏：工业机器人文章标签：机器人 c++

于 2024-04-30 11:49:56 首次发布

本文链接：https://blog.csdn.net/m0_65521841/article/details/138340621

版权

工业机器人专栏收录该内容

4 篇文章 1 订阅

订阅专栏

KUKA iiwa 机器人实时控制 – 4. 客户端FRI C++程序

本文将对KUKA iiwa 客户端FRI C++程序进行介绍。

本教程其他文章请通过下面链接查看
KUKA iiwa 机器人实时控制 – 1. FRI通信硬件连接
 KUKA iiwa 机器人实时控制 – 2. 基本环境配置
 KUKA iiwa 机器人实时控制 – 3. 控制柜FRI JAVA程序
KUKA iiwa 机器人实时控制 – 4. 客户端FRI C++程序

测试平台

KUKA 机器人：KUKA iiwa 14
KUKA iiwa编程软件： Sunrise Workbench 1.7
KUKA iiwa 通信包： KUKA Sunrise.Connectivity FRI 1.7
操作系统：

Windows 10 运行Sunrise Workbench软件
Ubuntu 20.04 运行 FRI 客户端程序

1. FRI 位置控制C++例程

下面以KUKA提供的FRI位置控制demoLBRJointSineOverlay为例进行介绍，该例程向控制柜以固定频率发送关节角度值，控制柜中的JAVA程序收到数据后，将控制KUKA iiwa 指定关节做正弦运动。

该程序 C++ 源代码可以在这里下载。
LBRJointSineOverlay 包含几个程序

LBRJointSineOverlayApp.cpp //主程序
LBRJointSineOverlayClient.cpp //FRI 发送指令的具体实现

(1) LBRJointSineOverlayApp.cpp

LBRJointSineOverlayApp.cpp是主程序，用于通信的初始化。下面代码中对重要语句进行了注释。

// LBRJointSineOverlayApp.cpp

#include <stdlib.h>
#include <stdio.h>
#include <string.h> // strstr
#include "LBRJointSineOverlayClient.h"
#include "friUdpConnection.h"
#include "friClientApplication.h"

using namespace KUKA::FRI;

#define DEFAULT_PORTID 30200 //定义通信端口，默认为30200， 不需要修改
#define DEFAULT_JOINTMASK 0x8 //关节使能掩码，关节对应为置1表示使能，0x8表示第4个关节使能
//下面参数用于配置 正弦运动
#define DEFAULT_FREQUENCY 0.25 
#define DEFAULT_AMPLITUDE 0.04
#define DEFAULT_FILTER_COEFFICIENT 0.99

int main (int argc, char** argv)
{
   // parse command line arguments
   if (argc > 1)
   {
	   if ( strstr (argv[1],"help") != NULL)
	   {
	      printf(
	            "\nKUKA LBR joint sine overlay test application\n\n"
	            "\tCommand line arguments:\n"
	            "\t1) remote hostname (optional)\n"
	            "\t2) port ID (optional)\n"
	            "\t3) bitmask encoding of joints to be overlayed (optional)\n"
	            "\t4) sine frequency in Hertz (optional)\n"
	            "\t5) sine amplitude in radians (optional)\n"
	            "\t6) filter coefficient from 0 (off) to 1 (optional)\n"            
	      );
	      return 1;
	   }
   }
   char* hostname = (argc >= 2) ? argv[1] : NULL;//hostname 不需要配置，使用默认值即可
   int port = (argc >= 3) ? atoi(argv[2]) : DEFAULT_PORTID;//使用默认端口
   unsigned int jointMask = (argc >= 4) ? (unsigned int)atoi(argv[3]) : DEFAULT_JOINTMASK;//关节使能掩码根据自己需要设置
   double frequency = (argc >= 5) ? atof(argv[4]) : DEFAULT_FREQUENCY;
   double amplitude = (argc >= 6) ? atof(argv[5]) : DEFAULT_AMPLITUDE;
   double filterCoeff = (argc >= 7) ? atof(argv[6]) : DEFAULT_FILTER_COEFFICIENT;

   // create new sine overlay client
   LBRJointSineOverlayClient client(jointMask, frequency, amplitude, filterCoeff);

   // create new udp connection
   UdpConnection connection;
   // pass connection and client to a new FRI client application
   ClientApplication app(connection, client);
   
   // connect client application to KUKA Sunrise controller
   app.connect(port, hostname);//与控制柜建立连接

   // repeatedly call the step routine to receive and process FRI packets
   bool success = true;
   while (success)
   {
   //该函数将以固定频率向控制柜发送关节角度值，并检测当前通信状态
   //通信频率在JAVA程序中通过 setSendPeriodMilliSec( ) 设置，客户端将服从控制柜的频率
      success = app.step();
   }

   // disconnect from controller
   app.disconnect();//停止FRI通信
   
   return 1;
}

(2) LBRJointSineOverlayClient.cpp

#include <cstring>
#include <cstdio>
#include "LBRJointSineOverlayClient.h"
#include "friLBRState.h"

// Visual studio needs extra define to use math constants
#define _USE_MATH_DEFINES
#include <cmath>

using namespace KUKA::FRI;
//******************************************************************************
LBRJointSineOverlayClient::LBRJointSineOverlayClient(unsigned int jointMask, 
      double freqHz, double amplRad, double filterCoeff) 
   : _jointMask(jointMask)
   , _freqHz(freqHz)
   , _amplRad(amplRad)
   , _filterCoeff(filterCoeff)
   , _offset(0.0)
   , _phi(0.0)
   , _stepWidth(0.0)
{
   printf("LBRJointSineOverlayClient initialized:\n"
         "\tjoint mask: 0x%x\n"
         "\tfrequency (Hz): %f\n"
         "\tamplitude (rad): %f\n"
         "\tfilterCoeff: %f\n",
         jointMask, freqHz, amplRad, filterCoeff);
}

//******************************************************************************
LBRJointSineOverlayClient::~LBRJointSineOverlayClient()
{
}
      
//******************************************************************************
void LBRJointSineOverlayClient::onStateChange(ESessionState oldState, ESessionState newState)
{
   LBRClient::onStateChange(oldState, newState);
   // (re)initialize sine parameters when entering Monitoring
   switch (newState)
   {
      case MONITORING_READY:
      {
         _offset = 0.0;
         _phi = 0.0;
         _stepWidth = 2 * M_PI * _freqHz * robotState().getSampleTime();
         break;
      }
      default:
      {
         break;
      }
   }
}
   
//******************************************************************************
void LBRJointSineOverlayClient::command()
{
   // calculate new offset
   double newOffset = _amplRad * sin(_phi);
   _offset = _offset * _filterCoeff + newOffset * (1.0 - _filterCoeff);
   _phi += _stepWidth;
   if (_phi >= 2 * M_PI) _phi -= 2 * M_PI;      
   // add offset to ipo joint position for all masked joints
   double jointPos[LBRState::NUMBER_OF_JOINTS];
   memcpy(jointPos, robotState().getIpoJointPosition(), LBRState::NUMBER_OF_JOINTS * sizeof(double));
   for (int i=0; i< LBRState::NUMBER_OF_JOINTS; i++)
   {
      if (_jointMask & (1<<i))
      {
         jointPos[i] += _offset;
      }
   }
   robotCommand().setJointPosition(jointPos);
}
//******************************************************************************
// clean up additional defines
#ifdef _USE_MATH_DEFINES
#undef _USE_MATH_DEFINES
#endif

command() 是我们实现自己算法所需要关注的函数，该函数中的 robotCommand().setJointPosition(jointPos) 向控制柜以固定频率发送角度值，单位为rad。

如需要发送自定的角度值，只需令jointPos设置为自定义的角度值即可，

注意！发送给相同关节的相邻的两个角度值间隔不宜过大，否则会出现关节速度过大的情况！

2. 代码编译与运行

在Ubuntu系统中解压下载的代码压缩包 FRI-Client-SDK_Cpp.zip，在终端中进入GNUMake文件夹(FRI-Client-SDK_Cpp/build/GNUMake)，执行命令

make

编译成功后，可执行程序位于bin文件夹（FRI-Client-SDK_Cpp/bin），使用终端进入该文件夹，输入下面的命令执行例程

./LBRJointSineOverlay

FRI客户端程序进入等待连接的状态…

请添加图片描述

使用Smartpad执行 LBRJointSineOverlay 程序，当出现下图信息，表示连接已成功建立。

注意！执行LBRJointSineOverlay程序前，请使用 Smartpad 将速度缩放比例调至10%以下，防止关节速度过快！

在这里插入图片描述

此时，KUKA iiwa机器人开始运动。FRI客户端最终打印信息如下图所示。程序结束后，会断开FRI连接。

请添加图片描述

执行完上面的程序，我们就完成了一次基于FRI通信的KUKA iiwa关节位置实时控制例程的运行。

3. 关节力控与笛卡尔力控

如果需要对各个关节或在笛卡尔坐标系下进行力控制，只需要参考本教程或者官方例程中的其他程序，替换掉setJointPosition()函数即可，控制柜JAVA程序中的关节控制模式也需要与之匹配，下面介绍两种修改方案。

(1) 关节扭矩控制模式

C++中command() 函数robotCommand().setJointPosition(...) 替换为 robotCommand().setTorque(...);
JAVA程序要进行下面修改

...
FRIJointOverlay overlay = new FRIJointOverlay (session, ClientCommandMode.TORQUE);
...
JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(200, 200, 200, 200, 200, 200, 200);
PositionHold posHold = new PositionHold(ctrMode, -1, TimeUnit.SECONDS);
_lbr.move(posHold.addMotionOverlay(jointOverlay));

(2) 笛卡尔力和扭矩控制模式

C++中command() 函数robotCommand().setJointPosition(...) 替换为 robotCommand().setWrench(...);

JAVA程序要进行下面修改

...
FRIJointOverlay overlay = new FRIJointOverlay (session, ClientCommandMode.WRENCH);
...
CartesianImpedanceControlMode ctrMode = new CartesianImpedanceControlMode();
PositionHold posHold = new PositionHold(ctrMode, -1, TimeUnit.SECONDS);
_lbr.move(posHold.addMotionOverlay(jointOverlay));