search

ros-industrial / kuka_experimental

Experimental packages for KUKA manipulators within ROS-Industrial (http://wiki.ros.org/kuka_experimental)
Apache License 2.0
277 stars 218 forks source link

kuka_experimental/kuka_eki_hw_interface for KRC2 #196

Closed Levaru closed 1 year ago

Levaru commented 3 years ago

Hi, we have loaned a robot to test our software but we were only able to get an old one with the KRC2 controller. I successfully installed the KUKA Ethernet KRL Interface software (Version 1.2) but encountered problems when trying to implement kuka_experimental/kuka_eki_hw_interface.

It seems that the kuka_eki_hw_interface was written with KUKA.Ethernet KRL 2.x which has a different xml structure and commands compared to 1.2.

Here are some things that are different in 1.2:

I'm really lost concerning the last point. I was able to rename most of the functions but I'm not sure how to write those three files. I'm guessing that I'll also have to rewrite some of the source code for the kuka_eki_hw_interface ROS-node to make it compatible with the old version.

Does someone have any pointers on how to properly adjust the files for EKI V1.2?

gavanderhoorn commented 3 years ago

I'm (personally) not aware of anyone having performed this migration/port.

You could see whether any of the forks contain the changes you need.

Levaru commented 3 years ago

I managed to port it but it required some workarounds. The two most important changes are:

  1. Changing the socket in the ROS-Program from a UDP-Client to a TCP-Server. I'm actually not sure if TCP is absolutely required but that's what the documentation says.
  2. The 1.2 version doesn't have the EKI_CheckBuffer() function, so I had to implement a command counter that gets updated once the roboter reads the next command. Here I'm also not sure if this is the correct solution, because I get some very choppy/laggy movement.

Another thing that really confuses me are the EKX_Get***Element() functions. Parameter 1 of these functions has the following description:

INTEGER Determines whether the next value or the current value is to be read from the buffer INT=0: the next value is read (First In First Out) INT=1: the current value is read (Last In First Out)

At first I had this set to 0 since I wouldn't want to miss any commands from the buffer right? This led to some strange movement behaviour, where the robot would oscillate during the movement by moving forward and then back a little bit, before executing the rest of the command normally. Some other times the robot would move 80% of the trajectory, then got back to the first 20%, then again forward to 80% and would repeat this multiple times, before finally reaching the target. After setting the parameter to 1 I didn't encounter this anymore, aside from some very choppy/laggy movement.

Here are my changes, could somebody please make a sanity check?

EkiHwInterface.xml

<Elements>
  <Element Tag="RobotCommand" Type="STRUCTTAG" Stacksize="5" />
    <Element Tag="RobotCommand.ID" Type="INTEGER" Stacksize="5" />
    <Element Tag="RobotCommand.Pos" Type="STRUCTTAG" Stacksize="5" />
      <Element Tag="RobotCommand.Pos.A1" Type="REAL" Stacksize="5" />
      <Element Tag="RobotCommand.Pos.A2" Type="REAL" Stacksize="5" />
      <Element Tag="RobotCommand.Pos.A3" Type="REAL" Stacksize="5" />
      <Element Tag="RobotCommand.Pos.A4" Type="REAL" Stacksize="5" />
      <Element Tag="RobotCommand.Pos.A5" Type="REAL" Stacksize="5" />
      <Element Tag="RobotCommand.Pos.A6" Type="REAL" Stacksize="5" />
</Elements>

EkiHwInterface+.xml

<RobotState>
  <Pos A1="" A2="" A3="" A4="" A5="" A6="">
  </Pos>
  <Vel A1="" A2="" A3="" A4="" A5="" A6="">
  </Vel>
  <Eff A1="" A2="" A3="" A4="" A5="" A6="">
  </Eff>
  <RobotCommand ID="" Size="5">
  </RobotCommand>
</RobotState>

XmlApiConfig.xml

<?xml version="1.0"?>
<XmlApiConfig xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="XMLCommunicationSetup.xsd">
  <XmlApiParam InitOnce="false"/>
  <Channel SensorName="EkiHwInterface" SensorType="EkiHwInterface">
    <TCP_IP IP="192.168.16.98" Port="54601" Route="true" MapPort="54601"/>
  </Channel>
</XmlApiConfig>

kuka_eki_hw_interface.src

&ACCESS RVP
&REL 5
def  kuka_eki_hw_interface()
  ; Declarations
  decl axis joint_pos_tgt
  decl int elements_read

  ; INI
  bas(#initmov, 0)  ; Basic initialization of axes

  eki_hw_iface_init()

  joint_pos_tgt = $axis_act
  ptp joint_pos_tgt

  ; Loop forever
  $advance = 5
  LOOP
    elements_read = eki_hw_iface_get(joint_pos_tgt)  ; Get new command from buffer if present
    ptp joint_pos_tgt c_ptp                          ; PTP to most recent commanded position
  ENDLOOP
end

def eki_hw_iface_init()
  decl int ekx_ret
  id_count = 0
  ; Setup interrupts
  ; Interrupt 15 - Connection cleanup on client disconnect
  global interrupt decl 15 when $flag[1]==false do eki_hw_iface_reset()
  interrupt on 15
  ; Interrupt 16 - Timer interrupt for periodic state transmission
  global interrupt decl 16 when $timer_flag[1]==true do eki_hw_iface_send()
  interrupt on 16
  wait sec 0.012          ; Wait for next interpolation cycle
  $timer[1] = -200        ; Time in [ms] before first interrupt call
  $timer_stop[1] = false  ; Start timer 1

  ; Create and open EKI interface
  ; ekx_ret = eki_init("EkiHwInterface")
  ekx_ret = EKX_Open("EkiHwInterface")
  EKX_HandleError(ekx_ret)

  $flag[1] = true
end

def eki_hw_iface_send()
  decl int ekx_ret   
  decl real vel_percent      

  if $flag[1] then  ; If connection alive
    ; Load state values into xml structure

    ; position
    ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A1", $axis_act.a1)
    EKX_HandleError(ekx_ret)
    ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A2", $axis_act.a2)
    EKX_HandleError(ekx_ret)
    ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A3", $axis_act.a3)
    EKX_HandleError(ekx_ret)
    ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A4", $axis_act.a4)    
    EKX_HandleError(ekx_ret)
    ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A5", $axis_act.a5)
    EKX_HandleError(ekx_ret)
    ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A6", $axis_act.a6)
    EKX_HandleError(ekx_ret)

    ekx_ret = EKX_WriteInteger("EkiHwInterface.RobotState.RobotCommand.ID", id_count)
    EKX_HandleError(ekx_ret)

    ; Send xml structure
    if $flag[1] then  ; Make sure connection hasn't died while updating xml structure
      ekx_ret = EKX_Send("EkiHwInterface.RobotState")
      EKX_HandleError(ekx_ret)
    endif
  endif

  ; Set timer for next interrupt [ms]
  $timer[1] = -10  ; ~10 ms for above send + 10 ms interrupt timer -> ~50 Hz state transmission
end

; eki_hw_iface_get
; Tries to read most recent element from buffer. q left unchanged if empty.
; Returns number of elements read.
deffct int eki_hw_iface_get(joint_pos_cmd :out)
  decl bool ekx_ret
  decl axis joint_pos_cmd
  decl bool is_new 
  decl int rec_id

  ekx_ret = EKX_WaitForSensorData(1, "EkiHwInterface.RobotCommand.ID", 10000)
  ekx_ret = EKX_GetIntegerElement(1, "EkiHwInterface.RobotCommand.ID", rec_id, is_new)

  ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A1", joint_pos_cmd.a1, is_new)
  ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A2", joint_pos_cmd.a2, is_new)
  ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A3", joint_pos_cmd.a3, is_new)
  ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A4", joint_pos_cmd.a4, is_new)
  ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A5", joint_pos_cmd.a5, is_new)
  ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A6", joint_pos_cmd.a6, is_new)

  id_count = rec_id

  return 1
endfct

def eki_hw_iface_reset()
  decl int ekx_ret_int
  decl bool ekx_ret_bool

  ekx_ret_bool = EKX_Close("EkiHwInterface")
  ekx_ret_int = EKX_Open("EkiHwInterface")
  EKX_HandleError(ekx_ret_int)
  $flag[1] = true
end

kuka_eki_hw_interface.cpp

namespace kuka_eki_hw_interface
{

    KukaEkiHardwareInterface::KukaEkiHardwareInterface() 
    : old_joint_position_(n_dof_, 0.0)
    , joint_position_(n_dof_, 0.0)
    , joint_velocity_(n_dof_, 0.0)
    , joint_effort_(n_dof_, 0.0)
    , joint_position_command_(n_dof_, 0.0)
    , joint_names_(n_dof_)
    , deadline_(ios_)
    , eki_cmd_id_count_(0)
    , eki_cmd_buff_len_(0)
    , eki_server_socket_(ios_)
    , eki_acceptor_(ios_, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), 0))
    {

    }

    bool KukaEkiHardwareInterface::eki_read_state(std::vector<double> &joint_position,
                                                  std::vector<double> &joint_velocity,
                                                  std::vector<double> &joint_effort,
                                                  int &cmd_buff_len)
    {
        static boost::array<char, 2048> in_buffer;
        boost::asio::streambuf st_buffer;

        // Read socket buffer (with timeout)
        deadline_.expires_from_now(boost::posix_time::seconds(eki_read_state_timeout_));  // set deadline
        boost::system::error_code ec = boost::asio::error::would_block;

        size_t len = 0;
        eki_server_socket_.async_receive(boost::asio::buffer(in_buffer), boost::bind(&KukaEkiHardwareInterface::eki_handle_receive, _1, _2, &ec, &len));
        do ios_.run_one(); while (ec == boost::asio::error::would_block);

        if (ec)
            return false;

        // Update joint positions from XML packet (if received)
        if (len == 0)
            return false;

        // Parse XML
        TiXmlDocument xml_in;
        in_buffer[len] = '\0';  // null-terminate data buffer for parsing (expects c-string)
        xml_in.Parse(in_buffer.data());

        TiXmlElement* robot_state = xml_in.FirstChildElement("RobotState");
        if (!robot_state)
            return false;
        TiXmlElement* pos = robot_state->FirstChildElement("Pos");
        TiXmlElement* vel = robot_state->FirstChildElement("Vel");
        TiXmlElement* eff = robot_state->FirstChildElement("Eff");
        TiXmlElement* robot_command = robot_state->FirstChildElement("RobotCommand");
        if (!pos || !vel || !eff || !robot_command)
            return false;

        // Extract axis positions
        double joint_pos;  // [deg]
        double joint_vel;  // [%max]
        double joint_eff;  // [Nm]
        char axis_name[] = "A1";
        for (int i = 0; i < n_dof_; ++i)
        {
            pos->Attribute(axis_name, &joint_pos);
            joint_position[i] = angles::from_degrees(joint_pos);  // convert deg to rad
            vel->Attribute(axis_name, &joint_vel);
            joint_velocity[i] = joint_vel;
            eff->Attribute(axis_name, &joint_eff);
            joint_effort[i] = joint_eff;

            axis_name[1]++;
        }

        // Extract last command id that was received by the robot 
        int id_count;
        robot_command->Attribute("ID", &id_count);

        bool wrap_around = id_count > eki_cmd_id_count_;
        if(!wrap_around)
            cmd_buff_len = eki_cmd_id_count_ - id_count;
        else
            cmd_buff_len = (INT32_MAX - id_count) + eki_cmd_id_count_;

        return true;
    }

    bool KukaEkiHardwareInterface::eki_write_command(const std::vector<double> &joint_position_command)
    {
        if( (eki_cmd_id_count_ + 1) > INT32_MAX )
            eki_cmd_id_count_ = 0;
        else
            ++eki_cmd_id_count_;

        TiXmlDocument xml_out;
        TiXmlElement* robot_command = new TiXmlElement("RobotCommand");
        TiXmlElement* pos = new TiXmlElement("Pos");
        TiXmlText* empty_text = new TiXmlText("");
        robot_command->LinkEndChild(pos);
        pos->LinkEndChild(empty_text);   // force <Pos></Pos> format (vs <Pos />)
        char axis_name[] = "A1";

        robot_command->SetAttribute("ID", std::to_string(eki_cmd_id_count_));

        for (int i = 0; i < n_dof_; ++i)
        {
            pos->SetAttribute(axis_name, std::to_string(angles::to_degrees(joint_position_command[i])).c_str());
            axis_name[1]++;
        }
        xml_out.LinkEndChild(robot_command);

        TiXmlPrinter xml_printer;
        xml_printer.SetStreamPrinting();  // no linebreaks
        xml_out.Accept(&xml_printer);

        size_t len = eki_server_socket_.send(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()));

        return true;
    }

    void KukaEkiHardwareInterface::start()
    {
        ROS_INFO_NAMED("kuka_eki_hw_interface", "Starting Kuka EKI hardware interface...");

        // Start client
        ROS_INFO_NAMED("kuka_eki_hw_interface", "... connecting to robot's EKI server...");
        eki_server_endpoint_ = boost::asio::ip::tcp::endpoint(boost::asio::ip::address_v4::any(), std::stoi(eki_server_port_));

        ROS_WARN_STREAM("IP: " << eki_server_endpoint_.address().to_string()); 
        ROS_WARN_STREAM("PORT: " << eki_server_endpoint_.port());

        eki_acceptor_ = boost::asio::ip::tcp::acceptor(ios_, eki_server_endpoint_.protocol());

        boost::system::error_code ec;

        eki_acceptor_.bind(eki_server_endpoint_, ec);
        while(ec.value() == 98)
        {
            ROS_WARN_STREAM("[ERROR] Code: " << ec.value() << " MSG: " << ec.message());
            ROS_WARN("Retrying...");
            eki_acceptor_.bind(eki_server_endpoint_, ec);
            ros::Duration(0.1).sleep();
        }
        if(ec != 0)
        {
            ROS_WARN_STREAM("[ERROR] Code: " << ec.value() << " MSG: " << ec.message());
            throw boost::system::system_error(ec ? ec : boost::asio::error::operation_aborted);
        }
        ROS_WARN("Connected!");
        eki_acceptor_.listen(1);

        eki_acceptor_.accept(eki_server_socket_, eki_server_endpoint_);

        eki_acceptor_.close();

        // Start persistent actor to check for eki_read_state timeouts
        deadline_.expires_at(boost::posix_time::pos_infin);  // do nothing unit a read is invoked (deadline_ = +inf)
        eki_check_read_state_deadline();

        // Initialize joint_position_command_ from initial robot state (avoid bad (null) commands before controllers come up)
        if (!eki_read_state(joint_position_, joint_velocity_, joint_effort_, eki_cmd_buff_len_))
        {
            std::string msg = "Failed to read from robot EKI server within alloted time of "
                              + std::to_string(eki_read_state_timeout_) + " seconds.  Make sure eki_hw_interface is running "
                              "on the robot controller and all configurations are correct.";
            ROS_ERROR_STREAM(msg);
            throw std::runtime_error(msg);
        }
        joint_position_command_ = joint_position_;

        ROS_INFO_NAMED("kuka_eki_hw_interface", "... done. EKI hardware interface started!");
    }

} // namespace kuka_eki_hw_interface
gavanderhoorn commented 3 years ago

Thanks for reporting back.

If you want to make it easier for others to check out your changes, please fork this repository, commit your changes to a new branch, then open a Pull Request.

Right now it's really difficult to figure out what you've changed. By opening a PR, we immediately get a nicely rendered diff.

Levaru commented 3 years ago

I wanted to avoid this, since I changed the indentation in all files. But I changed it all back and made a pull-request with a readable diff. I hope that I did this correctly.

Levaru commented 3 years ago

Some additional info about the issue with the EKX_Get***Element() FIFO/LIFO parameter. Here is an image showing the state of a joint angle during each eki_read_state and the current joint_state_command_ during that state. You can clearly see how the joint starts to move from 90deg to 0deg but about 87deg it moves back up again. This was with the parameter set to 0 which should be FIFO. Stutter

Levaru commented 3 years ago

And this is the same movement but with the parameter set to 1. (It's also a little bit faster but that's not relevant). NoStutter

As you can see, there is no oscillation/reverse movement.