fanuc_ucl 1.5.0

No project description provided

Unofficial Control Library for FANUC Robots

A library implementing a variety of FANUC robot proprietary protocols such as:

• Stream Motion (stmo)
• High Speed Position Output (hspo)
• Remote Motion Interface (rmi)
• SNPX based "HMI" (hmi)

The library is implemented in rust with the ability to be used as a crate in rust or a python module via pyo3. Has been tested with Linux(x86_64 and arm64), Windows(x86_64) and MacOS(arm64) although it likely works on all architectures that windows and macos support.

Installation

Rust

Add the following to your Cargo.toml:

[dependencies]
fanuc_ucl = "1"

or run cargo add fanuc_ucl in your project directory.

Python

The library is available on PyPI as fanuc_ucl, so you can install it using pip:

pip install fanuc_ucl==1

Usage

Python and rust have nearly identical APIs. The Python API is as strongly typed as possible with advanced type hints.

Unit and Format Safe Joint Representations

Every instance of working with direct joint representations requires also specifying the format and template of the joints, ensuring that the user is always aware of the units and the j2/j3 convention being used. Utilities to convert between different formats and templates are also provided as an independent API.

JointFormats:

• FanucDeg: Angles in degrees, with the j3 angle relative to the ground plane
• FanucRad: Angles in radians, with the j3 angle relative to the ground plane
• AbsDeg: Angles in degrees, with the j3 angle relative to the previous joint
• AbsRad: Angles in radians, with the j3 angle relative to the previous joint

JointTemplates can be arbitrarily created but the struct comes with a few pre-defined ones:

• SIX: The common 6 axis joint configuration of most FANUC robots
• SIX_LINEAR_TRACK: A 6 axis robot on a linear track, with the track being the 7th joint
• FOUR: A 4 axis joint configuration, common for FANUC palletizing robots
• FOUR_LINEAR_TRACK: A 4 axis robot on a linear track, with the track being the 5th joint
• FIVE: A 5 axis joint configuration, somewhat common for FANUC palletizing robots
• FIVE_LINEAR_TRACK: A 5 axis robot on a linear track, with the track being the 6th joint

use fanuc_ucl::joints::{JointFormat, JointTemplate};

fn main() {
    let joints = vec![-90.0, 0.0, 0.0, -180.0, 90.0, 180.0];
    let joints_conv = JointFormat::AbsRad.convert_from(JointFormat::FanucDeg, JointTemplate::SIX, joints);
    println!("Converted joints: {:?}", joints_conv);
}

from fanuc_ucl import JointFormat, JointTemplate

def main():
    joints = [-90.0, 0.0, 0.0, -180.0, 90.0, 180.0]
    joints_conv = JointFormat.AbsRad.convert_from(JointFormat.FanucDeg, JointTemplate.SIX, joints)
    print(f"Converted joints: {joints_conv}")

Stream Motion

Stream Motion gives real-time joint-level control of the robot at the interpolation rate (typically 8ms). The controller requests a position every cycle and the driver's I/O thread answers from a queue of motion commands; command_motion returns a handle that is set once the whole batch has been transmitted. The second argument to StreamMotionDriver::new controls whether the stream is marked finished when the queue runs dry, and individual packets can be flagged with set_last_command.

Batch streaming

use std::time::Duration;

use fanuc_ucl::{
    ThreadConfig,
    joints::{JointFormat, JointTemplate},
    stmo::{StreamMotionDriver, proto::MotionCommandPacket},
};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut driver = StreamMotionDriver::new([10, 0, 0, 1], false);
    driver.connect(Some(ThreadConfig::new(80, None)))?;
    driver.start(2.0)?;

    let limits = driver.fetch_movement_limits(0)?;
    println!("Velocity cap: {}", limits.vmax);

    // one command per cycle: sweep J1 through a 10 degree sine wave over 4 seconds
    let home = [0.0f32, 0.0, 0.0, 0.0, -90.0, 0.0];
    let mut commands = Vec::with_capacity(500);
    for i in 0..500 {
        let mut joints = home;
        joints[0] += 10.0 * (i as f32 / 500.0 * std::f32::consts::TAU).sin();
        commands.push(MotionCommandPacket::try_from_joints(
            JointFormat::FanucDeg,
            JointTemplate::SIX,
            joints,
        )?);
    }

    let handle = driver.command_motion(commands)?;
    // do other work while the batch streams out …
    handle.wait_timeout(Duration::from_secs(10))?;

    driver.stop();
    driver.disconnect();
    Ok(())
}

import math

from fanuc_ucl import JointFormat, JointTemplate, ThreadConfig, stmo

def main():
    driver = stmo.StreamMotionDriver("10.0.0.1")
    driver.connect(ThreadConfig(80, None))
    driver.start(2.0)

    limits = driver.fetch_movement_limits(0)
    print(f"Velocity cap: {limits.vmax}")

    # one command per cycle: sweep J1 through a 10 degree sine wave over 4 seconds
    home = [0.0, 0.0, 0.0, 0.0, -90.0, 0.0]
    commands = []
    for i in range(500):
        joints = home.copy()
        joints[0] += 10.0 * math.sin(i / 500.0 * math.tau)
        commands.append(stmo.MotionCommandPacket.try_from_joints(
            JointFormat.FanucDeg, JointTemplate.SIX, joints,
        ))

    handle = driver.command_motion(commands)
    # do other work while the batch streams out …
    handle.wait_timeout(10.0)

    driver.stop()
    driver.disconnect()

In-the-loop control

The control loop interface (StmoControlLoop) reacts to each robot status cycle as it arrives — useful for sensor-based feedback or adaptive trajectories.

use std::time::Duration;

use fanuc_ucl::{
    ThreadConfig,
    joints::{JointFormat, JointTemplate},
    stmo::{StreamMotionDriver, proto::MotionCommandPacket},
};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut driver = StreamMotionDriver::new([10, 0, 0, 1], false);
    driver.connect(Some(ThreadConfig::new(80, None)))?;
    driver.start(2.0)?;

    {
        let mut ctl = driver.control_loop()?;
        for _ in 0..500 {
            let status = ctl.wait_for_status(Duration::from_millis(100))?;
            let mut joints = status.joints(JointFormat::FanucDeg, JointTemplate::SIX);
            // compute the next setpoint from current feedback …
            joints[5] += 0.05;
            ctl.send_command(MotionCommandPacket::try_from_joints(
                JointFormat::FanucDeg,
                JointTemplate::SIX,
                joints,
            )?)?;
        }
    } // control loop dropped — the driver resumes normal queue behaviour

    driver.stop();
    driver.disconnect();
    Ok(())
}

from fanuc_ucl import JointFormat, JointTemplate, ThreadConfig, stmo

def main():
    driver = stmo.StreamMotionDriver("10.0.0.1")
    driver.connect(ThreadConfig(80, None))
    driver.start(2.0)

    with driver.control_loop() as ctl:
        for _ in range(500):
            status = ctl.wait_for_status(0.1)
            joints = list(status.joints(JointFormat.FanucDeg, JointTemplate.SIX))
            # compute the next setpoint from current feedback …
            joints[5] += 0.05
            ctl.send_command(stmo.MotionCommandPacket.try_from_joints(
                JointFormat.FanucDeg, JointTemplate.SIX, joints,
            ))

    driver.stop()
    driver.disconnect()

RMI

use std::time::Duration;

use fanuc_ucl::{rmi::{RmiDriver, RmiDriverConfig, proto}, joints::{JointFormat, JointTemplate}};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut driver = RmiDriver::new(RmiDriverConfig::default_with_ip([10, 0, 0, 1]));
    driver.connect(Some(ThreadConfig::new(80, None)))?;

    driver.send_full_reset()?.wait_timeout(Duration::from_secs(15))?;
    driver.send(proto::FrcInitialize::default())?.wait_timeout(Duration::from_secs(15))?;
    driver.send(proto::FrcSetOverRide::new(80))?.wait_timeout(Duration::from_secs(1))?;
    driver.send(proto::FrcSetUTool::new(1))?.wait_timeout(Duration::from_secs(1))?;
    driver.send(proto::FrcSetUFrame::new(1))?.wait_timeout(Duration::from_secs(1))?;

    let movement_cmd1 = proto::FrcJointMotionJRep::new(
        proto::JointAngles::new6(
            JointFormat::AbsDeg,
            JointTemplate::SIX,
            -90.0, 0.0, 0.0, -180.0, 90.0, 180.0
        ),
        proto::SpeedType::MilliSeconds,
        528,
        proto::TermType::FINE,
        0
    );
    let movement_cmd2 = proto::FrcJointMotionJRep::new(
        proto::JointAngles::new6(
            JointFormat::AbsDeg,
            JointTemplate::SIX,
            90.0, 0.0, 0.0, 180.0, -90.0, -180.0
        ),
        proto::SpeedType::MilliSeconds,
        528,
        proto::TermType::FINE,
        0
    );
    driver.send(movement_cmd1)?.wait_timeout(Duration::from_millis(600))?;
    driver.send(proto::FrcWaitTime::new(Duration::from_secs(1)))?.wait()?;
    driver.send(movement_cmd2)?.wait_timeout(Duration::from_millis(600))?;

    let pos_resp = driver.send(proto::FrcReadJointAngles::new(None))?.wait()?;
    println!("Current joint angles: {:?}", pos_resp.joints(JointFormat::AbsDeg, JointTemplate::SIX).as_array());

    Ok(())
}

from fanuc_ucl import JointFormat, JointTemplate, ThreadConfig, rmi

def main():
    driver = rmi.RmiDriver(rmi.RmiDriverConfig("10.0.0.1"))
    driver.connect(ThreadConfig(80, None))

    driver.send_full_reset().wait_timeout(20.0)
    driver.send(rmi.FrcInitialize()).wait_timeout(20.0)
    driver.send(rmi.FrcSetOverRide(80)).wait_timeout(1.0)
    driver.send(rmi.FrcSetUTool(1)).wait_timeout(1.0)
    driver.send(rmi.FrcSetUFrame(1)).wait_timeout(1.0)

    movement_cmd1 = rmi.FrcJointMotionJRep(
        rmi.JointAngles(
            JointFormat.AbsDeg,
            JointTemplate.SIX,
            *[-90.0, 0.0, 0.0, -180.0, 90.0, 180.0]
        ),
        rmi.SpeedType.MilliSeconds,
        528,
        rmi.TermType.FINE,
        0,
    )
    movement_cmd2 = rmi.FrcJointMotionJRep(
        rmi.JointAngles(
            JointFormat.AbsDeg,
            JointTemplate.SIX,
            *[90.0, 0.0, 0.0, 180.0, -90.0, -180.0]
        ),
        rmi.SpeedType.MilliSeconds,
        528,
        rmi.TermType.FINE,
        0,
    )
    driver.send(movement_cmd1).wait_timeout(0.6)
    driver.send(rmi.FrcWaitTime(1.0)).wait()
    driver.send(movement_cmd2).wait_timeout(0.6)

    pos_resp = driver.send(rmi.FrcReadJointAngles()).wait_timeout(0.2)
    print(f"Current joint angles: {pos_resp.joints(JointFormat.AbsDeg, JointTemplate.SIX).as_array()}")

HSPO

use std::{net::SocketAddr, thread::sleep, time::Duration};

use fanuc_ucl::{ThreadConfig, hspo::{HspoReceiver, destroy_broker, initialize_broker}, joints::{JointFormat, JointTemplate}};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    initialize_broker(
        SocketAddr::from(([0, 0, 0, 0], 15000)),
        Some(ThreadConfig::new(55, None)),
    ).expect("Broker couldnt be started");

    let receiver = HspoReceiver::try_new([10, 0, 0, 1], 128, Duration::from_millis(10))?;

    if let Some(joint_packet) = receiver.joint.wait_for(Duration::from_millis(16)) {
        println!(
            "Received joint packet: {:?}",
            joint_packet.joints(JointFormat::AbsDeg, JointTemplate::SIX)
        );
    }

    receiver.tcp.clear();
    sleep(Duration::from_secs(1));
    let opt_tcp_packet = receiver.tcp.try_recv();
    match opt_tcp_packet {
        Some(packet) => println!("Received TCP packet: {:?}", packet),
        None => println!("No TCP packet received within the timeout."),
    }
    let var_packets = receiver.var.recv_all();
    println!(
        "Received {} Variables packets: {:?}",
        var_packets.len(),
        var_packets
    );

    destroy_broker(true);
    Ok(())
}

from fanuc_ucl import JointFormat, JointTemplate, ThreadConfig, hspo

def main():
    hspo.initialize_broker("0.0.0.0:15000", ThreadConfig(55, None))

    receiver = hspo.HspoReceiver("10.0.0.1", 128)

    joint_packet = receiver.joint.wait_for(0.016)
    if joint_packet is not None:
        print(f"Received joint packet: {joint_packet.joints(JointFormat.AbsDeg, JointTemplate.SIX)}")

    receiver.tcp.clear()
    tcp_packet = receiver.tcp.try_recv()
    if tcp_packet is not None:
        print(f"Received TCP packet: {tcp_packet}")
    else:
        print("No TCP packet received within the timeout.")
    var_packets = receiver.var.recv_all()
    print(f"Received {len(var_packets)} Variables packets: {var_packets}")

    hspo.destroy_broker()

HMI

use std::time::Duration;

use fanuc_ucl::hmi::{DigitalOutput, GroupInput, GroupOutput, HmiDriver, SysVarArgs};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let mut driver = HmiDriver::new([10, 0, 0, 1]);
    driver.connect(Some(Duration::from_secs(1)), None)?;

    if driver.read::<DigitalOutput>(1)?.wait_timeout(Duration::from_millis(10))? {
        println!("DO1 is on");
    } else {
        println!("DO1 is off");
    }

    let groups = driver.read_array::<GroupInput>(1, 40)?.wait_timeout(Duration::from_millis(10))?;
    for (i, group) in groups.iter().enumerate() {
        println!("Group {}: {:?}", i + 1, group);
    }

    driver.write::<DigitalOutput>(1, true)?;
    driver.write_array::<GroupOutput>(1, &[10, 11, 12, 13])?;
    driver.write_array_unsafe::<GroupInput>(1, &[10, 11, 12, 13])?;

    let hspo_enable_var = driver.register_asg(
        SysVarArgs{
            var_name: "$MCGR_CFG.$ENABLE".to_string(),
            ..Default::default()
        },
        Duration::from_millis(24)
    )?;
    if hspo_enable_var.read(&driver)?.wait_timeout(Duration::from_millis(10))? {
        println!("HSPO is already enabled");
    } else {
        hspo_enable_var.write(&driver, true)?;
        println!("HSPO is now enabled");
    }

    // ASG has ALOT of functionality, so we won't cover it all here.
    // When the Rustdocs are finished it will delve more into it.

    Ok(())
}

from fanuc_ucl import hmi

def main():
    driver = hmi.HmiDriver("10.0.0.1")
    driver.connect()

    if driver.read(hmi.DigitalOutput, 1).wait_timeout(0.01):
        print("DO1 is on")
    else:
        print("DO1 is off")

    groups = driver.read(hmi.GroupInput, 1, 40).wait_timeout(0.01)
    for i, group in enumerate(groups):
        print(f"Group {i + 1}: {group}")

    driver.write(hmi.DigitalOutput, 1, True)
    driver.write(hmi.GroupOutput, 1, [10, 11, 12, 13])
    driver.write_unsafe(hmi.GroupInput, 1, [10, 11, 12, 13])

    hspo_enable_var = driver.register_sysvar_asg(bool, name="$MCGR_CFG.$ENABLE")
    if hspo_enable_var.read(driver).wait_timeout(0.01):
        print("HSPO is already enabled")
    else:
        hspo_enable_var.write(driver, True)
        print("HSPO is now enabled")

    # ASG has ALOT of functionality, so we won't cover it all here.
    # When the Rustdocs are finished it will delve more into it.

Roadmap

• Pydocs and Rustdocs for all public APIs
• Switch python terminal logging to pylog instead of tracing
• Implement an "In The Loop" interface for the StreamMotionDriver to make using feedback from sensors easier.
• Implement a unit-safe api for working with Cartesian poses.
• Update docs to show the usage of stream motion and in-the-loop examples
• Update docs to show the usage of async rmi/hmi response handles
• Add async to hspo
• Add support for async to python
• Removing all possible panic locations and have graceful error handling for all failure modes.
• Extensive unit testing, I wrote a special network testing library for this I just need to write the actual tests using it
• A C api for the library, to allow usage from other languages like c#. The main issue is the extensive usage of rust style enums and traits in the API, so this will require some careful design to make a clean and safe C api. This is a long term goal and will likely be a separate crate that depends on this one.