UnderAutomation
¿Una pregunta?

[email protected]

Contactos
UnderAutomation
⌘Q
Fanuc SDK documentation
SNPX
Documentation home

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

FeatureLimitation
Motion groupsGroup 1 (robot) + up to 3 auxiliary axes
Coordinate modeJoint OR Cartesian (cannot switch mid-session)
Update rate125Hz or 250Hz (controller dependent)
Control typePosition only (no velocity/torque control)

Quick Start

using UnderAutomation.Fanuc;
using UnderAutomation.Fanuc.StreamMotion;
using UnderAutomation.Fanuc.StreamMotion.Data;
// Create and connect
var robot = new FanucRobot();
robot.StreamMotion.Connect("192.168.1.1");
// Handle state packets
robot.StreamMotion.StateReceived += (sender, e) =>
{
var state = e.State;
Console.WriteLine($"Position: J1={state.JointPosition.J1:F2}°");
// Check if robot is ready for next command
if (state.Status.ReadyForCommands &&
!state.Status.InMotion &&
!state.Status.CommandReceived)
{
// Send your interpolated position here
var 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 streaming
robot.StreamMotion.Start();
// ... your application logic ...
// Stop and disconnect
robot.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

  1. Connect - Establish UDP socket connection
  2. Start - Send start packet to begin streaming session
  3. Receive States - Robot sends state packets at ~500Hz
  4. Send Commands - Your app sends interpolated positions at 125/250Hz
  5. Stop - Send stop packet to end session
  6. 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
/ATTR
OWNER = 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;
/MN
1: LBL[1] ;
2: IBGN start[1] ;
3: IBGN end[1] ;
4: JMP LBL[1] ;
/POS
/END

Program Explanation

LineInstructionDescription
1LBL[1]Loop label
2IBGN start[1]Start Stream Motion session
3IBGN end[1]End Stream Motion session
4JMP LBL[1]Loop back to handle next session

Running the Program

  1. Upload the program to the robot via FTP or teach pendant
  2. Start the program in AUTO mode
  3. The program loops, waiting for Stream Motion connections
  4. Your application connects and controls motion via UDP
  5. 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:

/MN
1: LBL[1] ;
2: DO[1]=ON ; // Turn on output before streaming
3: IBGN start[1] ;
4: IBGN end[1] ;
5: DO[1]=OFF ; // Turn off output after streaming
6: WAIT 1.00(sec) ; // Wait before next session
7: 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 settings
robot.StreamMotion.Connect(
ip: "192.168.1.1",
port: 60015,
sendTimeoutMs: 1000,
receiveTimeoutMs: 1000
);
// Check connection status
if (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 packets
robot.StreamMotion.Start();
// Check streaming status
if (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 streaming
robot.StreamMotion.Stop();
// Disconnect
robot.StreamMotion.Disconnect();

State Packets

The robot sends state packets continuously at approximately 500Hz. Each packet contains the robot's current state.

StatePacket Properties

PropertyTypeDescription
SequenceNumberuintPacket sequence for synchronization
TimeStampuintRobot timestamp (ms, 2ms resolution)
StatusRobotStatusStatus flags
JointPositionMotionDataCurrent joint positions (J1-J9)
CartesianPositionMotionDataCurrent Cartesian position (X,Y,Z,W,P,R + E1-E3)
MotorCurrentsMotionDataMotor currents in Amperes
IOReadIOReadResultResult of I/O read operation

RobotStatus Flags

FlagDescription
ReadyForCommandsRobot is ready to receive command packets
CommandReceivedRobot has received and is processing a command
SystemReadySystem ready (SYSRDY)
InMotionRobot is currently executing motion

Handling State Packets

robot.StreamMotion.StateReceived += (sender, e) =>
{
var state = e.State;
// Read current joint positions
Console.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 position
Console.WriteLine($"X: {state.CartesianPosition.X:F2} mm");
Console.WriteLine($"Y: {state.CartesianPosition.Y:F2} mm");
Console.WriteLine($"Z: {state.CartesianPosition.Z:F2} mm");
// Check status
Console.WriteLine($"Ready: {state.Status.ReadyForCommands}");
Console.WriteLine($"In Motion: {state.Status.InMotion}");
// Read motor currents
Console.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

PropertyTypeDescription
DataStyleDataStyleJoint or Cartesian
MotionDataMotionDataTarget position
IsLastDataboolMarks end of motion sequence
ReadIOTypeIOTypeType of I/O to read
ReadIOIndexushortI/O index to read
ReadIOMaskushortI/O read mask
WriteIOTypeIOTypeType of I/O to write
WriteIOIndexushortI/O index to write
WriteIOMaskushortI/O write mask
WriteIOValueushortValue to write

MotionData Structure

MotionData holds 9 axis values that can be accessed by axis number or name:

var motion = new MotionData();
// Joint-style access
motion.J1 = 10.0f; // Joint 1
motion.J2 = 20.0f; // Joint 2
motion.J3 = 30.0f; // Joint 3
motion.J4 = 0.0f; // Joint 4
motion.J5 = -45.0f; // Joint 5
motion.J6 = 90.0f; // Joint 6
motion.J7 = 0.0f; // Extended axis 1
motion.J8 = 0.0f; // Extended axis 2
motion.J9 = 0.0f; // Extended axis 3
// Cartesian-style access
motion.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 1
motion.E2 = 0.0f; // Extended axis 2
motion.E3 = 0.0f; // Extended axis 3
// Generic axis access
motion.Axis1 = 10.0f;
motion.Values[0] = 10.0f; // Raw array access

Sending Commands

// Method 1: Full command packet
var 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 motion
robot.StreamMotion.SendJointCommand(
new MotionData { J1 = 10, J2 = 20, J3 = 30, J4 = 0, J5 = -45, J6 = 90 },
isLastData: false
);
// Method 3: Convenience method for Cartesian motion
robot.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 conditions
bool canSendCommand = status.ReadyForCommands &&
!status.CommandReceived &&
!status.InMotion;
if (canSendCommand)
{
// Safe to send next command
robot.StreamMotion.SendCommand(nextCommand);
}
};

⚠️ Common Mistakes

MistakeConsequence
Sending commands too fastCommands dropped, erratic motion
Ignoring CommandReceivedDuplicate commands sent
Ignoring InMotionBuffer overflow, jerk errors
Not checking ReadyForCommandsCommands 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;
}
}

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 parameters
double 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 axis
if (_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 profile
return 2 * Math.Sqrt(distance / aMax);
}
else
{
// Trapezoidal profile
double 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 handling
if (_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 send
if (!status.ReadyForCommands || status.InMotion || status.CommandReceived)
return;
// Get next interpolated position
if (_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 1
var velocityAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Velocity);
Console.WriteLine($"Axis 1 max velocity: {velocityAck.ThresholdsNoLoad[19]:F2} deg/s");
// Query acceleration limits
var accelAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Acceleration);
Console.WriteLine($"Axis 1 max acceleration: {accelAck.ThresholdsNoLoad[19]:F2} deg/s²");
// Query jerk limits
var jerkAck = robot.StreamMotion.RequestThreshold(1, ThresholdType.Jerk);
Console.WriteLine($"Axis 1 max jerk: {jerkAck.ThresholdsNoLoad[19]:F2} deg/s³");
// Query all axes
for (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:

IndexVelocity %
05%
110%
......
19100%

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 packet
robot.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 0
WriteIOValue = 0x0001 // Set bit 0 ON
};
robot.StreamMotion.SendCommand(command);

Best Practices

1. Always Implement Proper Interpolation

// ❌ WRONG - Will cause jerk errors
robot.StreamMotion.SendJointCommand(new MotionData { J1 = 90 }); // Jump to 90°
// ✅ CORRECT - Smooth interpolated motion
for (double t = 0; t <= 1.0; t += 0.008) // 125Hz
{
double s = t * t * t * (t * (t * 6 - 15) + 10); // S-curve
double j1 = startJ1 + (targetJ1 - startJ1) * s;
// ... wait for proper state, then send
}

2. Respect the Protocol State Machine

// ❌ WRONG - Ignoring state
robot.StreamMotion.StateReceived += (s, e) =>
{
robot.StreamMotion.SendCommand(command); // May fail!
};
// ✅ CORRECT - Check state first
robot.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 reconnection
TryReconnect();
}
};

4. Query Limits Before Motion

// Get axis limits before starting motion
var 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 streaming
robot.StreamMotion.Start();

5. Clean Shutdown

try
{
// Your streaming logic
}
finally
{
if (robot.StreamMotion.IsStreaming)
{
robot.StreamMotion.Stop();
}
}

Troubleshooting

Common Errors

ErrorCauseSolution
"Joint jerk limit exceeded"Motion too fast/abruptImplement proper S-curve interpolation
"Robot not ready for commands"Wrong stateWait for ReadyForCommands=true
Connection timeoutNetwork issueCheck IP, port, firewall
"Streaming already started"Double startCheck IsStreaming before Start()
Commands not executedSent too fastFollow 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
{
// Connect
Console.WriteLine("Connecting to robot...");
_robot.StreamMotion.Connect("192.168.1.1");
Console.WriteLine("Connected!");
// Query limits
Console.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 streaming
Console.WriteLine("Starting streaming...");
_robot.StreamMotion.Start();
// Wait for completion
Console.WriteLine("Moving through waypoints... Press Enter to stop.");
Console.ReadLine();
}
finally
{
// Clean shutdown
Console.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 waiting
if (_waitingForAck)
{
if (!status.InMotion && !status.CommandReceived)
{
_waitingForAck = false;
}
else
{
return; // Still waiting
}
}
// Check if ready to send
if (!status.ReadyForCommands || status.InMotion || status.CommandReceived)
return;
// Start next waypoint if current move complete
if (!_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 position
if (_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:

  1. Install J519 option on your controller
  2. Run the TP program with IBGN start[] / IBGN end[]
  3. Implement smooth interpolation - never jump to positions
  4. Follow the protocol state machine - check status before sending
  5. Query and respect axis limits - velocity, acceleration, jerk
  6. Handle errors gracefully - implement reconnection logic

Integre fácilmente robots Universal Robots, Fanuc, Yaskawa o Staubli en sus aplicaciones .NET, Python, LabVIEW o Matlab

UnderAutomation
ContactosLegal

© All rights reserved.