Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Questions: does abb_libegm allows for joint velocity control? #15

Closed
ywen27G opened this issue Aug 4, 2018 · 14 comments
Closed

Questions: does abb_libegm allows for joint velocity control? #15

ywen27G opened this issue Aug 4, 2018 · 14 comments

Comments

@ywen27G
Copy link

ywen27G commented Aug 4, 2018

Hi All,

I know this perhaps is not the best place to ask questions. However, we are on a tight schedule and have to make a decision recently to purchase a large robot for force control.

I wonder if abb_libegm allows for the control of joint_velocity in real-time? If so, could you please briefly tell me its real-time performance of joint velocity tracking and its difference from abb_librws?

Thank you a lot for your good contribution for me to ask this question here. Really appreciate your time and help.

@jontje
Copy link
Contributor

jontje commented Aug 7, 2018

Hi @mitch1044444,

Yes, abb_libegm allows for joint velocity control. Concerning the real-time aspect it will depend on your requirements.

The highest frequency EGM can run at the moment is 250 Hz, so every 4 ms the robot controller can accept new control references. However, there is also a control lag of 10-20 ms depending on the robot type and how EGM has been configured (the numbers are from the official EGM documentation).

Concerning the main difference between abb_libegm and abb_librws:

  • abb_libegm: Interface to Externally Guided Motion (EGM), which enables streaming of motion references. EGM is purely for motion control.
  • abb_librws: Interface to Robot Web Services (RWS), which grants access to a number of services and resources in the robot controller. These are interacted with via HTTP requests, and it is not designed for real-time. RWS can be used for motion control, but then it would be more like download position target(s) to the robot controller and then move.

I hope this helps!

@ywen27G
Copy link
Author

ywen27G commented Aug 17, 2018

Thank you so much for your helpful information @jontje . Just want to check if my understanding is correct.

  1. If we have a ABB robot with IRC5 controller, then we install the ABB driver here, abb_libegm and Robotware. I read from ABB websites

    RobotStudio includes a matching version of RobotWare. Previous versions can be downloaded from within RobotStudio.

    so we can find a Robotware with the version between 6.0 and 6.06.01. That should allow us to communicate with the robot right?

  2. I assume we will need to send EGM commands as String to the abb_libegm library. Also, I wonder if you happen to know where I may find some good examples using EGM? From my own experience, those industry packages usually are not very user-friendly to mechanical engineers... Just want to check if you happen to know them.

You already have helped me a lot. Thank you so much.

@jontje
Copy link
Contributor

jontje commented Aug 21, 2018

No problem, and here are a few clarifications.

ABB Robot Requirements

For EGM to work with the robot, then the following conditions needs to be fulfilled:

  • A system with RobotWare 6.0 or later. The recommended version for abb_libegm is currently 6.06.01 since that is what abb_libegm has been verified against.
  • RobotWare versions can be downloaded via RobotStudio.
    In RobotStudio -> Add-Ins tab -> Find RobotWare -> Choose RobotWare version -> Download
    The downloaded RobotWare version(s) will then show up when using the Installation Manager, in RobotStudio, to install a system on the robot controller.
  • A license for the RobotWare option Externally Guided Motion (689-1).
    Note: This is required to be purchased from ABB Robotics, and the license is used during installation of a robot controller system.

Runtime Requirements

If a robot controller system has been installed with the correct RobotWare version, and has the EGM option, then the following needs to be fulfilled as well.

Robot Controller Side

  • Configuration of the EGM client (in the robot controller) settings. E.g. IP-address and port number to the external EGM server.
  • A RAPID program that executes on the robot controller system. The RAPID program need to initialize the EGM communication, and then activate an EGM communication session (to tell the robot to listen for external motion references).

External Computer Side (e.g. with ROS)

  • The ABB driver you linked to will unfortunately not work together with EGM. That driver is using another approach based on RAPID sockets and downloading trajectories to the robot controller, instead of streaming motion references.
  • You can test the joint velocity feature of abb_libegm package (in a simple ROS node with a dependency on abb_libegm) with something like this:
// Boost components (for managing asynchronous UDP sockets).
boost::asio::io_service io_service;
boost::thread_group worker_threads;

// EGM interface components (for setting up and providing APIs to a EGM server).
abb::egm::BaseConfiguration configuration;
configuration.use_velocity_outputs = true;
abb::egm::EGMControllerInterface egm_interface(io_service, 6511 /* Port number (needs to match the robot controller EGM settings) */, configuration);
worker_threads.create_thread(boost::bind(&boost::asio::io_service::run, &io_service));

// Simple joint velocity control loop.
abb::egm::wrapper::Input input;
abb::egm::wrapper::Output output;
while (ros::ok())
{
  if (egm_interface.waitForMessage(500))
  {
    egm_interface.read(&input);

    // Calculate, and set references. For example:
    output.Clear();
    output.mutable_robot()->mutable_joints()->mutable_velocity()->add_values(1.0);
    // Etc.

    egm_interface.write(output);
  }
}

I haven't written down all the details, but this should give you an idea of the requirements.

@ywen27G
Copy link
Author

ywen27G commented Aug 21, 2018

Thank you so much. Really appreciate your help.

@Mitch981521

This comment has been minimized.

@gavanderhoorn

This comment has been minimized.

@ChunyangXia
Copy link

I haven't written down all the details, but this should give you an idea of the requirements.

Hi, Do you know how to set up the instructions in RAPID to receive the change of velocity from C++.
Please help me .Thank you very much!

@ChunyangXia
Copy link

  • A RAPID program that executes on the robot controller system. The RAPID program need to initialize the EGM communication, and then activate an EGM communication session (to tell the robot to listen for external motion references).

@jontje Can you share your PARID code that set up the EMG for changing velocity realtime. This will very helpful. Hope to get your help

@jontje
Copy link
Contributor

jontje commented Jun 3, 2019

Hello @ChunyangXia,

You should be able to use the RobotWare StateMachine Add-In.

You can install it via RobotStudio --> Add-Ins tab --> search for StateMachine --> press Add.
Then you can create a new RobotWare system that uses the Add-In. If you do, then the new system will include RAPID code and system configurations.

If you only want the RAPID code, then you can create a dummy virtual system according to the example in the User Manual and extract the code from there.

After you have the RAPID code, then you need to make sure that the RAPID EGMRunX instructions's PosCorrGain argument is set to zero (to allow for pure velocity control).

@ChunyangXia
Copy link

@jontje Hi Jontje,
Thanks for your help. Is there any example program or demo for using the "abb_libegm" library. I am not very clear how to use it. Thanks again

@gavanderhoorn
Copy link
Member

Please refer to #18.

@ChunyangXia
Copy link

After you have the RAPID code, then you need to make sure that the RAPID EGMRunX instructions's PosCorrGain argument is set to zero (to allow for pure velocity control).

@jontje
Thanks for your help. After setting PosCorrGain=0, the velocity can be controled realtime, but the movement lose control, robot will go along X direction until safe guard stop. Do you know why? How to regulate the path of movement under this situation. The following is my RAPID. Thanks again

MODULE EGM_test_UDP

VAR egmident egmID1;
VAR egmstate egmSt1;

CONST egm_minmax egm_minmax_lin1:=[-1,1]; !in mm
CONST egm_minmax egm_minmax_rot1:=[-2,2];! in degees
CONST robtarget p20:=[[34,-499,123],[0.000494947,0.662278052,-0.749217059,-0.007831731],[0,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];

PERS tooldata UISpenholder:=[TRUE,[[0,0,114.25],[1,0,0,0]],[1,[-0.095984607,0.082520613,38.69176324],[1,0,0,0],0,0,0]];
TASK PERS wobjdata wobjBordN:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[150,-500,8],[0.707106781,0,0,-0.707106781]]];

VAR pose posecorTable:=[[150,-500,8],[1,0,0,0]];
VAR pose posesenTable:=[[0,0,0],[1,0,0,0]];

PROC main()
    ! Move to start position. Fine point is demanded.
    MoveJ p20,v100,fine,UISpenholder;
    testuc_UDP; 
ENDPROC


PROC testuc_UDP()
    EGMReset egmID1;
    EGMGetId egmID1;
    egmSt1 := EGMGetState(egmID1);
    TPWrite "EGM state: "\Num := egmSt1;
    
    IF egmSt1 <= EGM_STATE_CONNECTED THEN
        ! Set up the EGM data source: UdpUc server using device "EGMsensor:"and configuration "default"
        EGMSetupUC ROB_1, egmID1, "default", "EGMSensor:" \pose;
    ENDIF
    
    !Which program to run
    runEGM;
    
    
    IF egmSt1 = EGM_STATE_CONNECTED THEN
        TPWrite "Reset EGM instance egmID1";
        EGMReset egmID1; 
    ENDIF  
ENDPROC
    
    
    PROC runEGM()
        EGMActPose egmID1\Tool:=UISpenholder \WObj:=wobj0, posecorTable,EGM_FRAME_WOBJ, posesenTable, EGM_FRAME_WOBJ 
        \x:=egm_minmax_lin1 \y:=egm_minmax_lin1 \z:=egm_minmax_lin1
        \rx:=egm_minmax_rot1 \ry:=egm_minmax_rot1 \rz:=egm_minmax_rot1\LpFilter:=2\Samplerate:=4\MaxSpeedDeviation:= 1000;
            
        EGMRunPose egmID1, EGM_STOP_RAMP_DOWN\x\y\z\CondTime:=20 \RampInTime:=0.05\RampOutTime:=0.5\PosCorrGain:=0;
        egmSt1:=EGMGetState(egmID1);
    ENDPROC

ENDMODULE

@gavanderhoorn
Copy link
Member

@ChunyangXia: let's not keep commenting on an already closed issue.

Please open a new one.

@Steven-ZY
Copy link

After you have the RAPID code, then you need to make sure that the RAPID EGMRunX instructions's PosCorrGain argument is set to zero (to allow for pure velocity control).

@jontje Thanks for your help. After setting PosCorrGain=0, the velocity can be controled realtime, but the movement lose control, robot will go along X direction until safe guard stop. Do you know why? How to regulate the path of movement under this situation. The following is my RAPID. Thanks again

MODULE EGM_test_UDP

VAR egmident egmID1;
VAR egmstate egmSt1;

CONST egm_minmax egm_minmax_lin1:=[-1,1]; !in mm
CONST egm_minmax egm_minmax_rot1:=[-2,2];! in degees
CONST robtarget p20:=[[34,-499,123],[0.000494947,0.662278052,-0.749217059,-0.007831731],[0,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];

PERS tooldata UISpenholder:=[TRUE,[[0,0,114.25],[1,0,0,0]],[1,[-0.095984607,0.082520613,38.69176324],[1,0,0,0],0,0,0]];
TASK PERS wobjdata wobjBordN:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[150,-500,8],[0.707106781,0,0,-0.707106781]]];

VAR pose posecorTable:=[[150,-500,8],[1,0,0,0]];
VAR pose posesenTable:=[[0,0,0],[1,0,0,0]];

PROC main()
    ! Move to start position. Fine point is demanded.
    MoveJ p20,v100,fine,UISpenholder;
    testuc_UDP; 
ENDPROC


PROC testuc_UDP()
    EGMReset egmID1;
    EGMGetId egmID1;
    egmSt1 := EGMGetState(egmID1);
    TPWrite "EGM state: "\Num := egmSt1;
    
    IF egmSt1 <= EGM_STATE_CONNECTED THEN
        ! Set up the EGM data source: UdpUc server using device "EGMsensor:"and configuration "default"
        EGMSetupUC ROB_1, egmID1, "default", "EGMSensor:" \pose;
    ENDIF
    
    !Which program to run
    runEGM;
    
    
    IF egmSt1 = EGM_STATE_CONNECTED THEN
        TPWrite "Reset EGM instance egmID1";
        EGMReset egmID1; 
    ENDIF  
ENDPROC
    
    
    PROC runEGM()
        EGMActPose egmID1\Tool:=UISpenholder \WObj:=wobj0, posecorTable,EGM_FRAME_WOBJ, posesenTable, EGM_FRAME_WOBJ 
        \x:=egm_minmax_lin1 \y:=egm_minmax_lin1 \z:=egm_minmax_lin1
        \rx:=egm_minmax_rot1 \ry:=egm_minmax_rot1 \rz:=egm_minmax_rot1\LpFilter:=2\Samplerate:=4\MaxSpeedDeviation:= 1000;
            
        EGMRunPose egmID1, EGM_STOP_RAMP_DOWN\x\y\z\CondTime:=20 \RampInTime:=0.05\RampOutTime:=0.5\PosCorrGain:=0;
        egmSt1:=EGMGetState(egmID1);
    ENDPROC

ENDMODULE

@ChunyangXia Hi, ChunyangXia. Have you already realized real-time velocity control? May I ask you some questions?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Development

No branches or pull requests

6 participants