Introduce kuka lwr4+ with shadow combination

[nrontsis]

I plan to extend sr_multi_description, sr_robot_launch in order to include a kuka+shadow combination.

[nrontsis]

I understood that I don't have permission to create branches in this repository, I will fork, and push my changes there.

[guihomework]

We are really interested in knowing how advanced is this kuka+shadow combination ? is this already done ? We worked on that too.

[nrontsis]

There is a pretty complete ros package for kuka here:
https://github.com/CentroEPiaggio/kuka-lwr

I hope to have it completely integrated it with Shadow (gazebo, ros_control, etc) mainly for simulation, pretty much similarly to what was done for ur10 and shadow hand.

It is not already done, but I am working on it as a part of the RAMCIP project, where SHADOW is also involved.

At what point did you reach? What was your approach? Do you have any code available?

Cheers!

[toliver]

@nrontsis Yes, I think a fork would be the best way.

About our work with the UR10 just let me point out that we have been using a fork of universal_robot (https://github.com/shadow-robot/universal_robot) and we are using 2 different branches there:

• one for simulation in gazebo F_sr_plugin_compatible
• another one for the real robot F_trajectory_record_play_stl_collision

The main reason to create these branches was to replace the transmissions in the arm robot description, so that they are children of ros_ethercat_model::Transmission instead of children of the ros_control transmission_interface::TransmissionLoader.

Our real time loop and our sr_gazebo_plugin only work with ros_ethercat_model::Transmission for the moment. This is something that we want to address in the near future so that the shadow hand is fully compatible with ros_control.

[nrontsis]

@toliver @iSaran

I uploaded an initial attempt.

You should install the following (careful at the branches)

https://github.com/nrontsis/kuka-lwr/tree/adaptations_for_SHADOW
https://github.com/nrontsis/sr-ros-interface/tree/F%23333_kuka_arm

Then you can execute:
roslaunch sr_robot_launch right_srhand_lwrarm.launch
which, will run gazebo and show the combined model. However gazebo crashes, due to some auxiliary "dummy" links added in KUKA's model in order to allow impedance control (variable stiffness in the joints). See the following issue for a discussion about this:
CentroEPiaggio/kuka-lwr#25

However, just to see how far this can go without a lot of changes, one can do the following:
Comment out line 216 of sr_gazebo_plugins/src/gazebo_ros_controller_manager.cpp
(the line ROS_ASSERT(joints_.size() == fake_state_->joint_states_.size());)

Running again roslaunch sr_robot_launch right_srhand_lwrarm.launch will open gazebo, without any controller active, which will cause the robot to fall under the effect of gravity.

Therefore, to my point of view, three issues should be solved:

1. Deal with the dummy links (possibly in coordination with the guys who wrote kuka-lwr package)
2. Work towards having working controllers for the kuka-shadow set.
3. See what adaptations should be made for fully running kuka in gazebo 2.2 (see Support for versions of gazebo other than 4 CentroEPiaggio/kuka-lwr#29)

Edit: @toliver, earlier this week @ugocupcic mentioned me these issues (https://github.com/shadow-robot/ramcip/issues/24), which I tried to incorporate in my fork https://github.com/nrontsis/kuka-lwr/tree/adaptations_for_SHADOW . Feel free to check them and comment if something doesn't seem correct to you :)

[guihomework]

Hi,

@nrontsis we have seen this kuka package too (and might migrate to it, are you developing that or just using ?), but we currently rely only on a kuka URDF and some controllers from https://github.com/RCPRG-ros-pkg because we have orocos control for the kukas.

The fact that shadow hand do not run run with a simulated ros_control (see previous message from @toliver) yet was a reason we did not switch to kuka with ros_control.

So our solution for arm+hands is not on ros_control integration but cleaner and generic robot_description generation (and soon moveit generation). We wanted to avoid repeating the same robot_description for different setups everytime, and use templates and wrappers.

This cleaner solution heavily relies on new features of xacro (handling python evaluation), which is released in jade-devel but perfectly compiles and runs on indigo-devel.

If you and shadow are interested we will create and release a sample package deploying robot_description for an arm and hand robot. Then sr_multi_description could migrate to this new way of deploying combined arm and tools.

[toliver]

@guihomework We are indeed interested in knowing more about your generic robot_description and moveit generation.

Having a moveit configuration per robot combination is certainly not an easily maintainable solution.

[nrontsis]

@guihomework No I am not developing it, I am just using it.
I agree with @toliver who has more experience with sr_multi_description.

[toliver]

@nrontsis , @iSaran and I might try to run your kuka+shadow in gazebo tomorrow.

About the controllers, have you tried to define and launch a trajectory controller like the one in https://github.com/shadow-robot/sr-ros-interface/blob/indigo-devel/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller.yaml

That is what we use to control the simulated UR10.

[nrontsis]

I have a new version:
https://github.com/nrontsis/kuka-lwr/commit/9432529df0f4bce48cd2373d99f3b8a3e3dfb069
https://github.com/nrontsis/sr-ros-interface/commit/419e49f63a51e58f59daeff552cf6ac042eeb377

It launches the controllers, and removes the stiffness joints and dummy links that caused problems in the sr_gazebo_plugins.

Running roslaunch sr_robot_launch right_srhand_lwrarm.launch shows the robot stabilizing in the initial configuration under the effect of gravity.

[guihomework]

I am working on a manipulators_description package that will be uploaded soon on our github. Stay tuned

[nrontsis]

@ugocupcic
Following the discussion we had in CentroEPiaggio/kuka-lwr#29

I currently avoid the problem by removing the stiffness joints. This, to my understanding, can work fine as a temporary solution, but only in simulation.

However, it would be really nice to be able to use the original kuka-lwr package, in order to control the hardware with the same ros package. But this is not urgent for us.

So currently I will wait for SHADOW to get rid of sr_gazebo_plugins, and if I need something sooner, I could look further into the solution @ugocupcic proposed (spawn the 2 robot models separately).

[carlosjoserg]

Spawning 2 robots, i.e. the arm, and then the hand on top of it, was almost impossible and not intuitive to me. So let me explain my solution (it does not avoid the joint state merging though).

IMO, the best thing to do is to work with the gazebo_ros_control plugin and use the default robot interface as a template to write your own to replace it. That's what I did initially for both hand and arm. But then, I found a problem with the plugin that I described in ros-simulation/gazebo_ros_pkgs#297, similar to the issue you are having now I believe.

So, I took the plugin code and added these lines (you can see it as well for the Pisa/IIT SoftHand here). This change required a small modification of the transmission_info struct to filter out joints that did not belong to the robot containing the plugin. This change is in the Centro Piaggio's fork of ros_control here. This way, arm and hand are independent robots that can be spawned individually, or spawned as an assembled robot (see Vito robot), without modifying any file, only the one that defines the assembly.

Another approach is to use a parameter to filter joints, as in the COB's plugins. At first I didn't like it, but actually, this is not a bad idea if you have joints that do not have a corresponding transmission specification, and it does not requires the change in the transmisison_interface package. It will require a modification of the gazebo_ros_control plugin as well, see here, so I will consider something like that for the next update in the kuka package.

Drawback of both (for now) is that you have a controller manager per robot, so commands are not synced (this happens with the real robot as well, but for the freqs. I'm using this is not a problem). You also get joint states for each robot that you need to merge for visualization, moveit, etc.. The sync issue can be solved either by writing one single HW interface for the composed robot, not easy to develop and maintain, or ideally, compose the HW interfaces (in real and sim). The latter is being discussed in multiple places, you can see one in ros-controls/ros_control#75

---

SO, apologies for the long post, final comment.

For now, I think that having an sr_gazebo_plugins is not a problem at all, on the contrary, it will be necessary to decouple stuff. For instance, the issue you are having with stiffness joints in the arm can be solved if you make the sr plugin to handle only the hand joints, as I did with the SoftHand, so you can exploit the kuka package at its best without modifications. I also wrote a custom transmission for the adaptive synergy mode in the hand, so I would recommend to port also sr transmissions to the transmissions_interface format as well.

[ugocupcic]

@carlosjoserg thanks for the pointers. Upgrading our transmissions to the transmission interface is in our TODO list. We just haven't had time to start on this yet. Hoping to start on it this week.

[guihomework]

I woul like to add some comments to that topic too as we also use a kuka + shadow hand here. @ugocupcic you think (from the CentroEPiaggio/kuka-lwr#29) that merging the joint states is a nightmare. Commanding is also an issue, when joints you want to control for a group span across two robots, one of which is not on ros_control at least.
To my mind the only cleanest way to get rid of that issue is indeed to redevelop a HW interface that is filled by two hardware driver at the same time as said by @carlosjoserg . This could go for simulation but in the real world, you might have completely different systems and it might be different for each robot you want to associate. We were thinking on getting the shadow hands on orocos, with the orocos SOEM ethercat component and so on, to be able to do the fusion of the control loop we had for the kuka robots. Doing the fusion in the ros_ethercat loop was a no go due to the loop not running in realtime yet (might be realtime safe with the roscontrol code).
We ended up with the not-so-clean solution of merging the joint_states and separating the control between the hand and the arms our of ros_control.
To do that, no modification of any code except urdf/xacro, just using the plugins and/or drivers in a separate namespace so that they load their own robot_description without the other robot's joints. Hence no problems of transmission incompatibilities.
This seems like the best solution so far, since we can combine different robots together and due to xacro can instantiate each single robot in their namespace, and the composed robot for planning and visualization. Planning quite easily be executed on separate controller managers.

Of course cleaning up the transmission format at the shadow side is a nice solution but might not be sufficient for other robot arms using a non-ros-control-compatible interface. Should we impose ros_control compatibility everywhere (is this still in beta ?) or find a way to make things cooperate without changing everything to that standard ?