【ABB】上位機實時控制機器人運動之EGM
發布時間：2020年05月19日
Externally Guided Motion (689-1 EGM)為ABB機器人方便客戶外部直接引導/獲取機器人位置等信息而設置的高級接口。
EGM提供了3種不同的特性：
EGM Position Stream：
RAPID任務中機械單元的當前和計划位置發送至外部系統。
速度能達到250Hz（最快4ms一個點）
EGM Position Guidance:
相關機器人不會沿RAPID中的編程路徑移動，而是沿某件外部裝置所生成的路徑移動。（即可以變相實現外部路徑直接規划）
機器人可以接受xyzabc笛卡爾坐標，也可以接受a1-a6關節坐標
最快能達到4ms
EGM Path Correction:
用某件外部裝置提供的測量值來修改／校正所編寫的機器人路徑。
速度為48ms的倍數
EGM 相關選項介紹見 ABB機器人EGM外部引導選項介紹
EGM與PC之間可以通過UDP建立高速通訊，具體實現方式參見 ABB機器人炸裂黑科技
ABB機器人炸裂黑科技 一文介紹了使用指令EGMStreamStart，機器人高速向PC發送位置的方法，若要通過PC進行實時控制，即使用EGM Position Guidance實現閉環控制（PC實時接受機器人位置反饋值並發送新位置給機器人，速度最快可達4ms），如何實現？
可以通過EGMActPose指令開啟設置，具體解釋如下：
EGMActPose egmID1\Tool:=MyTool,corr_frame_offs,EGM_FRAME_WORLD,corr_frame_offs,EGM_FRAME_WORLD\x:=egm_minmax_lin1\y:=egm_minmax_lin1\z:=egm_minmax_lin1\rx:=egm_minmax_rot1\ry:=egm_minmax_rot1\rz:=egm_minmax_rot1\LpFilter:=20\MaxSpeedDeviation:=60;
! 修正數據基於World坐標系，
! 最大關節速度為60°/s
! 設定xyz及姿態的停止收斂域
通過EGMRunPose來開啟機器人運動，使用該語句時，機器人會先輸出當前機器人位置並接受PC返回的UDP數據，即使用該指令后，無需再使用EGMStreamStart指令進行數據發送。
EGMRunPose egmID1,EGM_STOP_HOLD\x\y\z\CondTime:=1\RampInTime:=0.05;
! 僅接受xyz數據
! 若要接受姿態數據，也可加入\rx\ry\rz
! 達到位置收斂域后還需要穩定1s
C#端，UDP接收到機器人數據后，若要實現視頻中效果，則可以將MouseDown位置記錄，並將之后Mouse移動的位置與MouseDown的位置做減法並實時賦值給機器人做EGM運動，
void CreateSensorMessage(EgmSensor.Builder sensor)
{
// create a header
EgmHeader.Builder hdr = new EgmHeader.Builder();
hdr.SetSeqno(_seqNumber++)
.SetTm((uint)DateTime.Now.Ticks)
.SetMtype(EgmHeader.Types.MessageType.MSGTYPE_CORRECTION);
sensor.SetHeader(hdr);
// create some sensor data
EgmPlanned.Builder planned = new EgmPlanned.Builder();
EgmPose.Builder pos = new EgmPose.Builder();
EgmQuaternion.Builder pq = new EgmQuaternion.Builder();
EgmCartesian.Builder pc = new EgmCartesian.Builder();
//364.6839,2.01102E-28,368.9905
if (Form1.bCatch == true)
{
// 若窗體MouseDown后，則發送當前Mouse與MouseDown位置的差值給機器人
pc.SetX(robot_x + 0.5*(Form1.NewPoint.Y-Form1.DownPoint.Y))
.SetY(robot_y + 0.5*(Form1.NewPoint.X-Form1.DownPoint.X))
.SetZ(368);
}
else
{
// 若沒有MouseDown，則直接返回機器人的位置
pc.SetX(robot_x1)
.SetY(robot_y1)
.SetZ(368);
}
pq.SetU0(1.0)
.SetU1(0.0)
.SetU2(0.0)
.SetU3(0.0);
// 發送姿態數據
pos.SetPos(pc)
.SetOrient(pq);
planned.SetCartesian(pos); // bind pos object to planned
sensor.SetPlanned(planned); // bind planned to sensor object
return;
}
以下為主程序，其中CreateSensorMessage為上位實現方法。
public void SensorThread()
{
// create an udp client and listen on any address and the port _ipPortNumber
_udpServer = new UdpClient(_ipPortNumber);
var remoteEP = new IPEndPoint(IPAddress.Any, _ipPortNumber);
while (_exitThread == false)
{
// 從機器人獲取數據
var data = _udpServer.Receive(ref remoteEP);
if (data != null)
{
// de-serialize inbound message from robot
EgmRobot robot = EgmRobot.CreateBuilder().MergeFrom(data).Build();
// display inbound message
DisplayInboundMessage(robot);
// create a new outbound sensor message
EgmSensor.Builder sensor = EgmSensor.CreateBuilder();
CreateSensorMessage(sensor);
using (MemoryStream memoryStream = new MemoryStream())
{
EgmSensor sensorMessage = sensor.Build();
sensorMessage.WriteTo(memoryStream);
Console.WriteLine(sensorMessage.ToString());
//send the udp message to the robot
int bytesSent = _udpServer.Send(memoryStream.ToArray(),
(int)memoryStream.Length, remoteEP);
if (bytesSent < 0)
{
Console.WriteLine("Error send to robot");
}
}
}
}
}
PC作為服務端先啟動，然后啟動機器人即可。
