背景介绍
目前，市面上大多数的拖动试教机器人是UR的协作机器人和DLR-KUKA的iiwa机器人，相比于UR机器人，iiwa机器人在结构上有一点重要的不同。
UR在每个关节上采取的是双编码器的方式，分别测量电机角度和连杆角度。而iiwa机器人在每个关节上还加入了一个单轴力矩传感器（一般位于减速器输出端与末端连杆间），用于测量每个关节的输出力矩。如图所示，iiwa在牵引拖动时表现更好，同时iiwa机器人拥有更小的运动盲区。用户可以施加更小的力就可以牵着机器人按其意图运动，而不会担心机械臂运动受限。
ROS API说明
以iiwa为代表的七轴机械臂在高精度的工作中有着很重要的应用，但是作为以java语言为驱动的机械臂，相较于以python为驱动的UR机械臂而言，其上手难度更高，难以去进行二次开发。之前常用的开发主要通过SunriseWorkbench等软件进行的开发。难以满足现有的分布式控制需求。为此也有很多人通过使用ROSjava来对iiwa机器人进行开发。但是ROSjava已在几年前停止了维护。为此本文尝试通过TCP+ROS+Python的搭建方法来满足机械臂的驱动需求。本文为KUKA-iiwa 智能机器人提供了应用程序编程接口（API）。来支持后续的iiwa开发工作。该API建立在KUKA iiwa内核安全性的基础上，可实现紧密的工作并与操作员进行交互。它将功能引入了机器人操作系统（ROS），后者提供了一个分布式开发环境，同时允许多种新的设备形式轻松地进行接口。
API架构
开发的API设计得很简单，通过TCP通讯建立出一套方便与ROS交互开发的平台。API体系结构侧重于突破通常在Smartpad上运行的KUKA Sunrise控制器中可用的功能。通过使用下图所示的通用结构，可以看到该体系结构扩展了KUKA LBR iiwa的功能。API公开了在机器网络上操作的接口。这允许在KUKA LBR iiwa的操作中轻松部署和利用不同的传感方法和额外的计算资源。
KUKA-iiwa的守护程序还处理一些低级但通用和关键的控制任务，例如碰撞检测。这样的操作可以给我们的二次开发提供基础的安全保障。ROS-KUKA节点也以Python脚本语言实现，通过订阅ROS节点来使KUKA守护程序和ROS主节点之间起到中间接口的作用。通过python订阅来自其他控制ROS节点的命令，并将它们传递给KUKA服务端。这样可以实现良好的外部扩展。同样，它从KUKA服务端接收感官和状态信息，并通过ROS将其其发布到python中。从而实现在计算机上不安装KUKA Sunrise OS即可外部运行此ROS-KUKA节点，这意味着无需修改KUKA Sunrise Cabinet。因此，保留了KUKA提供的安全协议，并且添加了功能而不是对其进行了更改。无需在Sunrise OS上安装第三方软件（例如ROS），只需使用基于标准Sunrise的应用程序就可以设置KUKA iiwa ROS接口。
下表列出了可通过API调用的ROS主题以读取KUKA-iiwa内部的参数
| 订阅主题名称 | 描述 | 例子 |
| JointPosition [A1，A2，A3，A4，A5，A6，A7]time | 对应了iiwa7个自由度 | JointPosition [0.0, 0.17, 0.0, 1.92, 0.0,0.35, 0.0] 1459253274.1 |
| ToolPosition [X, Y, Z, A, B, C] time | 工具末端的位置以及三个旋转量 | ToolPosition [433.59711426170867,0.028881929589094864, 601.4449734558293,3.1414002368275726, 1.0471367465304213,3.141453681799645] 1459253274.11 |
| ToolForce [X, Y, Z] time | 力模式施加的力 | ToolForce [13.485958070668463,0.3785658886199012, 5.964988607372689] 1459253274.11’ |
| ToolTorque [A, B, C] time | 设置对应的三个扭矩参数 | ToolTorque [13.485958070668463, 0.3785658886199012, 5.964988607372689] 1459253274.11’ |
| JointAcceleration Float time | joint关节加速度值 | JointAcceleration 0.4 1459253274.11’ |
| JointVelocity Float time | joint关节速度值 | JointVelocity 1.0 1459253274.11’ |
| JointJerk Float time | 关节选取最优的jerk | JointJerk 1.0 1459253274.11’ |
| isCompliance Boolean time | 判断机器人状态是否符合规范 | isCompliance off 1459253274.11’ |
| isReadyToMove Boolean time | 判断机器人是否执行完所有步骤 | isReadyToMove true 1459253274.11’ |
| isCollision Boolean time | 检测是否碰撞 | isCollision false 1459253274.11’ |
| isMastered Boolean time | 是否为主程序 | isMastered true 1459253274.11’ |
| isJointOutOfRange Boolean time | 是否超出设定值范围 | isJointOutOfRange false 1459253274.11’ |
另外一个表列出了可通过字符串调用的ROS主题以向KUKA-iiwa发送指令
| 订阅主题名称 | 描述 | 例子 |
| setJointAcceleration F | 设定/变更关节加速度值 | ’setJointAcceleration 0.4’ |
| setJointVelocity F | 设定/更改关节速度值 | ’setJointVelocity 1.0’ |
| setJointJerk F | 设置/更改关节Jerk速度值 | ’setJointJerk 1.0’ |
| setPosition A1 A2 A3 A4 A5 A6 A7 ptp/lin | 根据关节位置移动机器人手臂。可以选择点对点（ptp）或线性（lin）运动。float类型的角度值（以度为单位）可以在A1-7中替换。如果不需要移动任何轴，可以使用-代替值。该示例为除A2不变的每个轴分配新位置。 | ’setPosition 0 21 0 -100 0 60 0 ptp’ |
| setPositionXYZABC X Y Z A B C ptp/lin | 在机器人笛卡尔空间中移动机器人末端执行器。可以选择点对点（ptp）或线性（lin）运动。这会将机器人末端执行器移动到特定位置[x，y，z]方向[a，b，c]（浮点中的值）。如果不需要更改任何参数，可以使用-代替值。 | ’setPositionXYZABC 700 0 290 - 180 0 -180 lin’ |
| MoveXYZABC X Y Z A B C | 仅通过点对点（ptp）运动在机器人笛卡尔空间中移动机器人末端执行器。对于给定值（以毫米和度为单位），这会将机器人末端执行器沿特定方向[x，y，z]和/或方向[a，b，c]移动。 | MoveXYZABC 10 20 0 30 0 0 |
| MoveCirc X1 Y1 Z1 A1 B1 C1 X2 Y2 Z2 A2 B2 C2 BlendingOri | 以给定的混合值将机器人末端执行器从第一个位置（ [x1 y1 z1 a1 b1c1]）的当前位置以弧形/圆周运动从其当前位置移动到第二个位置（[x2y2 z2 a2 b2 c2]） 。 | MoveCirc 700 0 290 -180 0 -180 710 0 300 -180 0 -180 0.1 |
| setCompliance X Y Z Z B C | 在每个x，y，z，a，b和c中具有特定刚度的情况下激活机器人的Compliance模式。给定的示例仅在x和y直角坐标系中以非常低的刚度激活Compliance。 | ’setCompliance 10 10 5000 300 300 300’ |
| resetCompliance | 停用机器人的“合规性”模式 | ’resetCompliance’ |
| resetCollision | 如果检测到任何碰撞，则重置碰撞 | ’resetCollision’ |
| forceStop | 停止机器人并删除所有等待执行的机器人运动队列 | ’forceStop’ |
| setWorkspace xmin ymin zmin xmax ymax zmax | 定义工作区边界 | ‘SetWorkspace 100 -300 0 600 300 500’ |
ROS环境搭建
安装api
如前所述，API有两个主要部分：1）KUKA守护程序（java）和2）ROSKUKA节点（Python）。KUKA守护程序可以作为标准KUKA应用程序安装在KUKA Sunrise Cabinet上：
1) 将“ ROS_API”文件夹作为新的Java包导入到您的KUKA Sunrise项目中（记得电脑与KUKA通过网线相连，并在Sunrise中发现示例程序后）。
ROS_API/ROS.java
package application;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.TimeUnit;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.PrintWriter;
import java.net.Socket;
import com.kuka.common.ThreadUtil;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import static com.kuka.roboticsAPI.motionModel.BasicMotions.*;
import com.kuka.roboticsAPI.conditionModel.ICallbackAction;
import com.kuka.roboticsAPI.conditionModel.ICondition;
import com.kuka.roboticsAPI.conditionModel.JointTorqueCondition;
import com.kuka.roboticsAPI.deviceModel.Device;
import com.kuka.roboticsAPI.deviceModel.JointEnum;
import com.kuka.roboticsAPI.deviceModel.JointPosition;
import com.kuka.roboticsAPI.deviceModel.LBR;
import com.kuka.roboticsAPI.executionModel.IFiredTriggerInfo;
import com.kuka.roboticsAPI.geometricModel.CartDOF;
import com.kuka.roboticsAPI.geometricModel.Frame;
import com.kuka.roboticsAPI.geometricModel.Tool;
import com.kuka.roboticsAPI.motionModel.ErrorHandlingAction;
import com.kuka.roboticsAPI.motionModel.IMotionContainer;
import com.kuka.roboticsAPI.motionModel.IErrorHandler;
import com.kuka.roboticsAPI.motionModel.MotionBatch;
import com.kuka.roboticsAPI.motionModel.OrientationReferenceSystem;
import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode;
import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
/*******************************************************
* 您需要在“模板数据->模板工具组”中创建一个tool1并将其命名为“ tool”。
* 在“ tool1”下插入新的空白框架，并将其命名为“ TCP”。
* 在“ tool1”的属性中，设置transformation值。
* 在“ tool1”的属性中，选择“ TCP”作为“Default Motion Frame”。
* 使机器人与Sunrise Workbench同步.
* 在机器人控制板上，对工具执行自动载荷数据确定.
*******************************************************/
public class ROS extends RoboticsAPIApplication {
public boolean RUN = false; // socket是否已连接并且应用程序正在运行？
public boolean isCompliance = false;//机器人是否为顺从模式
public boolean isCollision = false;//机器人是否碰撞
public LBR lbr; // Kuka iiwa机器人
public Tool tool;//导入tool工具
public PrintWriter outputStream;//输出数据流
public BufferedReader inputStream;//输入数据流
public Socket skt; // TCP Socket
public String CommandStr; // 命令字符串
String []strParams = new String[20];
float []params = new float[10];
public long LastReceivedTime = System.currentTimeMillis();//当前系统时间
public double JointAcceleration;//关节加速度
public double JointVelocity;//关节速度
public double JointJerk;//关节的加加速度
public double Acceleration = 1.0;//加速度
public double Velocity = 1.0;//速度
public double CartVelocity = 10;
public IMotionContainer handleCompliance;//处理顺从模式
public CartesianImpedanceControlMode cartImpCtrlMode = new CartesianImpedanceControlMode();//笛卡尔阻抗控制模式
public boolean isCartImpCtrlMode = false;//是CartImp Ctrl模式
private IErrorHandler errorHandler;
//===========================================================
public void initialize() {
getController("KUKA_Sunrise_Cabinet_1");//获取KUKA Sunrise Cabinet控制器
lbr = getContext().getDeviceFromType(LBR.class);//获取kuka机器人的编号
tool = getApplicationData().createFromTemplate("tool1");//获取template data 里面 tool1的参数
tool.attachTo(lbr.getFlange());//将对应的数据绑到机器人上
JointAcceleration = 1.0;
JointVelocity = 1.0;
JointJerk = 1.0;//加加速度
cartImpCtrlMode.parametrize(CartDOF.X).setStiffness(5000.0);//x方向的刚度设置，所需要的力
cartImpCtrlMode.parametrize(CartDOF.Y).setStiffness(5000.0);
cartImpCtrlMode.parametrize(CartDOF.Z).setStiffness(5000.0);
cartImpCtrlMode.parametrize(CartDOF.A).setStiffness(300.0);
cartImpCtrlMode.parametrize(CartDOF.B).setStiffness(300.0);
cartImpCtrlMode.parametrize(CartDOF.C).setStiffness(300.0);
cartImpCtrlMode.parametrize(CartDOF.ALL).setDamping(1.0);//设置阻尼
errorHandler = new IErrorHandler() {//触发的error打印
@Override
public ErrorHandlingAction handleError(Device device,
IMotionContainer failedContainer,
List<IMotionContainer> canceledContainers) {
getLogger().warn("Excecution of the following motion failed: "
+ failedContainer.getCommand().toString());
getLogger().info("The following motions will not be executed:");
for (int i = 0; i < canceledContainers.size(); i++) {
getLogger().info(canceledContainers.get(i)
.getCommand().toString());
}
return ErrorHandlingAction.Ignore;
}
};
getApplicationControl()
.registerMoveAsyncErrorHandler(errorHandler);//注册移动异步错误处理程序
}
public void socketConnection() // 正在连接到服务器 172.31.1.50 Port:1234
{
System.out.println("Connecting to server at 172.31.1.50 Port:1234");
while (true){
try{
skt = new Socket("172.31.1.50", 1234); // 如果需要，请根据运行ROS-KUKA节点服务器的系统修改IP和端口
System.out.println("KUKA iiwa is connected to the server.");
break;
}
catch(IOException e1){
System.out.println("ERROR connecting to the server!");
}
}
try{
outputStream = new PrintWriter(skt.getOutputStream(), true);//打印出数据
inputStream = new BufferedReader(new InputStreamReader(skt.getInputStream()));//从Buffer读取数据
RUN = true;
}
catch(IOException e)
{
System.out.println("Error creating inPort and outPort");
}
}
public void reply(PrintWriter outputStream, String buffer)//调用outputstream输出对应的指令
{
outputStream.write(buffer);
outputStream.flush();
}
public String getLine(BufferedReader inputStream)
{
String line;
try{
while(!inputStream.ready()){}
line = inputStream.readLine();
//System.out.println("Command received: " + line);
return line;
}
catch(Exception e){
return "Error command";
}
}
public void behaviourAfterCollision() {
isCollision = true;//将是否碰撞设为真
IMotionContainer handle;
CartesianImpedanceControlMode CollisionSoft = new CartesianImpedanceControlMode();//笛卡尔阻抗控制模式
CollisionSoft.parametrize(CartDOF.ALL).setDamping(.7);
CollisionSoft.parametrize(CartDOF.ROT).setStiffness(100);
CollisionSoft.parametrize(CartDOF.TRANSL).setStiffness(600);
handle = tool.moveAsync(positionHold(CollisionSoft, -1, TimeUnit.SECONDS));
/*getApplicationUI().displayModalDialog(ApplicationDialogType.WARNING,
"Collision Has Occurred!\n\n LBR is compliant...",
"Continue");*/
handle.cancel();
}
public ICondition defineSensitivity() {
//double sensCLS = getApplicationData().getProcessData("sensCLS").getValue(); // Uncomment if you have "sensCLS" defined.
double sensCLS = 30; // Modify the value if required.
//Offsetkompensation
double actTJ1 = lbr.getExternalTorque().getSingleTorqueValue(JointEnum.J1);
double actTJ2 = lbr.getExternalTorque().getSingleTorqueValue(JointEnum.J2);
double actTJ3 = lbr.getExternalTorque().getSingleTorqueValue(JointEnum.J3);
double actTJ4 = lbr.getExternalTorque().getSingleTorqueValue(JointEnum.J4);
double actTJ5 = lbr.getExternalTorque().getSingleTorqueValue(JointEnum.J5);
double actTJ6 = lbr.getExternalTorque().getSingleTorqueValue(JointEnum.J6);
double actTJ7 = lbr.getExternalTorque().getSingleTorqueValue(JointEnum.J7);
//Abbruchbedingungen pro Achse
JointTorqueCondition jt1 = new JointTorqueCondition(JointEnum.J1, -sensCLS+actTJ1, sensCLS+actTJ1);
JointTorqueCondition jt2 = new JointTorqueCondition(JointEnum.J2, -sensCLS+actTJ2, sensCLS+actTJ2);
JointTorqueCondition jt3 = new JointTorqueCondition(JointEnum.J3, -sensCLS+actTJ3, sensCLS+actTJ3);
JointTorqueCondition jt4 = new JointTorqueCondition(JointEnum.J4, -sensCLS+actTJ4, sensCLS+actTJ4);
JointTorqueCondition jt5 = new JointTorqueCondition(JointEnum.J5, -sensCLS+actTJ5, sensCLS+actTJ5);
JointTorqueCondition jt6 = new JointTorqueCondition(JointEnum.J6, -sensCLS+actTJ6, sensCLS+actTJ6);
JointTorqueCondition jt7 = new JointTorqueCondition(JointEnum.J7, -sensCLS+actTJ7, sensCLS+actTJ7);
ICondition forceCon = jt1.or(jt2, jt3, jt4, jt5, jt6, jt7);
return forceCon;
}
private IMotionContainer _currentMotion;//当前运动状态
public void MoveSafe(MotionBatch MB) {
ICondition forceCon = defineSensitivity();//定义灵敏度
ICallbackAction ica = new ICallbackAction() {//回调行为
@Override//override伪代码，表示方法重写，对ica.onTriggerFire调用
public void onTriggerFired(IFiredTriggerInfo triggerInformation)
{
triggerInformation.getMotionContainer().cancel();//获取运动容器
behaviourAfterCollision();//在碰撞后
}
};
if(isCartImpCtrlMode)//是否CartImp Ctrl模式
{
MB.setJointAccelerationRel(JointAcceleration)
.setJointVelocityRel(JointVelocity)
.setMode(cartImpCtrlMode)
.triggerWhen(forceCon, ica);
}
else
{
MB.setJointAccelerationRel(JointAcceleration)
.setJointVelocityRel(JointVelocity)
.triggerWhen(forceCon, ica);
}
if(lbr.isReadyToMove())//是否准备移动
this._currentMotion=tool.moveAsync(MB);//异步移动
}
//===========================================================
public void setTool(String message){ // 设置tool的名字，receives: "setTool TOOLNAME"
strParams = message.split(" ");
if (strParams.length==2){
tool = getApplicationData().createFromTemplate(strParams[1]);
tool.attachTo(lbr.getFlange());
getLogger().info("Switched to " + strParams[1]);
}
else{
getLogger().info("Unacceptable 'setTool' command!");
}
}
public void setJointAcceleration(String message){
strParams = message.split(" ");
if (strParams.length==2)
params[0] = Float.parseFloat(strParams[1]);
if (0.0 < params[0] && params[0] <= 1.0)
JointAcceleration = params[0];
else
getLogger().info("JointAcceleration must be 0<JA<=1");
}
public void setJointVelocity(String message){
strParams = message.split(" ");
if (strParams.length==2)
params[0] = Float.parseFloat(strParams[1]);
if (0.0 < params[0] && params[0] <= 1.0)
JointVelocity = params[0];
else
getLogger().info("JointVelocity must be 0<JV<=1");
}
public void setJointJerk(String message){//加加速度
strParams = message.split(" ");
if (strParams.length==2)
params[0] = Float.parseFloat(strParams[1]);
if (0.0 < params[0] && params[0] <= 1.0)
JointJerk = params[0];
else
getLogger().info("JointJerk must be 0<JJ<=1");
}
public void CartVelocity(String message){
strParams = message.split(" ");
if (strParams.length==2)
params[0] = Float.parseFloat(strParams[1]);
if (0.0 < params[0] && params[0] <= 10000)
CartVelocity = params[0];
else
getLogger().info("CartVelocity must be 0<CV<=10000");
}
public void setPosition(String message) // "setPosition A0 A1 - A3 A4 A5 A6" 这只是ptp动作
{ strParams = message.split(" ");
double A0 = lbr.getCurrentJointPosition().get(0);
double A1 = lbr.getCurrentJointPosition().get(1);
double A2 = lbr.getCurrentJointPosition().get(2);
double A3 = lbr.getCurrentJointPosition().get(3);
double A4 = lbr.getCurrentJointPosition().get(4);
double A5 = lbr.getCurrentJointPosition().get(5);
double A6 = lbr.getCurrentJointPosition().get(6);
if (strParams.length==8){
if( !strParams[1].equals("-") )
A0 = Math.toRadians( Float.parseFloat(strParams[1]) );
if( !strParams[2].equals("-") )
A1 = Math.toRadians( Float.parseFloat(strParams[2]) );
if( !strParams[3].equals("-") )
A2 = Math.toRadians( Float.parseFloat(strParams[3]) );
if( !strParams[4].equals("-") )
A3 = Math.toRadians( Float.parseFloat(strParams[4]) );
if( !strParams[5].equals("-") )
A4 = Math.toRadians( Float.parseFloat(strParams[5]) );
if( !strParams[6].equals("-") )
A5 = Math.toRadians( Float.parseFloat(strParams[6]) );
if( !strParams[7].equals("-") )
A6 = Math.toRadians( Float.parseFloat(strParams[7]) );
MotionBatch motion = new MotionBatch(ptp(A0,A1,A2,A3,A4,A5,A6).setJointJerkRel(JointJerk));//设置对应的7个角度
MoveSafe(motion);//移动到该点
}
else{
getLogger().info("Unacceptable 'setPosition' command!");
}
}
public void setPositionXYZABC(String message) // setPositionXYZABC x y z a - c ptp/lin
{
double x = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getX();
double y = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getY();
double z = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getZ();
double a = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getAlphaRad();
double b = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getBetaRad();
double c = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getGammaRad();
strParams = message.split(" ");
if (strParams.length==8){
if( !strParams[1].equals("-"))
x = Float.parseFloat(strParams[1]);
if( !strParams[2].equals("-"))
y = Float.parseFloat(strParams[2]);
if( !strParams[3].equals("-"))
z = Float.parseFloat(strParams[3]);
if( !strParams[4].equals("-"))
a = Math.toRadians( Float.parseFloat(strParams[4]) );
if( !strParams[5].equals("-"))
b = Math.toRadians( Float.parseFloat(strParams[5]) );
if( !strParams[6].equals("-"))
c = Math.toRadians( Float.parseFloat(strParams[6]) );
Frame f = new Frame(getApplicationData().getFrame("/Air"), x, y, z, a, b, c );//获取在Air下的对应参数，转化为7个角度值
if( strParams[7].equals("ptp"))//两种配置方法
MoveSafe( new MotionBatch(ptp(f).setJointJerkRel(JointJerk) ));
else if( strParams[7].equals("lin"))
MoveSafe( new MotionBatch(lin(f).setJointJerkRel(JointJerk).setCartVelocity(CartVelocity) ));
else
getLogger().info("Unacceptable motion! ptp/lin ?");
}
else{
getLogger().info("Unacceptable setPositionXYZABC command!");
}
}
public void MoveXYZABC(String message) // "MoveXYZABC x y 0 a 0 c" 这是一个动作
{
strParams = message.split(" ");
if (strParams.length==7){
double x = Float.parseFloat(strParams[1]);
double y = Float.parseFloat(strParams[2]);
double z = Float.parseFloat(strParams[3]);
double a = Math.toRadians( Float.parseFloat(strParams[4]) );
double b = Math.toRadians( Float.parseFloat(strParams[5]) );
double c = Math.toRadians( Float.parseFloat(strParams[6]) );
MotionBatch motion = new MotionBatch(linRel(x, y, z, a, b, c).setJointJerkRel(JointJerk).setCartVelocity(CartVelocity));//设置末端的位置和距离
MoveSafe(motion);//移动到该点
}
else{
getLogger().info("Unacceptable MoveXYZABC command!");
}
}
public void MoveCirc(String message) // "MoveCirc x1 y1 z1 a1 b1 c1 x2 y2 z2 a2 b2 c2 blending"
{
strParams = message.split(" ");
if (strParams.length==14){
double x1 = Float.parseFloat(strParams[1]);
double y1 = Float.parseFloat(strParams[2]);
double z1 = Float.parseFloat(strParams[3]);
double a1 = Math.toRadians( Float.parseFloat(strParams[4]) );
double b1 = Math.toRadians( Float.parseFloat(strParams[5]) );
double c1 = Math.toRadians( Float.parseFloat(strParams[6]) );
double x2 = Float.parseFloat(strParams[7]);
double y2 = Float.parseFloat(strParams[8]);
double z2 = Float.parseFloat(strParams[9]);
double a2 = Math.toRadians( Float.parseFloat(strParams[10]) );
double b2 = Math.toRadians( Float.parseFloat(strParams[11]) );
double c2 = Math.toRadians( Float.parseFloat(strParams[12]) );
double BlendingOri = Math.toRadians( Float.parseFloat(strParams[13]) );
Frame f1 = new Frame(getApplicationData().getFrame("/Air"), x1, y1, z1, a1, b1, c1 );
Frame f2 = new Frame(getApplicationData().getFrame("/Air"), x2, y2, z2, a2, b2, c2 );
MoveSafe( new MotionBatch( circ(f1, f2).setBlendingOri(BlendingOri).setOrientationReferenceSystem(OrientationReferenceSystem.Path).setCartVelocity(CartVelocity) ));
}
else{
getLogger().info("Unacceptable MoveCirc command!");
}
}
public void setForceStop(){//设置力停止
try{
if(!(this._currentMotion.isFinished() ||
this._currentMotion.hasError()))//判断是否停止或者存在问题
this._currentMotion.cancel();//如果没有停止并且没有报错，则退出
}
catch(Exception e){
// nop
};
CartesianImpedanceControlMode ForceStop = new CartesianImpedanceControlMode();//笛卡尔阻抗控制模式
ForceStop.parametrize(CartDOF.ALL).setDamping(.7);//设置所有阻尼为0.7
ForceStop.parametrize(CartDOF.ROT).setStiffness(300);//设置所有旋转刚度为300
ForceStop.parametrize(CartDOF.TRANSL).setStiffness(5000);//设置转动关节刚度为5000
handleCompliance = lbr.moveAsync(positionHold(ForceStop, 0, TimeUnit.SECONDS));//异步移动
handleCompliance.cancel();
isCompliance = false;//设置不为顺从模式
behaviourAfterCollision();//当锁死后触发
getLogger().info("ForceStop!");
}
public void setCompliance(String message){ //(为顺从模式) "setCompliance 10 10 5000 300 300 300"
// setCompliance "X-Stiffness" "Y-Stiffness" "Z-Stiffness" "A-R-Stiffness" "b-R-Stiffness" "c-R-Stiffness"
// x,y = 10 not to soft (robot stands by its own)
// x = 5000 max Stiffness.
// az, by, cx = 300 max rotational Stiffness.
CartesianImpedanceControlMode soft = new CartesianImpedanceControlMode();
strParams = message.split(" ");
if (strParams.length==7){
float x = Float.parseFloat(strParams[1]);
float y = Float.parseFloat(strParams[2]);
float z = Float.parseFloat(strParams[3]);
float az = Float.parseFloat(strParams[4]);
float by = Float.parseFloat(strParams[5]);
float cx = Float.parseFloat(strParams[6]);
soft.parametrize(CartDOF.ALL).setDamping(0.7);
soft.parametrize(CartDOF.X).setStiffness(x);
soft.parametrize(CartDOF.Y).setStiffness(y);
soft.parametrize(CartDOF.Z).setStiffness(z);
soft.parametrize(CartDOF.A).setStiffness(az);
soft.parametrize(CartDOF.B).setStiffness(by);
soft.parametrize(CartDOF.C).setStiffness(cx);
if (isCompliance)
handleCompliance.cancel();
handleCompliance = tool.moveAsync(positionHold(soft, -1, TimeUnit.SECONDS));
isCompliance = true;
getLogger().info("Compliance is ON");
}
else{
getLogger().info("Unacceptable setCompliance command!");
}
}
public void setCartImpCtrl(String message){ // "setCartImpCtrl 5000 5000 500 300 300 300 Damping"
strParams = message.split(" ");
if (strParams.length==8){
float x = Float.parseFloat(strParams[1]);
float y = Float.parseFloat(strParams[2]);
float z = Float.parseFloat(strParams[3]);
float az = Float.parseFloat(strParams[4]);
float by = Float.parseFloat(strParams[5]);
float cx = Float.parseFloat(strParams[6]);
float Damping = Float.parseFloat(strParams[7]);
cartImpCtrlMode.parametrize(CartDOF.X).setStiffness(x);
cartImpCtrlMode.parametrize(CartDOF.Y).setStiffness(y);
cartImpCtrlMode.parametrize(CartDOF.Z).setStiffness(z);
cartImpCtrlMode.parametrize(CartDOF.A).setStiffness(az);
cartImpCtrlMode.parametrize(CartDOF.B).setStiffness(by);
cartImpCtrlMode.parametrize(CartDOF.C).setStiffness(cx);
if (0.0 < Damping && Damping <= 1.0)
cartImpCtrlMode.parametrize(CartDOF.ALL).setDamping(Damping);
else{
cartImpCtrlMode.parametrize(CartDOF.ALL).setDamping(1.0);
getLogger().info("CartImpCtrl: Damping must be 0<JA<=1. Damping set to 1.0");
}
isCartImpCtrlMode = true;
getLogger().info("New CartImpCtrl mode is set.");
}
else{
getLogger().info("Unacceptable CartImpCtrl command!");
}
}
public void resetCartImpCtrl(){//重置对应的状态
cartImpCtrlMode.parametrize(CartDOF.X).setStiffness(5000);
cartImpCtrlMode.parametrize(CartDOF.Y).setStiffness(5000);
cartImpCtrlMode.parametrize(CartDOF.Z).setStiffness(5000);
cartImpCtrlMode.parametrize(CartDOF.ROT).setStiffness(300);
cartImpCtrlMode.parametrize(CartDOF.ALL).setDamping(1.0);
isCartImpCtrlMode = false;
getLogger().info("CartImpCtrl is OFF!");
}
public void resetCompliance(){
CartesianImpedanceControlMode resetComp = new CartesianImpedanceControlMode();
resetComp.parametrize(CartDOF.ALL).setDamping(.7);
resetComp.parametrize(CartDOF.ROT).setStiffness(300);
resetComp.parametrize(CartDOF.TRANSL).setStiffness(5000);
if (isCompliance){
handleCompliance.cancel();
handleCompliance = tool.moveAsync(positionHold(resetComp, 0, TimeUnit.SECONDS));
isCompliance = false;
}
getLogger().info("Compliance is OFF");
}
public void getJointPosition(PrintWriter outputStream)//获取关节角度
{
LastReceivedTime = System.currentTimeMillis();//获取当前的系统时间
/*对应的7个角度参数值*/
double a0 = Math.toDegrees( lbr.getCurrentJointPosition().get(0) );
double a1 = Math.toDegrees( lbr.getCurrentJointPosition().get(1) );
double a2 = Math.toDegrees( lbr.getCurrentJointPosition().get(2) );
double a3 = Math.toDegrees( lbr.getCurrentJointPosition().get(3) );
double a4 = Math.toDegrees( lbr.getCurrentJointPosition().get(4) );
double a5 = Math.toDegrees( lbr.getCurrentJointPosition().get(5) );
double a6 = Math.toDegrees( lbr.getCurrentJointPosition().get(6) );
/*加入间隔等待发送 */
String buffer = Double.toString(a0) + ", " + Double.toString(a1) + ", "
+ Double.toString(a2) + ", " + Double.toString(a3) + ", "
+ Double.toString(a4) + ", " + Double.toString(a5) + ", "
+ Double.toString(a6);
reply(outputStream, ">"+"Joint_Pos " + buffer);//按照对应格式发送指令
}
public void getToolPosition(PrintWriter outputStream)//获取当前的工具末端参数
{
//三轴所在tcp位置
double x = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getX();
double y = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getY();
double z = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getZ();
//角度
double a = Math.toDegrees( lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getAlphaRad() );
double b = Math.toDegrees( lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getBetaRad() );
double g = Math.toDegrees( lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getGammaRad() );
String buffer = Double.toString(x) + ", " + Double.toString(y) + ", " + Double.toString(z) + ", " + Double.toString(a) + ", " + Double.toString(b) + ", " + Double.toString(g);
LastReceivedTime = System.currentTimeMillis();
reply(outputStream, ">"+"Tool_Pos " + buffer);
}
public void getIsCompliance(PrintWriter outputStream)//根据boll判断是否为顺从模式
{
LastReceivedTime = System.currentTimeMillis();
if (isCompliance)
reply(outputStream, ">"+"isCompliance " + "true");
else
reply(outputStream, ">"+"isCompliance " + "false");
}
public void getToolForce(PrintWriter outputStream)////获取工具对应的所收的到力
{
double x = lbr.getExternalForceTorque(tool.getDefaultMotionFrame()).getForce().getX();
double y = lbr.getExternalForceTorque(tool.getDefaultMotionFrame()).getForce().getY();
double z = lbr.getExternalForceTorque(tool.getDefaultMotionFrame()).getForce().getZ();
String buffer = Double.toString(x) + ", " + Double.toString(y) + ", " + Double.toString(z);
LastReceivedTime = System.currentTimeMillis();
reply(outputStream, ">"+"Tool_Force " + buffer);
}
public void getToolTorque(PrintWriter outputStream)//对应的工具扭矩
{
double x = lbr.getExternalForceTorque(tool.getDefaultMotionFrame()).getTorque().getX();
double y = lbr.getExternalForceTorque(tool.getDefaultMotionFrame()).getTorque().getY();
double z = lbr.getExternalForceTorque(tool.getDefaultMotionFrame()).getTorque().getZ();
String buffer = Double.toString(x) + ", " + Double.toString(y) + ", " + Double.toString(z);
LastReceivedTime = System.currentTimeMillis();
reply(outputStream, ">"+"Tool_Torque " + buffer);
}
public void getJointAcceleration(PrintWriter outputStream)//关节加速度
{
LastReceivedTime = System.currentTimeMillis();
reply(outputStream, ">"+"JointAcceleration " + Double.toString(JointAcceleration));
}
public void getJointVelocity(PrintWriter outputStream)//关节速度
{
LastReceivedTime = System.currentTimeMillis();
reply(outputStream, ">"+"JointVelocity " + Double.toString(JointVelocity));
}
public void getJointJerk(PrintWriter outputStream)//关节加加速度
{
LastReceivedTime = System.currentTimeMillis();
reply(outputStream, ">"+"JointJerk " + Double.toString(JointJerk));
}
public void getIsCollision(PrintWriter outputStream)//是否碰撞
{
LastReceivedTime = System.currentTimeMillis();
if (isCollision)
reply(outputStream, ">"+"isCollision " + "true");
else
reply(outputStream, ">"+"isCollision " + "false");
}
public void getIsReadyToMove(PrintWriter outputStream)//是否传输完毕，进行移动
{
LastReceivedTime = System.currentTimeMillis();
if (lbr.isReadyToMove())
reply(outputStream, ">"+"isReadyToMove " + "true");
else
reply(outputStream, ">"+"isReadyToMove " + "false");
}
public void getIsMastered(PrintWriter outputStream)//发送是否为主节点信息
{
LastReceivedTime = System.currentTimeMillis();
if (lbr.isMastered())
reply(outputStream, ">"+"isMastered " + "true");
else
reply(outputStream, ">"+"isMastered " + "false");
}
private void JointOutOfRange() { //关节超出范围
CartesianImpedanceControlMode OutOfRange = new CartesianImpedanceControlMode();//笛卡尔阻抗控制模式
OutOfRange.parametrize(CartDOF.ALL).setDamping(.7);
OutOfRange.parametrize(CartDOF.ROT).setStiffness(300);
OutOfRange.parametrize(CartDOF.TRANSL).setStiffness(5000);
IMotionContainer handle;
handle = tool.moveAsync(positionHold(OutOfRange, -1, TimeUnit.SECONDS));
getApplicationUI().displayModalDialog(ApplicationDialogType.WARNING,
"Joint Out Of Range!\n\n",
"Continue");//在UI中显示问题
handle.cancel();
}
private void getOperationMode(PrintWriter outputStream) {//操作模式
String opMode = lbr.getOperationMode().toString();
reply(outputStream, ">"+"OperationMode " + opMode);
}
public void sleep(String message){ // sleep in seconds.
strParams = message.split(" ");
float delay;
if (strParams.length==2)
{
delay = Float.parseFloat(strParams[1]);
if(delay >= 0)
ThreadUtil.milliSleep((long) (delay*1000));
else
getLogger().info("Delay cannot be a negative value!");
}
else
getLogger().info("Unacceptable sleep command!");
}
public void getIsFinished(PrintWriter outputStream)//获取运动是否结束
{
LastReceivedTime = System.currentTimeMillis();
try{
if(this._currentMotion.isFinished())//判断是否已经运行结束
reply(outputStream, ">"+"isFinished " + "true");
else
reply(outputStream, ">"+"isFinished " + "false");
}
catch(Exception e){
reply(outputStream, ">"+"isFinished " + "true");
};
}
public void getHasError(PrintWriter outputStream)//是否存在错误
{
LastReceivedTime = System.currentTimeMillis();
try{
if(this._currentMotion.hasError())//判断当前电机状态是否出现问题
reply(outputStream, ">"+"hasError " + "true");
else
reply(outputStream, ">"+"hasError " + "false");
}
catch(Exception e){
reply(outputStream, ">"+"hasError " + "true");
};
}
//===========================================================
public Thread Send_iiwa_data = new Thread(){//发送iiwa的数据
public void run(){
while (RUN)
{
getJointPosition(outputStream);//向ros发送当前的关节角
getToolPosition(outputStream);//向ros发送末端工具的位置
getToolForce(outputStream);//向ros发送工具对应的收到的力
getToolTorque(outputStream);//向ros发送工具扭矩
getIsCompliance(outputStream);//向ros发送合规性
getJointAcceleration(outputStream);//向ros发送关节加速度
getJointVelocity(outputStream);//向ros发送关节速度
getJointJerk(outputStream);//向ros发送关节加加速度
getIsCollision(outputStream);//向ros发送是否碰撞
getIsReadyToMove(outputStream);//向ros发送是否传输完毕，进行移动
getIsMastered(outputStream);//向ros发送发送是否为主节点信息
getOperationMode(outputStream);//向ros发送操作模式
getIsFinished(outputStream);//向ros发送运动是否结束
getHasError(outputStream);//向ros发送是否存在错误
ThreadUtil.milliSleep(100);//10hz频率
}
}
};
public Thread MonitorWorkspace = new Thread(){
JointPosition pos_last;
JointPosition pos_tmp;
double x;
double y;
double z;
public void run(){
while (RUN)
{
//============================================
pos_tmp = lbr.getCurrentJointPosition();//获取7个角度，并对其限制
if ((isCompliance) &&
(Math.toDegrees(pos_tmp.get(0)) < -165 || Math.toDegrees(pos_tmp.get(0)) > 165 ||
Math.toDegrees(pos_tmp.get(1)) < -115 || Math.toDegrees(pos_tmp.get(1)) > 115 ||
Math.toDegrees(pos_tmp.get(2)) < -165 || Math.toDegrees(pos_tmp.get(2)) > 165 ||
Math.toDegrees(pos_tmp.get(3)) < -115 || Math.toDegrees(pos_tmp.get(3)) > 115 ||
Math.toDegrees(pos_tmp.get(4)) < -165 || Math.toDegrees(pos_tmp.get(4)) > 165 ||
Math.toDegrees(pos_tmp.get(5)) < -115 || Math.toDegrees(pos_tmp.get(5)) > 115 ||
Math.toDegrees(pos_tmp.get(6)) < -170 || Math.toDegrees(pos_tmp.get(6)) > 170 ))//是否符合顺从模式，但是数值不在此期间
{
setForceStop();//强制停止
MoveSafe(new MotionBatch(ptp(pos_last).setJointJerkRel(JointJerk)));//恢复上一个状态，限定一定的加加速度进行平滑移动
JointOutOfRange();////关节超出范围后发送信息
}
else
pos_last = pos_tmp;//将上一个命令保存
//============================================获取现在的X，Y，Z参数
x = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getX();
y = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getY();
z = lbr.getCurrentCartesianPosition(tool.getDefaultMotionFrame()).getZ();
/*
if isInGreen(x,y,z): //Green zone
{
JointVelocity = 1;
JointAcceleration = 1;
}
else if isInYello(x,y,z): //Yello zone
{
JointVelocity = 1;
JointAcceleration = 1;
}
else if isInRead(x,y,z) //Red zone
{
JointVelocity = 1;
JointAcceleration = 1;
}
*/
//============================================
ThreadUtil.milliSleep(10);
}
}
};
//===========================================================
public void run() {
socketConnection();//使用socket链接
Send_iiwa_data.start();//创建一个新的thread实现
MonitorWorkspace.start();//监控工作区
while( RUN )//其余的一直在循环获得数据
{
CommandStr = getLine(inputStream);
String []lineSplt = CommandStr.split(" ");//分割对应指令
if( (lineSplt[0].toString()).equals("setPosition".toString()))
setPosition(CommandStr);//根据关节位置移动机器人手臂。可以选择点对点（ptp）或线性（lin）运动
if( (lineSplt[0].toString()).equals("setPositionXYZABC".toString()))
setPositionXYZABC(CommandStr);//在机器人笛卡尔空间中移动机器人末端执行器。可以选择点对点（ptp）或线性（lin）运动。这会将机器人末端执行器移动到特定位置[x，y，z]方向[a，b，c]（浮点中的值）。如果不需要更改任何参数，可以使用-代替值
if( (lineSplt[0].toString()).equals("MoveXYZABC".toString()))
MoveXYZABC(CommandStr);//仅通过点对点（ptp）运动在机器人笛卡尔空间中移动机器人末端执行器。对于给定值（以毫米和度为单位），这会将机器人末端执行器沿特定方向[x，y，z]和/或方向[a，b，c]移动。
if( (lineSplt[0].toString()).equals("setCompliance".toString()))
setCompliance(CommandStr);//在每个x，y，z，a，b和c中具有特定刚度的情况下激活机器人的Compliance模式。给定的示例仅在x和y直角坐标系中以非常低的刚度激活Compliance(即为顺从模式)。
if( (lineSplt[0].toString()).equals("resetCartImpCtrl".toString()))
resetCartImpCtrl();//重置对应的
if( (lineSplt[0].toString()).equals("resetCompliance".toString()))
resetCompliance();//停用机器人的“Compliance”模式(即为顺从模式)
if( (lineSplt[0].toString()).equals("setCartImpCtrl".toString()))
setCartImpCtrl(CommandStr);
if( (lineSplt[0].toString()).equals("setTool".toString()))
setTool(CommandStr);//设置工具名称
if( (lineSplt[0].toString()).equals("setJointAcceleration".toString()))
setJointAcceleration(CommandStr);//设置对应的加速度
if( (lineSplt[0].toString()).equals("setJointVelocity".toString()))
setJointVelocity(CommandStr);//设置对应的速度
if( (lineSplt[0].toString()).equals("setJointJerk".toString()))
setJointJerk(CommandStr);//上述空间的切空间（加加速度，jerk）进行随机采样
if( (lineSplt[0].toString()).equals("setCartVelocity".toString()))
CartVelocity(CommandStr);//对应的速度设置
if( (lineSplt[0].toString()).equals("resetCollision".toString()))
isCollision = false;//如果检测到任何碰撞，则重置碰撞
if( (lineSplt[0].toString()).equals("forceStop".toString()))
setForceStop();//停止机器人并删除所有等待执行的机器人运动队列
if( (lineSplt[0].toString()).equals("MoveCirc".toString()))
MoveCirc(CommandStr);//以给定的混合值将机器人末端执行器 从第一个位置（ [x1 y1 z1 a1 b1 c1]）的当前位置以弧形/圆周运动从 其当前位置移动到第二个位置（[x2 y2 z2 a2 b2 c2]）
if( (lineSplt[0].toString()).equals("sleep".toString()))
sleep(CommandStr);
if( skt.isInputShutdown() || !skt.isConnected() || skt.isOutputShutdown() || skt.isClosed())
{
try {
skt.close();
} catch (IOException e) {
System.out.println("ERROR closing the port!");
}
RUN = false;
}
}
System.out.println("- - - APPLICATION TERMINATED - - -");
}
//===========================================================
/**
* Auto-generated method stub. Do not modify the contents of this method.
*/
public static void main(String[] args){
API_ROS_KUKA_V30032017 app = new API_ROS_KUKA_V30032017();
app.runApplication();
}
}
2) 您需要创建一个名称为“ Air”的新框架（参见下图）：
a) 在包资源管理器中选择项目。
b) 右键单击“应用程序”数据视图中的“World”，然后从上下文菜单中选择“Insert new frame”。创建新框架并将其作为父框架的子元素插入框架树中。
c) 系统自动生成框架名称。在“属性”视图中将名称更改为“ Air”。
d) 确保转换（x，y，z，a，b，c）的所有值均为0。
3) 创建一个默认工具（参见下图）：
a) 在包资源管理器中选择项目。
b) 在模板数据视图中，打开对象模板列表。
c) 右键单击“工具”的“模板组”，然后选择“名称为“ tool1”的新工具”。
d) 在“属性”视图中，输入框架相对于其父框架的变换数据（X，Y，Z：框架沿父框架轴的偏移量）（A，B，C：框架相对于参考的方向到父框架）
4) KUKA守护程序默认以IP地址172.31.1.50（在有线连接处设置固定IP，从而固定访问IP地址）和Port：1234连接到ROS-KUKA节点服务器。如果需要，请在“ API_ROS_KUKA_V30032017.java”行中更改此地址：skt = new Socket（“ 172.31.1.50”，1234）;并在conf.txt文件中修改“server 172.31.1.50 port 1234”
5) 所有运动命令均通过称为“ MoveSafe”的功能执行。此功能对所有运动施加外力控制。如果检测到外力，则运动会终止。可以通过更改“ defineSensitivity()”函数中的“ sensCLS”值来修改MoveSafe的灵敏度。您可能需要根据任务更改默认值。
6) 将项目与机器人控制器同步。
相关运行示例程序
在终端运行
roscore
rosrun kuka_controller server_V30032017.py
server_V30032017.py
#!/usr/bin/env python
# KUKA API for ROS
version = 'V15032017++'
# 该脚本生成用于与KUKA iiwa通信的ROS节点
# 依赖项：conf.txt，ROS服务器，Rospy，KUKA iiwa java SDK，OPEN机械手。
#######################################################################################################################
import thread, time, os
import rospy
from std_msgs.msg import String
def cl_black(msge): return '\033[30m'+msge+'\033[0m'
def cl_red(msge): return '\033[31m'+msge+'\033[0m'
def cl_green(msge): return '\033[32m'+msge+'\033[0m'
def cl_orange(msge): return '\033[33m'+msge+'\033[0m'
def cl_blue(msge): return '\033[34m'+msge+'\033[0m'
def cl_purple(msge): return '\033[35m'+msge+'\033[0m'
def cl_cyan(msge): return '\033[36m'+msge+'\033[0m'
def cl_lightgrey(msge): return '\033[37m'+msge+'\033[0m'
def cl_darkgrey(msge): return '\033[90m'+msge+'\033[0m'
def cl_lightred(msge): return '\033[91m'+msge+'\033[0m'
def cl_lightgreen(msge): return '\033[92m'+msge+'\033[0m'
def cl_yellow(msge): return '\033[93m'+msge+'\033[0m'
def cl_lightblue(msge): return '\033[94m'+msge+'\033[0m'
def cl_pink(msge): return '\033[95m'+msge+'\033[0m'
def cl_lightcyan(msge): return '\033[96m'+msge+'\033[0m'
#######################################################################################################################
# 类别：TCP通信类型 #####################
class iiwa_socket:
# M: __init__ ===========================
def __init__(self, ip, port):
self.BUFFER_SIZE = 1024
self.isconnected = False
self.JointPosition = ([None,None,None,None,None,None,None],None)
self.ToolPosition = ([None,None,None,None,None,None],None)
self.ToolForce = ([None,None,None],None)
self.ToolTorque = ([None,None,None],None)
self.isCompliance = (False, None)
self.isCollision = (False, None)
self.isReadyToMove = (False, None)
self.isMastered = (False, None)
self.OperationMode = (None, None)
self.OprationMode = (None, None)
self.JointAcceleration = (None,None)
self.JointVelocity = (None,None)
self.JointJerk = (None,None)
self.isFinished = (False, None)
self.hasError = (False, None)
self.isready = False
try:
# 启动连接线程
thread.start_new_thread( self.socket, (ip, port, ) )
except:
print cl_red('Error: ') + "Unable to start connection thread"
# ~M: __init__ ==========================
# M: Stop connection ====================
def close(self):
self.isconnected = False
# ~M: Stop connection ===================
# M: Connection socket ==================
def socket(self, ip, port):
import socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# 将socket绑定到端口
server_address = (ip, port)
os.system('clear')
print cl_pink('\n==========================================')
print cl_pink('< < < << SHEFFIELD ROBOTICS >> > > >')
print cl_pink('==========================================')
print cl_pink(' KUKA API for ROS')
print cl_pink(' Server Version: ' + version)
print cl_pink('==========================================\n')#设置对应的颜色打印
print cl_cyan('Starting up on:'), 'IP:', ip, 'Port:', port
try:
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock.bind(server_address)
except:
print cl_red('Error: ') + "Connection for KUKA cannot assign requested address:", ip, port
os._exit(-1)
# 监听传入的连接
sock.listen(1)
# 等待连接
print cl_cyan('Waiting for a connection...')
self.connection, client_address = sock.accept()
self.connection.settimeout(0.01)
print cl_cyan('Connection from'), client_address
self.isconnected = True
last_read_time = time.time()
while self.isconnected:
try:
data = self.connection.recv(self.BUFFER_SIZE)
last_read_time = time.time() # 保持接收时间
# 处理收到的数据包
for pack in data.split(">"): # 解析数据包
cmd_splt = pack.split()
if len(pack) and cmd_splt[0]=='Joint_Pos': # 如果是JointPosition
tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
if len(tmp)==7: self.JointPosition = (tmp, last_read_time)
if len(pack) and cmd_splt[0]=='Tool_Pos': # 如果是ToolPosition
tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
if len(tmp)==6: self.ToolPosition = (tmp, last_read_time)
if len(pack) and cmd_splt[0]=='Tool_Force': # 如果是ToolForce
tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
if len(tmp)==3: self.ToolForce = (tmp, last_read_time)
if len(pack) and cmd_splt[0]=='Tool_Torque': # 如果是ToolTorque
tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
if len(tmp)==3: self.ToolTorque = (tmp, last_read_time)
if len(pack) and cmd_splt[0]=='isCompliance': # 如果是Compliance
if cmd_splt[1] == "false": self.isCompliance = (False, last_read_time)
elif cmd_splt[1] == "true": self.isCompliance = (True, last_read_time)
if len(pack) and cmd_splt[0]=='isCollision': # 如果是Collision
if cmd_splt[1] == "false": self.isCollision = (False, last_read_time)
elif cmd_splt[1] == "true": self.isCollision = (True, last_read_time)
if len(pack) and cmd_splt[0]=='isReadyToMove': # 如果是ReadyToMove
if cmd_splt[1] == "false": self.isReadyToMove = (False, last_read_time)
elif cmd_splt[1] == "true": self.isReadyToMove = (True, last_read_time)
if len(pack) and cmd_splt[0]=='isMastered': # 如果是Mastered
if cmd_splt[1] == "false": self.isMastered = (False, last_read_time)
elif cmd_splt[1] == "true": self.isMastered = (True, last_read_time)
if len(pack) and cmd_splt[0] == 'OperationMode': # 如果是OperationMode
self.OperationMode = (cmd_splt[1], last_read_time)
if len(pack) and cmd_splt[0]=='JointAcceleration': # 如果是JointAcceleration
self.JointAcceleration = (float(cmd_splt[1]), last_read_time)
if len(pack) and cmd_splt[0]=='JointVelocity': # 如果是JointVelocity
self.JointVelocity = (float(cmd_splt[1]), last_read_time)
if len(pack) and cmd_splt[0]=='JointJerk': # 如果是JointJerk
self.JointJerk = (float(cmd_splt[1]), last_read_time)
if len(pack) and cmd_splt[0]=='isFinished': # 如果是Finished
if cmd_splt[1] == "false": self.isFinished = (False, last_read_time)
elif cmd_splt[1] == "true": self.isFinished = (True, last_read_time)
if len(pack) and cmd_splt[0]=='hasError': # 如果存在Error
if cmd_splt[1] == "false": self.hasError = (False, last_read_time)
elif cmd_splt[1] == "true": self.hasError = (True, last_read_time)
if ( all(item != None for item in self.JointPosition[0]) and
all(item != None for item in self.ToolPosition[0]) and
all(item != None for item in self.ToolForce[0]) and
all(item != None for item in self.ToolTorque[0]) and
self.isCompliance[1] != None and
self.isCollision[1] != None and
self.isReadyToMove[1] != None and
self.isMastered[1] != None and
self.OperationMode[1] != None ):
#self.OprationMode = (None, None)
self.isready = True
else:
self.isready = False
except:
elapsed_time = time.time() - last_read_time
if elapsed_time > 5.0: # 5秒内未收到包裹
self.close() #与iiwa断开
self.isconnected = False
print cl_lightred('No packet received from iiwa for 5s!')
self.connection.shutdown(socket.SHUT_RDWR)
self.connection.close()
sock.close()
self.isconnected = False
print cl_lightred('Connection is closed!')
def send(self, cmd):
thread.start_new_thread( self.__send, (cmd, ) )
def __send(self, cmd):
self.connection.sendall(cmd+'\r\n')
#######################################################################################################################
# 类：ROS类型节点 #####################
class kuka_iiwa_ros_node:
# M: __init__ ===========================
def __init__(self, ip, port): # Makes kuka_iiwa ROS node
self.iiwa_soc = iiwa_socket(ip, port)
# 等到iiwa连接到
while (not self.iiwa_soc.isready): pass
# 侦听kuka_iiwa命令
rospy.Subscriber("kuka_command", String, self.callback)
# 使发布者获得所有kuka_iiwa数据
pub_JointPosition = rospy.Publisher('JointPosition', String, queue_size=10)
pub_ToolPosition = rospy.Publisher('ToolPosition', String, queue_size=10)
pub_ToolForce = rospy.Publisher('ToolForce', String, queue_size=10)
pub_ToolTorque = rospy.Publisher('ToolTorque', String, queue_size=10)
pub_isCompliance = rospy.Publisher('isCompliance', String, queue_size=10)
pub_isCollision = rospy.Publisher('isCollision', String, queue_size=10)
pub_isReadyToMove = rospy.Publisher('isReadyToMove', String, queue_size=10)
pub_isMastered = rospy.Publisher('isMastered', String, queue_size=10)
pub_OperationMode = rospy.Publisher('OperationMode', String, queue_size=10)
pub_JointAcceleration = rospy.Publisher('JointAcceleration', String, queue_size=10)
pub_JointVelocity = rospy.Publisher('JointVelocity', String, queue_size=10)
pub_JointJerk = rospy.Publisher('JointJerk', String, queue_size=10)
pub_isFinished = rospy.Publisher('isFinished', String, queue_size=10)
pub_hasError = rospy.Publisher('hasError', String, queue_size=10)
# 制作node_iiwa节点
rospy.init_node('kuka_iiwa', anonymous=False)
rate = rospy.Rate(100) # 100hz更新速率。
while not rospy.is_shutdown() and self.iiwa_soc.isconnected:
#data_str = self.iiwa_soc.data + " %s" % rospy.get_time()
# 更新所有的kuka_iiwa数据
for [pub, data] in [ [pub_JointPosition, self.iiwa_soc.JointPosition],
[pub_ToolPosition, self.iiwa_soc.ToolPosition],
[pub_ToolForce, self.iiwa_soc.ToolForce],
[pub_ToolTorque, self.iiwa_soc.ToolTorque],
[pub_isCompliance, self.iiwa_soc.isCompliance],
[pub_isCollision, self.iiwa_soc.isCollision],
[pub_isReadyToMove, self.iiwa_soc.isReadyToMove],
[pub_isMastered, self.iiwa_soc.isMastered],
[pub_OperationMode, self.iiwa_soc.OperationMode],
[pub_JointAcceleration, self.iiwa_soc.JointAcceleration],
[pub_JointVelocity, self.iiwa_soc.JointVelocity],
[pub_JointJerk, self.iiwa_soc.JointJerk],
[pub_isFinished, self.iiwa_soc.isFinished],
[pub_hasError, self.iiwa_soc.hasError]]:
data_str = str(data[0]) +' '+ str(rospy.get_time())
##########rospy.loginfo(data_str)
pub.publish(data_str)
rate.sleep()
# ~M: __init__ ==========================
# M: callback ===========================
# 接收命令字符串并将其发送到KUKA iiwa
def callback(self, data):
###########rospy.loginfo(rospy.get_caller_id() + "Received command " + str(data.data) )
self.iiwa_soc.send(data.data) # e.g 'setPosition 45 60 0 -25 0 95 0' for going to start position
def read_conf():
f_conf = os.path.abspath(os.path.dirname(__file__)) + '/conf.txt'
if os.path.isfile(f_conf):
IP = ''
Port = ''
for line in open(f_conf, 'r'):
l_splt = line.split()
if len(l_splt)==4 and l_splt[0] == 'server':
IP = l_splt[1]
Port = int(l_splt[3])
if len(IP.split('.'))!=4 or Port<=0:
print cl_red('Error:'), "conf.txt doesn't include correct IP/Port! e.g. server 172.31.1.50 port 1234"
exit()
else:
print cl_red('Error:'), "conf.txt doesn't exist!"
exit()
return [IP, Port]
# ~M: Reading config file for Server IP and Port ================
if __name__ == '__main__':
[IP, Port] = read_conf()
try:
node = kuka_iiwa_ros_node(IP, Port) # Make a Kuka_iiwa ROS node
except rospy.ROSInterruptException:
pass
在KUKA-iiwa的控制台中运行: “ROS”
现在，API已启动并正在运行。最后一步是运行ROS节点以与API通信并控制机器人。Python提供了一些基本示例。例如，要开始，您可以运行：
rosrun kuka_controller demo_ReadData.pydemo_ReadData.py
#!/usr/bin/env python
# KUKA API for ROS
version = 'V15032017'
from client_lib import *
import time, os
# Making a connection object.
my_client = kuka_iiwa_ros_client()
while (not my_client.isready):
pass
# Initializing Tool 1
my_client.send_command('setTool tool1')
for i in range(1000):
os.system('clear')
print 'OperationMode\t=', my_client.OperationMode
print 'isCollision\t=', my_client.isCollision
print 'isCompliance\t=', my_client.isCompliance
print 'isMastered\t=', my_client.isMastered
print 'isready\t=', my_client.isready
print 'isReadyToMove\t=', my_client.isReadyToMove
print 'ToolPosition\t=', my_client.ToolPosition
print 'ToolForce\t=', my_client.ToolForce
print 'ToolTorque\t=', my_client.ToolTorque
print 'JointAcceleration\t=', my_client.JointAcceleration
print 'JointJerk\t=', my_client.JointJerk
print 'JointPosition\t=', my_client.JointPosition
print 'JointVelocity\t=', my_client.JointVelocity
time.sleep(1)在此演示中，机器人不会移动。该演示程序接收机器人数据（如关节位置等）并打印出来。
评论（3）
您还未登录，请登录后发表或查看评论