Joint Velocity Control doesn't track Constant Velocity

[danielj195]

We are wanting to use EGM to perform velocity control to move our robot to a designated position. The problem we are running into is that the robot does not maintain the desired velocity for the entire duration of the movement. If you change the reference velocity in the code @jontje provided to a constant (code provided below), the robot will move for only a few seconds and then stop, rather than run at that velocity for the desired amount of time. We are thinking that this may be due to a sudden change in velocity or some safety feature? Here are the steps we've taken to try to remedy the issue:

-Use the EGM_STOP_HOLD parameter in the EGMRunJoint command -Tried running the robot at small velocities, to see if it was due to high acceleration

• Set the MaxSpeedDeviation to a super high number to allow for large changes in speed

All of these produced the same result: The robot would move for only a couple of seconds and then stop.

On a side note, does anyone know if abb_libegm is compatible with ROS-melodic/ubuntu 18.04?

We'd greatly appreciate any help with this issue!

[gavanderhoorn]

@danielj195: just as on ROS Answers, I'd like to ask you to not post screenshots of code, but instead copy-paste it into the issue itself. Be sure to use three backticks to format everything properly.

Thanks.

[Mitch981521]

My apologies...here is the code corresponding to my question.

EGM Code:

#include <ros/ros.h>
#include <abb_libegm/egm_controller_interface.h>

int main(int argc, char** argv)
{
  //----------------------------------------------------------
  // Preparations
  //----------------------------------------------------------
  // Initialize the node.
  ros::init(argc, argv, "joint_velocity_controller_node");
  ros::NodeHandle node_handle;

  // Boost components for managing asynchronous UDP socket(s).
  boost::asio::io_service io_service;
  boost::thread_group thread_group;

  // Create EGM configurations.
  abb::egm::BaseConfiguration configuration;
  configuration.use_velocity_outputs = true;

  // Create an EGM interface:
  // * Sets up an EGM server (that the robot controller's EGM client can connect to).
  // * Provides APIs to the user (for setting motion references, that are sent in reply to the EGM client's request).
  //
  // Note: It is important to set the correct port number here,
  //       as well as configuring the settings for the EGM client in thre robot controller.
  //       If using the included RobotStudio Pack&Go file, then port 6511 = ROB_1, 6512 = ROB_2, etc.
  abb::egm::EGMControllerInterface egm_interface(io_service, 6513, configuration);

  if(!egm_interface.isInitialized())
  {
    ROS_ERROR("EGM interface failed to initialize (e.g. due to port already bound)");
    return 0;
  }

  // Spin up a thread to run the io_service.
  thread_group.create_thread(boost::bind(&boost::asio::io_service::run, &io_service));

  //----------------------------------------------------------
  // Execute a joint velocity controller loop.
  //
  // Note 1: The EGM communication session is started by the
  //         EGMRunJoint RAPID instruction.
  //
  // Note 2: To get pure velocity control, then the EGM client
  //         (in the robot controller) need its position
  //         correction gain to be set to 0. This is done with
  //         the EGMRunJoint RAPID instruction.
  //----------------------------------------------------------
  ROS_INFO("========== Joint velocity controller (open-loop) sample ==========");
  bool wait = true;
  abb::egm::wrapper::Input input;
  abb::egm::wrapper::Joints initial_velocity;
  const int egm_rate = 250.0; // [Hz] (EGM communication rate, specified by the EGMActJoint RAPID instruction).
  int sequence_number = 0;    // [-] (sequence number of a received EGM message).
  double time = 0.0;          // [seconds] (elapsed time during an EGM communication session).

  abb::egm::wrapper::Output output;
  double reference = 0.0;  // [degrees/s].
  double amplitude = 10.0; // [degrees/s].
  double frequency = 0.25; // [Hz].

  ROS_INFO("1: Wait for an EGM communication session to start...");
  while(ros::ok() && wait)
  {
    if(egm_interface.isConnected())
    {
      if(egm_interface.getStatus().rapid_execution_state() == abb::egm::wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED)
      {
        ROS_WARN("RAPID execution state is UNDEFINED (might happen first time after controller start/restart). Try to restart the RAPID program.");
      }
      else
      {
        wait = egm_interface.getStatus().rapid_execution_state() != abb::egm::wrapper::Status_RAPIDExecutionState_RAPID_RUNNING;
      }
    }

    ros::Duration(0.5).sleep();
  }

  while(ros::ok())
  {
    // Wait for a new EGM message from the EGM client (with a timeout of 500 ms).
    if(egm_interface.waitForMessage(500))
    {
      // Read the message received from the EGM client.
      egm_interface.read(&input);
      sequence_number = input.header().sequence_number();

      if(sequence_number == 0)
      {
        // Reset all references, if it is the first message.
        output.Clear();
        initial_velocity.CopyFrom(input.feedback().robot().joints().velocity());
        output.mutable_robot()->mutable_joints()->mutable_velocity()->CopyFrom(initial_velocity);
      }
      else
      {
        time = sequence_number/((double) egm_rate);

        // Compute joint velocity reference.
        reference = 5.0 //CHANGED TO CONSTANT VELOCITY 

        if(output.mutable_robot()->mutable_joints()->mutable_velocity()->values_size() > 3)
        {
          // Set references for joint 1, 2 and 3.
          output.mutable_robot()->mutable_joints()->mutable_velocity()->set_values(0, reference);
          output.mutable_robot()->mutable_joints()->mutable_velocity()->set_values(1, reference);
          output.mutable_robot()->mutable_joints()->mutable_velocity()->set_values(2, reference);
        }

        if(sequence_number%egm_rate == 0)
        {
          ROS_INFO_STREAM("References: Joint 1-3 = " << reference << " [degrees/s]");
        }
      }

      // Write references back to the EGM client.
      egm_interface.write(output);
    }
  }

  // Perform a clean shutdown.
  io_service.stop();
  thread_group.join_all();

  return 0;
}

RobotStudio Code:

MODULE test_egm
    !======================================================================================================
! Copyright (c) 2018, ABB Schweiz AG
! All rights reserved.
!
! Redistribution and use in source and binary forms, with
! or without modification, are permitted provided that
! the following conditions are met:
!
!    * Redistributions of source code must retain the
!      above copyright notice, this list of conditions
!      and the following disclaimer.
!    * Redistributions in binary form must reproduce the
!      above copyright notice, this list of conditions
!      and the following disclaimer in the documentation
!      and/or other materials provided with the
!      distribution.
!    * Neither the name of ABB nor the names of its
!      contributors may be used to endorse or promote
!      products derived from this software without
!      specific prior wrSitten permission.
!
! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
! AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
! IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
! ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
! LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
! DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
! SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
! CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
! OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
! THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
!======================================================================================================

    !***********************************************************
    ! Program data
    !***********************************************************
    ! Home position.
    LOCAL CONST jointtarget home := [[0, 0, 0, 0, 30, 0], [9E9, 9E9, 9E9, 9E9, 9E9, 9E9]];

    ! Identifier for the EGM correction.
    LOCAL VAR egmident egm_id;

    ! Limits for convergance.
    LOCAL VAR egm_minmax egm_condition := [-0.1, 0.1];

    ! EGM pose frames.
    LOCAL CONST pose egm_correction_frame := [[0, 0, 0], [1, 0, 0, 0]];
    LOCAL CONST pose egm_sensor_frame     := [[0, 0, 0], [1, 0, 0, 0]];

    ! The work object (here set to coincidence with ROB_2's base frame).
    LOCAL PERS wobjdata egm_wobj := [FALSE, TRUE, "", [[0, 1000, 0],[1, 0, 0, 0]], [[0, 0, 0],[1, 0, 0, 0]]];

    !***********************************************************
    !
    ! Procedure main
    !
    !   This RAPID code exemplify how to run EGM joint position
    !   motions.
    !
    !   Note: Update the UCDevice "ROB_1" with correct
    !         values for the remote address and port
    !         (i.e. to the EGM server).
    !
    !         Update via RobotStudio:
    !         Controller tab -> Configuration ->
    !         Communication -> Transmission Protocol
    !
    !***********************************************************
    PROC main()
        Joint;
    ENDPROC

    PROC Joint()
            WHILE TRUE DO
            MoveAbsJ home, v200, fine, tool0;

            ! Register an EGM id.
            EGMGetId egm_id;

            ! Setup the EGM communication.
            EGMSetupUC ROB_1, egm_id, "default", "ROB_1", \Joint, \CommTimeout:=10.0;

            ! Prepare for an EGM communication session.
            EGMActJoint egm_id
                        \J1:=egm_condition
                        \J2:=egm_condition
                        \J3:=egm_condition
                        \J4:=egm_condition
                        \J5:=egm_condition
                        \J6:=egm_condition
                        \SampleRate:=4
                        \MaxSpeedDeviation:=2000.0;

            ! Start the EGM communication session.
            EGMRunJoint egm_id, EGM_STOP_HOLD, \J1 \J2 \J3 \J4 \J5 \J6;

            ! Release the EGM id.
            EGMReset egm_id;

            WaitTime 5;
        ENDWHILE

    ERROR
        IF ERRNO = ERR_UDPUC_COMM THEN
            TPWrite "Communication timedout";
            TRYNEXT;
        ENDIF
    ENDPROC
ENDMODULE

[jontje]

We are wanting to use EGM to perform velocity control to move our robot to a designated position. The problem we are running into is that the robot does not maintain the desired velocity for the entire duration of the movement. If you change the reference velocity in the code @jontje provided to a constant (code provided below), the robot will move for only a few seconds and then stop, rather than run at that velocity for the desired amount of time. We are thinking that this may be due to a sudden change in velocity or some safety feature?

To be able to use pure velocity control, then you need to set the EGM client's position correction gain to zero. You do this by adding the \PosCorrGain:=0 parameter to the RAPID EGMRunJoint instruction. Otherwise the EGM controller (inside the robot controller) could try and follow conflicting goals.

• Set the MaxSpeedDeviation to a super high number to allow for large changes in speed

I wouldn't use a number larger than the lowest max joint speed specified in the robot's data sheet.

On a side note, does anyone know if abb_libegm is compatible with ROS-melodic/ubuntu 18.04?

I haven't had any problem to use abb_libegm in ROS Melodic.

[Mitch981521]

Thank you very much! I'm glad it was just having to change a parameter. Good to hear that it works for ROS melodic.

[gavanderhoorn]

I'm going to close this, as it seems the (initial) question was answered and no further discussion has taken place.

Feel free to keep commenting on this issue of course if that makes sense.