Stream Motion
Stream Motion is Fanuc's real-time streaming motion control interface, available as the J519 paid option on compatible controllers. It enables external applications to send continuous motion commands to the robot at high frequency (125Hz or 250Hz), making it ideal for applications requiring smooth, coordinated motion.
Stream Motion is Fanuc's real-time streaming motion control interface, available as the J519 paid option on compatible controllers. It enables external applications to send continuous motion commands to the robot at high frequency (125Hz or 250Hz), making it ideal for applications requiring smooth, coordinated motion such as:
- Force control and adaptive machining
- Sensor-guided motion
- Real-time trajectory generation
- Human-robot collaboration
- Custom motion planning algorithms
Requirements
Controller Requirements
- Fanuc controller with J519 Stream Motion option installed
- Compatible robot model (not all models support Stream Motion - verify with Fanuc)
- Network connectivity (UDP port 60015 by default)
Key Limitations
| Feature | Limitation |
|---|---|
| Motion groups | Group 1 (robot) + up to 3 auxiliary axes |
| Coordinate mode | Joint OR Cartesian (cannot switch mid-session) |
| Update rate | 125Hz or 250Hz (controller dependent) |
| Control type | Position only (no velocity/torque control) |
Quick Start
using UnderAutomation.Fanuc;using UnderAutomation.Fanuc.StreamMotion;using UnderAutomation.Fanuc.StreamMotion.Data;// Create and connectvar robot = new FanucRobot();robot.StreamMotion.Connect("192.168.1.1");// Handle state packetsrobot.StreamMotion.StateReceived += (sender, e) =>{var state = e.State;Console.WriteLine($"Position: J1={state.JointPosition.J1:F2}°");// Check if robot is ready for next commandif (state.Status.ReadyForCommands &&!state.Status.InMotion &&!state.Status.CommandReceived){// Send your interpolated position herevar command = new CommandPacket{DataStyle = DataStyle.Joint,MotionData = new MotionData{J1 = nextJ1, J2 = nextJ2, J3 = nextJ3,J4 = nextJ4, J5 = nextJ5, J6 = nextJ6}};robot.StreamMotion.SendCommand(command);}};// Start streamingrobot.StreamMotion.Start();// ... your application logic ...// Stop and disconnectrobot.StreamMotion.Stop();
Architecture Overview
Stream Motion uses a UDP-based protocol with the following communication pattern:
┌─────────────────┐ ┌─────────────────┐
│ Your App │ │ Fanuc Robot │
│ │ │ │
│ ┌───────────┐ │ Start Packet │ ┌───────────┐ │
│ │ Stream │──┼────────────────────> | │ Stream │ │
│ │ Motion │ │ │ │ Motion │ │
│ │ Client │<─┼──────────────────────┼──│ Server │ │
│ └───────────┘ │ State Packets │ └───────────┘ │
│ │ │ (~500Hz) │ ▲ │
│ │ │ │ │ │
│ └─────────┼──────────────────────┼───────┘ │
│ │ Command Packets │ │
│ │ (125/250Hz) │ │
└─────────────────┘ └─────────────────┘
Communication Flow
- Connect - Establish UDP socket connection
- Start - Send start packet to begin streaming session
- Receive States - Robot sends state packets at ~500Hz
- Send Commands - Your app sends interpolated positions at 125/250Hz
- Stop - Send stop packet to end session
- Disconnect - Close socket
Robot Program Setup
A TP (Teach Pendant) program must be running on the robot to handle Stream Motion commands.
Required TP Program
Create a program named for instance START_STREAM_MOTION_J519.ls with this content:
/PROG START_STREAM_MOTION_J519/ATTROWNER = MNEDITOR;PROTECT = READ_WRITE;TCD: STACK_SIZE = 0,TASK_PRIORITY = 50,TIME_SLICE = 0,BUSY_LAMP_OFF = 0,ABORT_REQUEST = 0,PAUSE_REQUEST = 0;DEFAULT_GROUP = 1,*,*,*,*;CONTROL_CODE = 00000000 00000000;/MN1: LBL[1] ;2: IBGN start[1] ;3: IBGN end[1] ;4: JMP LBL[1] ;/POS/END
Program Explanation
| Line | Instruction | Description |
|---|---|---|
| 1 | LBL[1] | Loop label |
| 2 | IBGN start[1] | Start Stream Motion session |
| 3 | IBGN end[1] | End Stream Motion session |
| 4 | JMP LBL[1] | Loop back to handle next session |
Running the Program
- Upload the program to the robot via FTP or teach pendant
- Start the program in AUTO mode
- The program loops, waiting for Stream Motion connections
- Your application connects and controls motion via UDP
- When streaming stops, the program loops back to wait for the next connection
Adding Custom Logic
You can add instructions outside the Stream Motion block:
/MN1: LBL[1] ;2: DO[1]=ON ; // Turn on output before streaming3: IBGN start[1] ;4: IBGN end[1] ;5: DO[1]=OFF ; // Turn off output after streaming6: WAIT 1.00(sec) ; // Wait before next session7: JMP LBL[1] ;/POS/END
Connection and Streaming
Connecting to the Robot
var robot = new FanucRobot();// Connect with default settings (port 60015)robot.StreamMotion.Connect("192.168.1.1");// Or with custom settingsrobot.StreamMotion.Connect(ip: "192.168.1.1",port: 60015,sendTimeoutMs: 1000,receiveTimeoutMs: 1000);// Check connection statusif (robot.StreamMotion.IsConnected){Console.WriteLine("Connected to Stream Motion");}
Standalone Client
You can also use the StreamMotionClient class directly without FanucRobot:
var client = new StreamMotionClient();client.Connect("192.168.1.1");client.StateReceived += OnStateReceived;client.Start();
Starting and Stopping Streaming
// Start streaming - begins receiving state packetsrobot.StreamMotion.Start();// Check streaming statusif (robot.StreamMotion.IsStreaming){Console.WriteLine($"Streaming active. Packets received: {robot.StreamMotion.PacketCount}");Console.WriteLine($"Robot frequency: {robot.StreamMotion.RobotFrequency:F1} Hz");Console.WriteLine($"Measured frequency: {robot.StreamMotion.MeasuredFrequency:F1} Hz");}// Stop streamingrobot.StreamMotion.Stop();// Disconnectrobot.StreamMotion.Disconnect();
State Packets
The robot sends state packets continuously at approximately 500Hz. Each packet contains the robot's current state.
StatePacket Properties
| Property | Type | Description |
|---|---|---|
SequenceNumber | uint | Packet sequence for synchronization |
TimeStamp | uint | Robot timestamp (ms, 2ms resolution) |
Status | RobotStatus | Status flags |
JointPosition | MotionData | Current joint positions (J1-J9) |
CartesianPosition | MotionData | Current Cartesian position (X,Y,Z,W,P,R + E1-E3) |
MotorCurrents | MotionData | Motor currents in Amperes |
IORead | IOReadResult | Result of I/O read operation |
RobotStatus Flags
| Flag | Description |
|---|---|
ReadyForCommands | Robot is ready to receive command packets |
CommandReceived | Robot has received and is processing a command |
SystemReady | System ready (SYSRDY) |
InMotion | Robot is currently executing motion |
Handling State Packets
robot.StreamMotion.StateReceived += (sender, e) =>{var state = e.State;// Read current joint positionsConsole.WriteLine($"J1: {state.JointPosition.J1:F3}°");Console.WriteLine($"J2: {state.JointPosition.J2:F3}°");Console.WriteLine($"J3: {state.JointPosition.J3:F3}°");Console.WriteLine($"J4: {state.JointPosition.J4:F3}°");Console.WriteLine($"J5: {state.JointPosition.J5:F3}°");Console.WriteLine($"J6: {state.JointPosition.J6:F3}°");// Read Cartesian positionConsole.WriteLine($"X: {state.CartesianPosition.X:F2} mm");Console.WriteLine($"Y: {state.CartesianPosition.Y:F2} mm");Console.WriteLine($"Z: {state.CartesianPosition.Z:F2} mm");// Check statusConsole.WriteLine($"Ready: {state.Status.ReadyForCommands}");Console.WriteLine($"In Motion: {state.Status.InMotion}");// Read motor currentsConsole.WriteLine($"Motor 1 current: {state.MotorCurrents.Axis1:F2} A");};
Command Packets
Command packets are sent to instruct the robot to move to a new position.
CommandPacket Properties
| Property | Type | Description |
|---|---|---|
DataStyle | DataStyle | Joint or Cartesian |
MotionData | MotionData | Target position |
IsLastData | bool | Marks end of motion sequence |
ReadIOType | IOType | Type of I/O to read |
ReadIOIndex | ushort | I/O index to read |
ReadIOMask | ushort | I/O read mask |
WriteIOType | IOType | Type of I/O to write |
WriteIOIndex | ushort | I/O index to write |
WriteIOMask | ushort | I/O write mask |
WriteIOValue | ushort | Value to write |
MotionData Structure
MotionData holds 9 axis values that can be accessed by axis number or name:
var motion = new MotionData();// Joint-style accessmotion.J1 = 10.0f; // Joint 1motion.J2 = 20.0f; // Joint 2motion.J3 = 30.0f; // Joint 3motion.J4 = 0.0f; // Joint 4motion.J5 = -45.0f; // Joint 5motion.J6 = 90.0f; // Joint 6motion.J7 = 0.0f; // Extended axis 1motion.J8 = 0.0f; // Extended axis 2motion.J9 = 0.0f; // Extended axis 3// Cartesian-style accessmotion.X = 500.0f; // X position (mm)motion.Y = 100.0f; // Y position (mm)motion.Z = 300.0f; // Z position (mm)motion.W = 180.0f; // W rotation (deg)motion.P = 0.0f; // P rotation (deg)motion.R = 0.0f; // R rotation (deg)motion.E1 = 0.0f; // Extended axis 1motion.E2 = 0.0f; // Extended axis 2motion.E3 = 0.0f; // Extended axis 3// Generic axis accessmotion.Axis1 = 10.0f;motion.Values[0] = 10.0f; // Raw array access
Sending Commands
// Method 1: Full command packetvar command = new CommandPacket{DataStyle = DataStyle.Joint,MotionData = new MotionData { J1 = 10, J2 = 20, J3 = 30, J4 = 0, J5 = -45, J6 = 90 },IsLastData = false};robot.StreamMotion.SendCommand(command);// Method 2: Convenience method for joint motionrobot.StreamMotion.SendJointCommand(new MotionData { J1 = 10, J2 = 20, J3 = 30, J4 = 0, J5 = -45, J6 = 90 },isLastData: false);// Method 3: Convenience method for Cartesian motionrobot.StreamMotion.SendCartesianCommand(new MotionData { X = 500, Y = 100, Z = 300, W = 180, P = 0, R = 0 },isLastData: false);
Protocol State Machine
The Stream Motion protocol follows a strict state machine. Understanding this is critical for proper operation.
State Transitions
┌─────────────────────────────────────┐
│ │
▼ │
┌───────────────────────────┐ │
│ READY FOR COMMANDS │ │
│ ReadyForCommands = true │ │
│ CommandReceived = false │ │
│ InMotion = false │ │
└───────────────────────────┘ │
│ │
│ Send Command │
▼ │
┌───────────────────────────┐ │
│ COMMAND RECEIVED │ │
│ CommandReceived = true │ │
└───────────────────────────┘ │
│ │
│ Robot starts motion │
▼ │
┌───────────────────────────┐ │
│ IN MOTION │ │
│ InMotion = true │────────────────────────>┤
│ (may receive multiple │ Motion complete │
│ packets in this state) │ │
└───────────────────────────┘ │
Correct Command Timing
Only send the next command when ALL these conditions are met:
robot.StreamMotion.StateReceived += (sender, e) =>{var status = e.State.Status;// Check all three conditionsbool canSendCommand = status.ReadyForCommands &&!status.CommandReceived &&!status.InMotion;if (canSendCommand){// Safe to send next commandrobot.StreamMotion.SendCommand(nextCommand);}};
⚠️ Common Mistakes
| Mistake | Consequence |
|---|---|
| Sending commands too fast | Commands dropped, erratic motion |
Ignoring CommandReceived | Duplicate commands sent |
Ignoring InMotion | Buffer overflow, jerk errors |
Not checking ReadyForCommands | Commands rejected |
Motion Interpolation
Why Interpolation is Required
Stream Motion operates at 125Hz or 250Hz. Each command should represent a small incremental move. The robot's motion planner expects smooth, continuous position updates that respect:
- Velocity limits - Maximum speed for each axis
- Acceleration limits - Maximum rate of velocity change
- Jerk limits - Maximum rate of acceleration change (smoothness)
Simple Linear Interpolation (Educational Example)
public class SimpleInterpolator{private float[] _startPosition = new float[6];private float[] _targetPosition = new float[6];private float[] _currentPosition = new float[6];private double _duration;private DateTime _startTime;private bool _isMoving;public void StartMove(float[] start, float[] target, double durationSeconds){Array.Copy(start, _startPosition, 6);Array.Copy(target, _targetPosition, 6);_duration = durationSeconds;_startTime = DateTime.UtcNow;_isMoving = true;}public float[] GetNextPosition(){if (!_isMoving) return _currentPosition;double elapsed = (DateTime.UtcNow - _startTime).TotalSeconds;double t = Math.Min(1.0, elapsed / _duration);// Linear interpolation (NOT smooth - educational only!)for (int i = 0; i < 6; i++){_currentPosition[i] = _startPosition[i] +(float)(t * (_targetPosition[i] - _startPosition[i]));}if (t >= 1.0) _isMoving = false;return _currentPosition;}}
S-Curve Interpolation (Recommended)
For smooth, jerk-limited motion, use an S-curve or quintic polynomial profile:
public class SCurveInterpolator{private float[] _startPosition = new float[6];private float[] _targetPosition = new float[6];private float[] _distance = new float[6];private float[] _direction = new float[6];private double _duration;private DateTime _startTime;private bool _isMoving;public void StartMove(float[] start, float[] target, double maxSpeed, double maxAccel){Array.Copy(start, _startPosition, 6);Array.Copy(target, _targetPosition, 6);// Calculate per-axis parametersdouble maxDuration = 0;for (int i = 0; i < 6; i++){float delta = target[i] - start[i];_distance[i] = Math.Abs(delta);_direction[i] = delta >= 0 ? 1f : -1f;// Calculate duration for this axisif (_distance[i] > 0 && maxSpeed > 0){double axisDuration = CalculateTrapezoidalDuration(_distance[i], maxSpeed, maxAccel);if (axisDuration > maxDuration) maxDuration = axisDuration;}}_duration = maxDuration;_startTime = DateTime.UtcNow;_isMoving = maxDuration > 0;}public float[] GetNextPosition(){if (!_isMoving) return _targetPosition;double elapsed = (DateTime.UtcNow - _startTime).TotalSeconds;double t = Math.Min(1.0, elapsed / _duration);float[] position = new float[6];for (int i = 0; i < 6; i++){// S-curve: s(t) = 6t⁵ - 15t⁴ + 10t³double s = t * t * t * (t * (t * 6 - 15) + 10);double traveled = _distance[i] * s;position[i] = _startPosition[i] + (_direction[i] * (float)traveled);}if (t >= 1.0) _isMoving = false;return position;}public bool IsMoving => _isMoving;private double CalculateTrapezoidalDuration(double distance, double vMax, double aMax){double tAccel = vMax / aMax;double dAccel = 0.5 * aMax * tAccel * tAccel;if (2 * dAccel >= distance){// Triangle profilereturn 2 * Math.Sqrt(distance / aMax);}else{// Trapezoidal profiledouble tCruise = (distance - 2 * dAccel) / vMax;return 2 * tAccel + tCruise;}}}
Integration with Stream Motion
private SCurveInterpolator _interpolator = new SCurveInterpolator();private bool _waitingForCommandAck = false;robot.StreamMotion.StateReceived += (sender, e) =>{var status = e.State.Status;// State machine for proper protocol handlingif (_waitingForCommandAck){if (status.CommandReceived){// Command acknowledged, now wait for motion complete}if (!status.InMotion && !status.CommandReceived){// Motion complete, ready for next command_waitingForCommandAck = false;}return; // Don't send while waiting}// Check if ready to sendif (!status.ReadyForCommands || status.InMotion || status.CommandReceived)return;// Get next interpolated positionif (_interpolator.IsMoving){float[] nextPos = _interpolator.GetNextPosition();var command = new CommandPacket{DataStyle = DataStyle.Joint,MotionData = new MotionData{J1 = nextPos[0], J2 = nextPos[1], J3 = nextPos[2],J4 = nextPos[3], J5 = nextPos[4], J6 = nextPos[5]},IsLastData = !_interpolator.IsMoving};robot.StreamMotion.SendCommand(command);_waitingForCommandAck = true;}};
Querying Axis Limits
Before commanding motion, you can query the robot's axis limits. This must be done before starting streaming.
// Query velocity limits for axis 1var velocityAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Velocity);Console.WriteLine($"Axis 1 max velocity: {velocityAck.ThresholdsNoLoad[19]:F2} deg/s");// Query acceleration limitsvar accelAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Acceleration);Console.WriteLine($"Axis 1 max acceleration: {accelAck.ThresholdsNoLoad[19]:F2} deg/s²");// Query jerk limitsvar jerkAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Jerk);Console.WriteLine($"Axis 1 max jerk: {jerkAck.ThresholdsNoLoad[19]:F2} deg/s³");// Query all axesfor (uint axis = 1; axis <= 6; axis++){var ack = robot.StreamMotion.RequestThreshold(axis, ThresholdType.Velocity);Console.WriteLine($"J{axis} max velocity: {ack.ThresholdsNoLoad[19]:F2} deg/s");}
AckPacket Threshold Arrays
The threshold arrays contain 20 values at different velocity percentages:
| Index | Velocity % |
|---|---|
| 0 | 5% |
| 1 | 10% |
| ... | ... |
| 19 | 100% |
Use ThresholdsNoLoad for unloaded robot and ThresholdsMaxLoad for maximum payload.
I/O Operations
Stream Motion supports reading and writing digital I/O during motion.
Reading I/O
var command = new CommandPacket{DataStyle = DataStyle.Joint,MotionData = motionData,ReadIOType = IOType.DigitalInput,ReadIOIndex = 1, // DI[1]ReadIOMask = 0xFFFF // Read all bits};robot.StreamMotion.SendCommand(command);// Result available in next state packetrobot.StreamMotion.StateReceived += (s, e) =>{var ioResult = e.State.IORead;Console.WriteLine($"I/O Value: {ioResult.Value}");};
Writing I/O
var command = new CommandPacket{DataStyle = DataStyle.Joint,MotionData = motionData,WriteIOType = IOType.DigitalOutput,WriteIOIndex = 1, // DO[1]WriteIOMask = 0x0001, // Bit 0WriteIOValue = 0x0001 // Set bit 0 ON};robot.StreamMotion.SendCommand(command);
Best Practices
1. Always Implement Proper Interpolation
// ❌ WRONG - Will cause jerk errorsrobot.StreamMotion.SendJointCommand(new MotionData { J1 = 90 }); // Jump to 90°// ✅ CORRECT - Smooth interpolated motionfor (double t = 0; t <= 1.0; t += 0.008) // 125Hz{double s = t * t * t * (t * (t * 6 - 15) + 10); // S-curvedouble j1 = startJ1 + (targetJ1 - startJ1) * s;// ... wait for proper state, then send}
2. Respect the Protocol State Machine
// ❌ WRONG - Ignoring staterobot.StreamMotion.StateReceived += (s, e) =>{robot.StreamMotion.SendCommand(command); // May fail!};// ✅ CORRECT - Check state firstrobot.StreamMotion.StateReceived += (s, e) =>{if (e.State.Status.ReadyForCommands &&!e.State.Status.CommandReceived &&!e.State.Status.InMotion){robot.StreamMotion.SendCommand(command);}};
3. Handle Errors Gracefully
robot.StreamMotion.ReceiveError += (s, e) =>{Console.WriteLine($"Error: {e.Exception.Message}");if (!e.IsStreaming){// Connection lost, attempt reconnectionTryReconnect();}};
4. Query Limits Before Motion
// Get axis limits before starting motionvar limits = new double[6];for (uint i = 1; i <= 6; i++){var ack = robot.StreamMotion.RequestThreshold(i, ThresholdType.Velocity);limits[i-1] = ack.ThresholdsNoLoad[19];}// Now start streamingrobot.StreamMotion.Start();
5. Clean Shutdown
try{// Your streaming logic}finally{if (robot.StreamMotion.IsStreaming){robot.StreamMotion.Stop();}}
Troubleshooting
Common Errors
| Error | Cause | Solution |
|---|---|---|
| "Joint jerk limit exceeded" | Motion too fast/abrupt | Implement proper S-curve interpolation |
| "Robot not ready for commands" | Wrong state | Wait for ReadyForCommands=true |
| Connection timeout | Network issue | Check IP, port, firewall |
| "Streaming already started" | Double start | Check IsStreaming before Start() |
| Commands not executed | Sent too fast | Follow protocol state machine |
Diagnostic Logging
robot.StreamMotion.StateReceived += (s, e) =>{var state = e.State;Console.WriteLine($"[{DateTime.Now:HH:mm:ss.fff}] " +$"Seq={state.SequenceNumber} " +$"Ready={state.Status.ReadyForCommands} " +$"CmdRcv={state.Status.CommandReceived} " +$"Motion={state.Status.InMotion} " +$"J1={state.JointPosition.J1:F3}");};
Checking Frequencies
Console.WriteLine($"Robot frequency: {robot.StreamMotion.RobotFrequency:F1} Hz");Console.WriteLine($"Measured frequency: {robot.StreamMotion.MeasuredFrequency:F1} Hz");Console.WriteLine($"Packets received: {robot.StreamMotion.PacketCount}");
Complete Example
Here's a complete example that moves the robot through a sequence of waypoints:
using System;using System.Threading;using UnderAutomation.Fanuc;using UnderAutomation.Fanuc.StreamMotion;using UnderAutomation.Fanuc.StreamMotion.Data;class StreamMotionExample{private FanucRobot _robot;private SCurveInterpolator _interpolator;private float[][] _waypoints;private int _currentWaypoint;private bool _waitingForAck;private readonly object _lock = new object();public void Run(){_robot = new FanucRobot();_interpolator = new SCurveInterpolator();// Define waypoints (J1-J6 in degrees)_waypoints = new float[][]{new float[] { 0, 0, 0, 0, 0, 0 },new float[] { 30, 10, -20, 0, 45, 0 },new float[] { -30, 20, -40, 90, 0, 45 },new float[] { 0, 0, 0, 0, 0, 0 }};_currentWaypoint = 0;try{// ConnectConsole.WriteLine("Connecting to robot...");_robot.StreamMotion.Connect("192.168.1.1");Console.WriteLine("Connected!");// Query limitsConsole.WriteLine("Querying axis limits...");for (uint axis = 1; axis <= 6; axis++){var ack = _robot.StreamMotion.RequestThreshold(axis, ThresholdType.Velocity);Console.WriteLine($" J{axis} max velocity: {ack.ThresholdsNoLoad[19]:F1} deg/s");}// Setup event handlers_robot.StreamMotion.StateReceived += OnStateReceived;_robot.StreamMotion.ReceiveError += OnReceiveError;// Start streamingConsole.WriteLine("Starting streaming...");_robot.StreamMotion.Start();// Wait for completionConsole.WriteLine("Moving through waypoints... Press Enter to stop.");Console.ReadLine();}finally{// Clean shutdownConsole.WriteLine("Stopping...");if (_robot.StreamMotion.IsStreaming){_robot.StreamMotion.Stop();}_robot.StreamMotion.Disconnect();Console.WriteLine("Done.");}}private void OnStateReceived(object sender, StateReceivedEventArgs e){lock (_lock){var status = e.State.Status;// Handle acknowledgment waitingif (_waitingForAck){if (!status.InMotion && !status.CommandReceived){_waitingForAck = false;}else{return; // Still waiting}}// Check if ready to sendif (!status.ReadyForCommands || status.InMotion || status.CommandReceived)return;// Start next waypoint if current move completeif (!_interpolator.IsMoving){if (_currentWaypoint >= _waypoints.Length - 1){_currentWaypoint = 0; // Loop back}float[] start = new float[]{e.State.JointPosition.J1,e.State.JointPosition.J2,e.State.JointPosition.J3,e.State.JointPosition.J4,e.State.JointPosition.J5,e.State.JointPosition.J6};_currentWaypoint++;float[] target = _waypoints[_currentWaypoint];Console.WriteLine($"Moving to waypoint {_currentWaypoint}");_interpolator.StartMove(start, target, maxSpeed: 50, maxAccel: 100);}// Get next interpolated positionif (_interpolator.IsMoving){float[] pos = _interpolator.GetNextPosition();var command = new CommandPacket{DataStyle = DataStyle.Joint,MotionData = new MotionData{J1 = pos[0], J2 = pos[1], J3 = pos[2],J4 = pos[3], J5 = pos[4], J6 = pos[5]},IsLastData = !_interpolator.IsMoving};_robot.StreamMotion.SendCommand(command);_waitingForAck = true;}}}private void OnReceiveError(object sender, ReceiveErrorEventArgs e){Console.WriteLine($"Error: {e.Exception.Message}");}static void Main(string[] args){new StreamMotionExample().Run();}}
Summary
Stream Motion (J519) provides powerful real-time robot control, but requires careful implementation:
- ✅ Install J519 option on your controller
- ✅ Run the TP program with
IBGN start[]/IBGN end[] - ✅ Implement smooth interpolation - never jump to positions
- ✅ Follow the protocol state machine - check status before sending
- ✅ Query and respect axis limits - velocity, acceleration, jerk
- ✅ Handle errors gracefully - implement reconnection logic