Digital I/O signal activation

[pala5]

Helo @mhubii.

I have a question that came up while using your packages (lbr_frI_ros2_stack and fri).
I have the LBR Iiwa 14 820R with the Sunrise.OS 1.16.1.9 (FRI 1.16), and I am working with Ubuntu 22.04 and ROS2 Humble distro.

We have installed an external digital I/O card, configurable from Sunrise.Workvisual, required to control some external devices.
I have seen, that in the FRIClientSDK, for the FRI 1.16 version, in the frilLBRCommand class, there is the function setDigitalIOValue, but I did not found this functionality in this package.

I am missing something or it was not considered?
Could you provide some help regarding this issue?

[mhubii]

Hi @pala5 , thanks for raising this issue. This is indeed missing right now

[pala5]

Hi @mhubii, are you planning to work on it soon?

[mhubii]

well this is an important feature that should be supported. How urgent is it on your end?

[pala5]

Well, I could say it is a matter of life and death.

[pala5]

Hey @mhubii!
Do you have time to work on that?
Thanks

[mhubii]

really occupied these days :(, I will give it a quick look tomorrow, hope this get you unstuck. Genuinely sorry

[pala5]

Hey @mhubii, do not worry! Thank you very much for your effort.

[pala5]

Hey @mhubii!
I don't want to be a bore, but do you have any update?
Thanks

[mhubii]

@mhubii Hello, in my previous work, I implemented the connection between the motorized spindle -Beckhoff- robot control cabinet, which enabled me to control the start and stop of the motorized spindle and the speed through the sunrise workbench. Start-stop is achieved by sending a digital signal of DC+24V, and the speed is achieved by sending a continuous DC 0-10V analog signal. Now I want to realize the control of ROS2 and motorized spindle through FRI-1.15, but I have no clue. Could you please give me some advice, so that I can understand how I should modify fri to achieve my goal?

Originally posted by @TalentLeshen-DLUT in #231 (comment)

I looked through the KUKA Fast Robot Interface C++ SDK (version 1.15), I think I can by setting

1. void KUKA: : FRI: : LBRCommand: : setBooleanIOValue (const char *, const bool),
2. void KUKA: : FRI: : LBRCommand: : setDigitalIOValue (const char , const unsigned long long),
3. void KUKA: : FRI: : LBRCommand: : setAnalogIOValue (const char , const double)

to make robot sent to receive ROS2 bool as well as the simulation, to control the start-stop of the motorized spindle and speed,
But I have no idea how to get ROS2 to send msgs.

Originally posted by @TalentLeshen-DLUT in #231 (comment)

[mhubii]

here is a list of things that would need to be done

• Add I/O to IDL messages: https://github.com/lbr-stack/lbr_fri_idl (this could be a starting point)
• Add I/O to lbr_fri_ros2: https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/rolling/lbr_fri_ros2/src/interfaces
• Add I/O to lbr_ros2_control: https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/rolling/lbr_ros2_control
• Add I/O to robot_description: https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/rolling/lbr_description/ros2_control/lbr_system_interface.xacro
• Potentially modify the LBRServer.java

[TalentLeshen-DLUT]

@mhubii Hello, I have modified the project according to your method this week, you can see these changes in the fork below.
https://github.com/TalentLeshen-DLUT/lbr_fri_ros2_stack/tree/dev-humble-io
After I finished the modification, the project could be built with normal colcon, but when I connected the hardware, I found that I could not connect with it. I've tried for a long time, but I can't solve the problem. I think I need your help.

[mhubii]

hi @TalentLeshen-DLUT. Apologize the late response. Seems you've been putting quite some effort! I am back online now. Would you mind working this through together?

[TalentLeshen-DLUT]

@mhubii First let me wish you a belated Merry Christmas and a happy New Year. I can't imagine how happy I would be if I could get your help. I think I'd love for you to configure io with me. During your absence, I tried to modify lbr_fri_idl,lbr_fri_ros2 and lbr_ros2_control according to the method you gave. I created msg for analog io and digital io, and tried both modifying and creating analog_command files directly in position_command, which you can see in the following pages. (Correspondingly, I modified part of ros2_control)
https://github.com/TalentLeshen-DLUT/lbr_fri_ros2_stack/tree/dev-humble-io (In this project, I make changes directly in position_command,wrench_command, and torque_command)
https://github.com/TalentLeshen-DLUT/lbr_fri_ros2_stack/tree/dev-humble-io-2 (In this project, I created analog_command and modified digital_command)
In addition, I modified async_client, but the result is not satisfactory, they both have the same problem: miss state interfaces and miss command interfaces. I don't know what caused this problem, I guess it is that the hardware of lbr_bringup needs to be modified.

[TalentLeshen-DLUT]

@mhubii I'm sorry that the above may confuse you because I used machine translation and didn't check it. In fact, in Project 1, I tried to configure IO directly in the position,wrench, and torque commands. In Project 2, I tried to configure IO files (analog command and digital command) . But they ran into the same problem :miss state interface and miss command interface. I guess I need to change the hardware part of lbr bringup, but I'm not sure.

[mhubii]

no worries! Happy new year to you too! Very excited to work on this together. I am a little occupied today but accepted your invite and will try to raise issues / pull requests there. Hope we can build a robust solution and merge changes back in.

[TalentLeshen-DLUT]

Hi, @mhubii

I have implemented part of the io_command_controller on ROS2. When I run the following command:

ros2 launch lbr_bringup hardware.launch.py model:=iiwa14
I can see a topic: /lbr/io_command_controller/xxxx.

Now, I believe the next step is to modify the LBR_Server.java. Could anyone provide some advice or guidance on how to proceed, so I can complete this task as quickly as possible?

Any help would be greatly appreciated!

[TalentLeshen-DLUT]

I have successfully modified LBRServer.java and established a connection to the KUKA IIWA14 robot. However, when attempting to publish boolean and analog messages, I am unable to find the appropriate topics. I found the following topics:

- /lbr/boolean_command_topic
- /lbr/digital_command_topic
- /lbr/analog_command_topic

However, these are not what I need, as I cannot find topics related to Speed and Start. I suspect there may be an issue in my project setup, but I have not yet identified the root cause. I would appreciate any guidance to resolve this.

Any help would be greatly appreciated!

Project Link：
https://github.com/TalentLeshen-DLUT/lbr_fri_ros2_stack/tree/dev-humble-io-2

LBRSever.java:

package lbr_fri_ros2;

import static com.kuka.roboticsAPI.motionModel.BasicMotions.positionHold;

import java.util.Arrays;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException;

import javax.inject.Inject;  

import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.controllerModel.Controller;
import com.kuka.roboticsAPI.deviceModel.LBR;

import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
import com.kuka.roboticsAPI.motionModel.controlModeModel.*;
import com.kuka.connectivity.fastRobotInterface.*;
import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
import com.kuka.generated.ioAccess.SpindleIOGroup;

public class LBRServer extends RoboticsAPIApplication {
    // Robot and controller objects
    private LBR lbr_;
    private Controller lbr_controller_;

    // If dependency injection is supported, MediaFlangeIOGroup will be automatically injected
    @Inject
    protected SpindleIOGroup spindle;

    // Define control mode enum
    private enum CONTROL_MODE {
        POSITION_CONTROL,
        JOINT_IMPEDANCE_CONTROL,
        CARTESIAN_IMPEDANCE_CONTROL;
    }

    // Convert enum to a string array for user selection
    public static String[] getNames(Class<? extends Enum<?>> e) {
        return Arrays.toString(e.getEnumConstants()).replaceAll("^.|.$", "").split(", ");
    }

    // FRI parameter settings
    private String client_name_;
    private String[] client_names_ = {"172.31.1.148", "192.170.10.1"};
    private int send_period_;    
    private String[] send_periods_ = {"1", "2", "5", "10"};  // Send period in milliseconds

    // FRI-related objects
    private FRIConfiguration fri_configuration_;
    private FRISession fri_session_;
    private FRIJointOverlay fri_overlay_;

    // Motion control and command mode settings
    private AbstractMotionControlMode control_mode_;
    private String[] control_modes_ = getNames(CONTROL_MODE.class);
    private ClientCommandMode command_mode_;
    private String[] command_modes_ = getNames(ClientCommandMode.class);
    
    /**
     * Request user configuration: send period, remote IP, control mode, and command mode.
     */
    public void request_user_config() {
        // Choose the send period
        int id = getApplicationUI().displayModalDialog(
                ApplicationDialogType.QUESTION, 
                "Select the desired FRI send period [ms]:", 
                send_periods_
        );
        send_period_ = Integer.valueOf(send_periods_[id]);
        getLogger().info("Send period set to: " + send_period_);
        
        // Choose the remote IP address
        id = getApplicationUI().displayModalDialog(
                ApplicationDialogType.QUESTION,
                "Select your remote IP address:",
                client_names_
        );
        client_name_ = client_names_[id];
        getLogger().info("Remote address set to: " + client_name_);

        // Choose the control mode
        id = getApplicationUI().displayModalDialog(
                ApplicationDialogType.QUESTION,
                "Select the desired FRI control mode: ",
                control_modes_                
        );

        CONTROL_MODE control_mode = CONTROL_MODE.values()[id];
        switch (control_mode) {
            case POSITION_CONTROL:
                control_mode_ = new PositionControlMode();
                break;
            case JOINT_IMPEDANCE_CONTROL:
                control_mode_ = new JointImpedanceControlMode(200, 200, 200, 200, 200, 200, 200);
                break;
            case CARTESIAN_IMPEDANCE_CONTROL:
                control_mode_ = new CartesianImpedanceControlMode();
                break;
        }
        getLogger().info("Control mode set to: " + control_mode.name());
        
        // Choose the client command mode
        id = getApplicationUI().displayModalDialog(
                ApplicationDialogType.QUESTION,
                "Select the desired FRI client command mode: ",
                command_modes_                
        );
        command_mode_ = ClientCommandMode.values()[id];
        getLogger().info("Client command mode set to: " + command_mode_.name());    
    }
    
    /**
     * Configure the FRI connection and register MediaFlange IO.
     */
    public void configure_fri() {
        // Initialize FRIConfiguration using the existing method
        fri_configuration_ = FRIConfiguration.createRemoteConfiguration(lbr_, client_name_);
        fri_configuration_.setSendPeriodMilliSec(send_period_);
        
        // Register IO ports 
        if (spindle != null) {
            fri_configuration_.registerIO(spindle.getOutput("Speed"));
            fri_configuration_.registerIO(spindle.getOutput("Start"));
            getLogger().info("MediaFlange IO registered.");
        } else {
            getLogger().info("Spindle IO not available.");
        }
        
        getLogger().info("Creating FRI connection to " + fri_configuration_.getHostName());
        getLogger().info(
            "SendPeriod: " + fri_configuration_.getSendPeriodMilliSec() + "ms |" +
            " ReceiveMultiplier: " + fri_configuration_.getReceiveMultiplier()
        );
        
        fri_session_ = new FRISession(fri_configuration_);
        fri_overlay_ = new FRIJointOverlay(fri_session_, command_mode_);
        
        // Add an FRI session listener to monitor connection status and quality changes
        fri_session_.addFRISessionListener(new IFRISessionListener() {
            @Override
            public void onFRISessionStateChanged(FRIChannelInformation friChannelInformation) {
                getLogger().info("Session State change " + friChannelInformation.getFRISessionState().toString());
            }

            @Override
            public void onFRIConnectionQualityChanged(FRIChannelInformation friChannelInformation) {
                getLogger().info("Quality change signalled " + friChannelInformation.getQuality());
                getLogger().info("Jitter " + friChannelInformation.getJitter());
                getLogger().info("Latency " + friChannelInformation.getLatency());
            }
        });        
        
        // Attempt to establish the connection (wait up to 10 seconds)
        try {
            fri_session_.await(10, TimeUnit.SECONDS);
        } catch (final TimeoutException e) {
            getLogger().error(e.getLocalizedMessage());
            return;
        }
        
        getLogger().info("FRI connection established.");
    }
    
    @Override
    public void initialize() {
        // Get the controller and robot devices
        lbr_controller_ = (Controller) getContext().getControllers().toArray()[0];
        lbr_ = (LBR) lbr_controller_.getDevices().toArray()[0];
        
        // Request user configuration
        request_user_config();
        
        // Configure the FRI connection and register IO
        configure_fri();
    }

    @Override
    public void run() {
        // Hold the robot's current pose using the specified control mode, overlaying the FRI control layer
        lbr_.move(positionHold(control_mode_, -1, TimeUnit.SECONDS).addMotionOverlay(fri_overlay_));
    }
    
    @Override
    public void dispose() {
        // Close the FRI connection and release resources
        getLogger().info("Disposing FRI session.");
        fri_session_.close();
        super.dispose();
    }
    
    /**
     * Main entry point.
     * @param args Command line arguments.
     */
    public static void main(final String[] args) {
        LBRServer app = new LBRServer();
        app.runApplication();
    }
}