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 ?

[carlosjoserg]

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 ?

If you haven't read it, I think you will find this discussion interesting: https://groups.google.com/forum/#!topic/ros-sig-robot-control/qUAnF6enTUo

In my case, the decision was clear before reading that (and after trying orocos to move the arm with existing open-source projects), but that confirmed that I made a good decision for my use cases.

ros_control has not reached v1.0 yet, but it is active and very well supported, and despite some naming conventions due to inheritance from pr2 stuff that can be misleading, it is simple to understand... I need no more.

[guihomework]

@carlosjoserg thanks for the link to that discussion. I think from the comparison done there, it all depends the use case indeed. We need to do complex controller setup so orocos was the way to go and Shadow not being fully standard ros_control capable (gazebo plugin, and some transmission issues) led us to use two controller managers in two namespaces

To come back to the question how to combine an arm with the shadow hand. There are currently good motivation to fix the shadow side, at least two arms (kuka and ur10) are using ros_control and proved to be incompatible if run in the same namespace.
I have two concerns

1. Shadow plugin/driver would need to distinguish between their specific transmission interface and the one from third parties, but the opposite might be true also. Third parties would need to not crash when loading shadow interfaces (ur10 crashes if it seems shadow interfaces). This looks a strong request for everybody to handle the modification of the transmission of third parties.
2. WhatI I don't understand with modifying the transmission interface (you introduced the namespace in the transmission if I understood well) to select if they concern arm or not is : Why in the first place does the plugin/controller need to read the full robot_description (with hand transmission) ?, Why would a partial robot_description (in the namespace) not be a good solution too as you will have to merge joint states afterwards anyway and have separate controller managers ?
   we spawn with robot_description arm+hand, and instantiate hand/robot_description and arm/robot_description, and pass that to respective plugins when spawning arm+hand.

[carlosjoserg]

Shadow plugin/driver would need to distinguish between their specific transmission interface and the one from third parties, but the opposite might be true also.

Yes. If you manage to pass to the Shadow plugin only Shadow transmissions, and the third-party plugins focus only on third-party transmissions, the problem would be solved. The issue comes, I think, when both Shadow and third-party use the same tag name <transmission>, that makes parsing the full robot description difficult.

Perhaps the solution is in the answer to the last question in this comment.

ur10 crashes if it seems shadow interfaces

I'm unaware of the ur10 plugin implementation, but very likely that the package uses the default gazebo_ros_control interface, which in turn uses transmissions to generate the handles, and reads from robot_description by default. If you have transmission tags with sr types in there, sure, it will crash.

This looks a strong request for everybody to handle the modification of the transmission of third parties.

I believe so, but I don't see any problem, you can wrap your transmission implementation with the ros_controls/transmission_interface if you want both to coexist, and select one or the other with xacro arguments.

Why in the first place does the plugin/controller need to read the full robot_description (with hand transmission) ?

I couldn't spawn the hand on top of the arm and keep them attached while the arm moved, can you? Besides, if you want to compensate gravity, the arm must know what's on top of it as well (unless you have adaptive controllers). Having a full robot_description is also useful for MoveIt!, and plan dual arm motions.

we spawn with robot_description arm+hand, and instantiate hand/robot_description and arm/robot_description, and pass that to respective plugins when spawning arm+hand.

How do you generate the hand/robot_description and arm/robot_description individually?

[guihomework]

Dear @carlosjoserg, thanks for your comments.
Regarding your answer to 2) and your question how we proceed with an arm (a kuka with an gazebo plugin not using ros_control) and the hand.
Indeed, we spawn an arm+hand in gazebo to ensure fixed arm hand mount, using a robot_description for the full arm+hand. It is generated using the new xacro (in jade) features, having substitution args like : arm:=kuka tool:=shadow_motor and some calibration file telling how is the shadow_motor attached to an end-effector. In this arm+hand robot_description are the two different plugins, but we pass the namespace (arm and hand respectively) both plugin permit that currently (I did not verify gazebo_ros_control permits that too).
The same xacro is then additionally used twice more to generated /arm/robot_description this time leaving the tool arg empty, and on /hand/robot_description leaving the arm arg empty.
In total we have two namespaced single robot robot_description and one full robot_description used for spawing.
This is nicelly packaged in sr_upload, kuka_upload and arm_and_hand_upload launch files and depending if we start the arm or the hand or both we load one or two or three robot_descriptions.

This is actually the only solution we found without modifying code. We would be happy to use a simpler solution. Your idea of parsing the full description is simpler in the robot_description to load but requires maybe more care on the plugins and at some point my break again on a fancy transmission or so.
I wished everybody develop their transmission with the wrapper solution you suggest, and that nothing breaks. So far I think there were problems on this side.

@nrontsis I will add a second robot to the example manipulators_description package to cover more case, and will upload that very soon. Probably also adding example launch files to show how we proceeded.

[nrontsis]

Sorry for the delay, you went a little too far for me, and I postponed my reply until I had time to read your comments thoroughly.

@carlosjoserg Thank you for the detailed comments. From my understanding, mounting shadow to kuka could work without the modification (removing stiffness joints, removing gazebo plugin, and changing transmissions) I did to kuka-lwr. Therefore, I will close this PR.

However, as I am currently rushing to catch a deadline that includes some simulations with the kuka+shadow combination, I will continue working on my solution for the next week, and then I will possibly come back to ask you problems that concern the more proper way to integrate them.

@guihomework Any example would be helpful. Currently I am struggling to understand your comments, (both yours @guihomework and @carlosjoserg) so having an example would definitely help me understand your point of view.

[carlosjoserg]

@nrontsis

The thing is that you modified the kuka description to use the SR transmission types so the SR plugin didn't crash, but it should be the opposite way, specially if SR is already working on porting thier transmissions to the ros control transmission package in #307. I don't know how the SR plugin is implemented but probably is doing similar to the gazebo_ros_control plugin, that is, it is parsing all transmissions/joints from robot_description and passing them to the hardware interface, and this one does not know what to do with it due to different types.

IMHO, a good solution should look in the direction of composing hardware instead of modifying. So...

What @guihomework suggests is to have a full robot description for moveit, rviz, gazebo spawn, etc. and then separate robot descriptions for each sub-robot, in this case kuka arm and shadow hand so the plugins focus on the joints they have to. This is achieved using xacro substitution args.

What I'm doing in CentroEPiaggio/kuka-lwr#42 is to create a kuka plugin, that instantiate an object of the abstract class lwr_hw that takes care of recognizing the kuka joints given the robot name. The plugin actually uses the derived-class that has the hooks to gazebo lwr_hw_sim. So, the transmissions are selected within the hardware interface by using the joint names and not the opposite as done in the gazebo_ros_control plugin, see here. In fact, any derived-class just needs to implement init(), read() and write() depending on the end-platform (FRI, Gazebo, VRep, etc.) with their corresponding low-level hooks and everything will be controllable by ROS-controllers 😎 Still won't work if there is a gazebo_ros_control plugin around because what I said above though, but will if each component plugin focuses to work on their joints, as in our Vito robot (the hand has its own plugin as well since the gazebo_ros_control doesn't understand the custom transmission).

[guihomework]

@nrontsis I was too busy to finish the manipulators_description package I would like to share, that would include both a nice way to generate multi robot description but also would provice launch files examples.
Instead I created an example with shadow's ur10+hand robot in sr_multi_decription + sr_robot_launch + sr_multi_moveit
you can find the changes here
ubi-agni@103a7f2

as a summary :

• using one arm+hand robot_description that instantiates the gazebo_ros_control plugin for ur10 (as for kuka) and the sr_gazebo plugin for the hand, in their respective (ra/rh) namespaces. They await a robot_description on the ra/rh namespace
• in ra namespace, one arm robot_description + controller spawing in the namespace + robot_state_publisher
• in rh namespace, one hand robot_description + controller spawning in the namespace (including joint_space_publisher which was before silently coming from arm launching) + robot_state_publisher
• an added static link just for the tf to be complete at the arm to hand link since two robot_state_publishers are used now, each listening to the namespaced /joint_states
• an added joint_state merger (same as what @carlosjoserg does) that merges /ra/joint_state and /rh/joint_states to recover /joint_states

I had to add files to instantiate right robots that do not exist in the original ur_description or sr_description to get the right prefixes also for the separate robot_description. In my future package this will be easier to implement with helper xacros.

Had to do a very small change in moveit controllers.yaml to tell the controllers are now in namespaces ra/ and rh/

I could move both arm and hand via the rqt joint_trajectory_controller plugin
I could plan and move the arm with moveit.

and most of all this now works with the original unmodified ur_description (and surprizingly also with the modified one with sr_transmissions)
So no code modification of third parties, just some changes in the way one instantiate the robots (and the plugins).

I was almost sure it would work with gazebo_ros_control plugin the same ways it works for our FRI plugin but with this example I am 100% sure.

I hope this clarifies our technique.

[guihomework]

I think I found one limitation of our method separating the plugins (robot_description) in namespaces. Now the arm plugin is not aware of the hand chain since it has its own separate robot_description containing the arm only, and hence has no information about the payload (mass, cog) it is handling.
Hence I have a question : @carlosjoserg how do you handle the payload of the arm at the end-effector ? I have seen you use KDL gravity compensation but you limit this computation to the last link of the robot. Because you have the whole chain (the whole urdf) in your approach, is the payload mass and COG automatically handled by JntToGravity even if the chain ends at the arm tip ? Or do you handle the payload mass externally via controllers that would be aware of the mass ?
Would it make sence, in a "smart" plugin that reads the whole chain, and know its start and stop segments, to handle additional segments and compute dynamically the equivalent, mass, inertia, cog of the tool ?

In the real kuka, everything is static as we had to give the tool mass and COG to get a nice gravity compensation at first and nice movements as well, so the simulation mode should have that too imho.
If I copy this real world setup, then the simulated arm plugin does not need to know the whole chain, but requires the payload mass and COG.

Does this make sense ?

[carlosjoserg]

I think I found one limitation...

Yep, I always thought the same.

Because you have the whole chain (the whole urdf) in your approach, is the payload mass and COG automatically handled by JntToGravity even if the chain ends at the arm tip ?

No, it is not automatically handled, howoever, it is possible to do it because, yes, we have the whole chain in a single URDF.

That's gravity compensation with a known model, that is, until the last link of the arm so far.

If you know the model, you can add a dummy tip link without geometry and set its inertial properties according to the tool you mount in as you do in the teach pendant FRI. Or you can parse what's after the arm last link with KDL and compute mass, COG and even inertia, of the chain/tree upstream as a point-mass.

If you don't know the model, you can use 3 static poses (as FRI does too) to determine mass&COG (inertia is much more complicate), or you can write an adaptive controller to estimate all dynamic parameters while the robot moves (including the robot itself)

It's all in ideas, no code so far.

Would it make sence, in a "smart" plugin...

IMHO, it is really hard to have one single generic plugin that rules it all. I think that each piece of hardware should have a plugin that replicates the important details of it, and that's what I'm aiming for by not using the gazebo_ros_control plugin.

The composability is another story, but happens also in reality.

In the real kuka, everything is static....

The idea for our kuka package is that the dependency on the FRI setup be zero. In the impedance mode, there is a plan to cancel the gravity term (which uses the tool definition from FRI), and send it from ROS (using xacro, yaml, urdf, code, still to decide)

so the simulation mode should have that too imho.
Does this make sense ?

Yes, it is all about minimizing the efforts to sync simulation and reality, you will see it with the kuka after I merge CentroEPiaggio/kuka-lwr#42

[guihomework]

Thanks @carlosjoserg for the detailed possible techniques, I already coded the reading of static parameters for payload mass&COG and it might probably suffice for now as we do not plan (yet) to do complex and fast movements with our kuka+shadow.

At ISIR https://github.com/kuka-isir/rtt_lwr they use a F/T sensor at the end-effector to dynamically compensate for external forces of the payload and close the loop through OROCOS. Of course this requires exactly what you mention, namely cancel the gravity term from FRI to add your own. This is what they do there too.

I will follow your work and the one at ISIR, but for the time being I will stay on the code I derived from RCPRG https://github.com/RCPRG-ros-pkg/lwr_robot (running orocos, and merging real and simulation behind FRI protocol) that runs nice in a separate namespace.

I hope this discussion also helps the Shadow team (and others) to view the different techniques and their advantages/disadvantages