Example of realtime control interface

[BryanStuurman]

Could we get a short example of how to bring up and use the realtime control interface? I didn't see any documentation, and my own experimentation (with the virtual robot) isn't yielding any results.

[BryanStuurman]

I am having errors using the realtime control, the robot will throw an alarm on the driver console and with exception to a slow speedj command I cannot get motion.

In general, I do the following: rt connect, rt set output, and publish to either servoj_rt_stream or speedj_rt_stream at 100Hz. When publishing to speedj_rt_stream, if the time parameter is less than 1 second, the driver catches an alarm from the robot:

[ERROR] [1645640721.270025990]:  level : 3
[ERROR] [1645640721.270116117]:  group : 2
[ERROR] [1645640721.270167147]:  index : 1908
[ERROR] [1645640721.270207906]:  param : 6
[ERROR] [1645640721.270530584]:  param : desired position=   -nan[deg], limit=360.000[deg]
[ERROR] [1645640721.270595405]:  param : 

alarm index 1908 isn't detailed in my troubleshooting manual, but I'm sure it has to do with the -NaN angle applied to a joint. Being a speedj command I'm not clear as to where this angle comes from.

If I publish the same command but with the time parameter == 1 second, the joint actually moves for almost a second, then the robot applies the brakes and alarms a different alarm:

[ WARN] [1645640277.941111842]:  level : 2
[ WARN] [1645640277.941139835]:  group : 5
[ WARN] [1645640277.941169241]:  index : 7056
[ WARN] [1645640277.941192577]:  param :
[ WARN] [1645640277.941214045]:  param :
[ WARN] [1645640277.941237741]:  param :

7056 being a stop alarm, the robot is concerned that it is moving when it expects to be stopped.

Trying to stream data to servoj_rt_stream on the other hand yields the following with time==0.01s:

[ERROR] [1645641011.968111126]:  level : 3
[ERROR] [1645641011.968119205]:  group : 4
[ERROR] [1645641011.968125636]:  index : 1025
[ERROR] [1645641011.968131645]:  param : 6
[ERROR] [1645641011.968137672]:  param : 0x8611
[ERROR] [1645641011.968143688]:  param :

1025 is a position following error, which is interesting. I tried playing with the vel and acc parameters but couldn't change this result or encourage motion. and bumping the time parameter to 1s does not yield motion (unlike speedj) but a new alarm code:

[ WARN] [1645641136.037746125]:  level : 2
[ WARN] [1645641136.037870836]:  group : 2
[ WARN] [1645641136.037941981]:  index : 1215
[ WARN] [1645641136.038137778]:  param : Target limit velocity limit error, [6]joint, target limit velocity[225.000 deg/s], limit velocity[180.000 deg/s]
[ WARN] [1645641136.038269408]:  param :
[ WARN] [1645641136.038425394]:  param : 

1215 isnt in my manual either, but it's clear the robot has decided that a joint is required to go faster than possible. This doesn't change even if I specify lower velocities in the servoj call.

The actual code being used is on my github at: https://github.com/BryanStuurman/doosan-robot/blob/feature_rt_roscontrol/dsr_example/py/scripts/simple/realtime.py

[InigoMoreno]

@BryanStuurman I've seen that in your fork you attempt to implement ros_control for the Doosan. Is that working? I am also interested in using ros_control.

[BryanStuurman]

@InigoMoreno Unfortunately it is not. Originally I tried implementing the write loop with the regular servoj command and it was continually applying the brakes, servoj_rt and speedj_rt are giving me issues as described above.

One thing that does seem to be working is the state feedback - the data looks good and it comes at the correct rate.

Anyways, watch this space, I'm sure @doosan-robotics will get us up and running with the realtime interface and once that's taken care of my fork will have working roscontrol interfaces.

[BryanStuurman]

@InigoMoreno success today! I got realtime velocity control of the robot using doosans api example https://github.com/BryanStuurman/doosan_api_example (sorry about the shambles that repo is in) My python code now works to move the robot using their topics and services, in conjunction with my modified driver. I should have a proper roscontrol version running by Monday. Regrettably position control is pretty broken, I won't implement that. Have a look and listen at/to the video of the robot responding to servoj_rt commands: https://drive.google.com/file/d/1f3BM3Qq8TFiEaH6bo9KmR1khlGVdms-T/view?usp=sharing

To use this you'll need doosan firmware version 2.9, which if you haven't received yet you can get it from your vendor, or perhaps on doosan robot lab if you have access to the Partner/Service Data pane.

[BryanStuurman]

@InigoMoreno My fork has a working velocity controller now under the feature_rt_ros_control branch, the acc and vel limits passed to the rt interface are hardcoded for now, sorry bout that. You can test it out by bringing up your driver and then running rosrun dsr_example_py roscontrol_test.py - sorry this also has hardcoded paths, that will wiggle joint 5 a few degrees in velocity mode. Also be mindful that if you stop publishing to the velocity controller it will continue to drive the robot with the last commanded velocity.

My updated doosan driver may help with issue #92 and issue #101 as one can use the velocity_controllers/JointTrajectoryController instead of the doosan trajectory controller "service"

[InigoMoreno]

This looks promissing, I'll look into it once I get the firmware version 2.9.

[InigoMoreno]

@BryanStuurman It worked! I managed to run your example by setting controller:=velocity in dsr_control.launch and running roscontrol_test.py (changing the namespace of the topics) and the robot moved! A few questions:

1. It didn't work in simulation, only with the real robot. I guess the drcf is not updated to 2.9 in melodic.
2. I couldn't manage to use the velocity_controllers/JointTrajectoryController that you mentioned. How do you bring that up? I tried setting controller:=trajectory but I got the following error:

   [DRHWInterface::prepareSwitch]: [INTERFACE] [prepareSwitch] [START] POSITION INTERFACE REQUESTED, but it is not available due to servoj_rt implementation issues.
   [ControllerManager::switchController]: Could not switch controllers. The hardware interface combination for the requested controllers is unfeasible.

   I guess it's trying to use the position interface instead of the velocity one, but I don't know how to change that.

[BryanStuurman]

1

Indeed the doosan simulator does not simulate the UDP realtime interface, my fork requires a real robot, or you need to comment line 981 in dsr_hw_interface.cpp and uncomment line 982 to get it to run (though not actually work!!) in virtual mode. https://github.com/BryanStuurman/doosan-robot/blob/feature_rt_roscontrol/dsr_control/src/dsr_hw_interface.cpp#L981

2

So I hadn't actually added that, you would've had to create a new controller definition for the trajectory controller and load that. Fear not, Dmitri and I should have one of those put together this week and a more standard set of launch files. Doosan has implemented a "trajectory controller" by intercepting the trajectory message and streaming it to the robot with movesj() (line 1311). However they've also declared a position_controller/trajectory controller in dsr_control.yaml which tries to use roscontrol to move the robot - it's a mess and I didn't see it before, I've removed that controller so at least you wont get the error messages from it.

[BryanStuurman]

We've dug in and decided to create a bare bones version that resembles other robot drivers, so we made another branch https://github.com/BryanStuurman/doosan-robot/tree/ros_control_compat

you can get a velocity trajectory controller running with that branch of my repo by running the following commands

roslaunch dsr_ros_control load_robot.launch
roslaunch dsr_ros_control start_robot_interface_streaming.launch rosrun rqt_controller_manager rqt_controller_manager

then use the rqt controller manager gui to start the trajectory controller, or build another launch file to call the previous two and then use the controller_manager to spawn your controller of choice.

[baolin-hri]

@BryanStuurman It worked! I managed to run your example by setting controller:=velocity in dsr_control.launch and running roscontrol_test.py (changing the namespace of the topics) and the robot moved! A few questions:

1. It didn't work in simulation, only with the real robot. I guess the drcf is not updated to 2.9 in melodic.

2. I couldn't manage to use the `velocity_controllers/JointTrajectoryController` that you mentioned. How do you bring that up? I tried setting `controller:=trajectory` but I got the following error:

[DRHWInterface::prepareSwitch]: [INTERFACE] [prepareSwitch] [START] POSITION INTERFACE REQUESTED, but it is not available due to servoj_rt implementation issues.
[ControllerManager::switchController]: Could not switch controllers. The hardware interface combination for the requested controllers is unfeasible.

I guess it's trying to use the position interface instead of the velocity one, but I don't know how to change that.

Hi

We tried to do the same thing (launching the dsr_control.launch and running the roscontrol_test.py node) but the robot did not move, although no errors were present. When we check the /dsr01a0509/joint_states topic we got zeros for position, velocity and acceleration. Any suggestions on how to debug this?

Thank you

[BryanStuurman]

@liubl90 If you're using my fork, you should post an issue over there and I will be happy to help you debug it.

Issues can't be raised on forks, how delightful; we will do our work here.

Switch to roscontrol compat if need be, build, source and run the following commands:

roslaunch dsr_ros_control load_robot.launch   

roslaunch dsr_ros_control start_robot_interface_streaming.launch   

rosrun rqt_controller_manager rqt_controller_manager   

using rqt controller manager select the "dsr_joint_velocity_controller" and start it. then you should be able to run roscontrol_test.py

Launching the doosan launch files wont specify the velocity controllers, and tries to load the position controllers instead, which are currently broken, hence the error message you received - you can see the code here in prepareswitch: https://github.com/BryanStuurman/doosan-robot/blob/ros_control_compat/dsr_control/src/dsr_hw_interface.cpp#L1071

[baolin-hri]

@BryanStuurman I am trying to use the velocity controller under the feature_rt_ros_control branch roslaunch dsr_control dsr_control.launch (real robot, a0509, velocity controller) But i got the following errors: Neither the velocity nor the position controller can be found.

When i use the ros_control_compat branch i also met some errors

What am I doing wrong? And how do I fix this?

[BryanStuurman]

• switch to the ros_control_compat branch
• start three terminals and source the workspace in each
• in sequence run the following three commands, one per terminal:
• roslaunch dsr_ros_control load_robot.launch model:=<your robot model>
• roslaunch dsr_ros_control start_robot_interface_streaming.launch host:=<your robot ip> model:=<your robot model>
• rosrun rqt_controller_manager rqt_controller_manager

This will bring up the rqt controller manager interface, which can be used to select a controller: You can see in that image I have right clicked and started the joint velocity controller. Now the /dsr_joint_velocity_controller/command topic is available for use.

[baolin-hri]

@BryanStuurman Thanks for your reply. I followed your suggestions and launched three commands in sequence under the ros_control_compat branch

roslaunch dsr_ros_control load_robot.launch model:=a0509
roslaunch dsr_ros_control start_robot_interface_streaming.launch host:=192.168.137.160 model:=a0509 mode:=real After I ran those two commands, and I also got those errors

Two launch file: load_robot.txt

start_robot_interface_streaming.txt

The system is Ubuntu18.04, ros: Melodic

I don't know what are the problems. Looking froward to your reply.

[BryanStuurman]

@baolin-hri It looks like the last print you received from init() was "[dsr_hw_interface] host %s, port=%d bCommand: %d, mode: %s\n" (line 976), at which point the code hung trying to connect to your robot.

I have added two additional ROS INFO prints on line 984 and line 992: ROS_INFO("[dsr_hw_interface] Conventional control connected successfully!"); ROS_INFO("[dsr_hw_interface] REALTIME control connected successfully!"); https://github.com/BryanStuurman/doosan-robot/commit/c527ad0777d9471d077fad77e81243e789d14ebb Please pull and rebuild.

Those will appear after the connect commands complete. If they do not appear there is a communication problem:

• check your ip settings, can you ping the robot from this computer?
• check your robot firmware, do you have the latest version?

If this doesn't help, please note that I've been testing and developing on Ubuntu 20.04 and ROS Noetic. The DRFL driver uses separate binaries (closed source too!) for ROS Noetic, melodic and kinetic, and offers 32 bit support only for kinetic. I am not set up to do testing under any other environments - as such I cannot support the issue if it's a problem with melodic/kinetic. In addition, though probably not causing your problem specifically, I use colcon as my build tool instead of catkin.

[baolin-hri]

Thank you @BryanStuurman The problem was solved. (1) I updated the ubuntu18.04 to 20.04 (it should be work on ubuntu18.04) (2) I reset the ip address of teach pendant to from 192.168.137.160 to 192.168.137.100 Then it works.

[InigoMoreno]

If this is working correctly you could try and merge your changes?

[pucciland95]

May I ask what is the real time control loop speed (in Hz) for doosan controllers?

Cheers

[baolin-hri]

May I ask what is the real time control loop speed (in Hz) for doosan controllers?

Cheers

Hi Niccolo, My algorithms work at 40 Hz. I sent the joint velocity to the robot and it works well. The working frequency on the official documentation is much higher than this.

[BryanStuurman]

@pucciland95 The default rate in the driver is 100Hz on the realtime fork we're discussing here.

@InigoMoreno I'm happy to see you popping up here again! I do not think my fork is suitable for merging in it's current state, since the realtime control aspect is not optional. I've done no testing of the interactions between using realtime controls and using the other motion commands - they could fail spectacularly when mixed. I've also done some deletions of interfaces that didn't work well in my application. Finally it's been years since I started the fork and I suspect the repos have diverged to the point where a simple merge would cause unresolvable chaos. I think the best approach is cataloguing the important changes to implement realtime control and then patching them into a fresh fork that uses parameters to enable them optionally. Subsequently we must test the existing feature set to make sure there's no bad interactions, and disable those features which don't behave.