KUKA iiwa: enable impedence mode configuration from C++

[ahundt]

Enabling CartesianImpedenceControlMode

We want to enable cartesian/joint impedance mode via the C++ interface to the kuka iiwa.

Since this is a configuration change there are a couple questions that need answering:

• [ ] Do we want to have things set up so it happens less frequently than joint messages/isn't required on every single message.
• [ ] Should there be a special ConfigurationUpdate flag/bool set to enable the above?
  • Currently I'm defaulting to yes because I added that flag in my original plan
• [ ] What ROS msgs should we use?
  • [ ] Probably use the config msgs from iiwa_stack
  • [ ] Are there standard ROS messages we should use instead? (I'm guessing no)

    Details

Current message in flatbuffers:

CartesianImpedenceControlMode https://github.com/ahundt/grl/blob/master/include/grl/flatbuffer/KUKAiiwa.fbs#L85

JointImpedenceControlMode https://github.com/ahundt/grl/blob/master/include/grl/flatbuffer/KUKAiiwa.fbs#L106

I was planning to put those in the config, rather than the motion command, and having it set in KUKAiiwaArmConfiguration, because it is independent of if the arm is controlled in cartesian or joint space.

Those should be set only if controlMode:EControlMode is in the appropriate mode. I think the fields still need to be added at the moment, so something like:

https://github.com/ahundt/grl/blob/master/include/grl/flatbuffer/KUKAiiwa.fbs#L199

should be modified with something like controlModeConfig:ControlModeConfig; below:

table NoControlMode{}

union ControlModeConfig { PositionControlMode, CartesianImpedenceControlMode, JointImpedenceControlMode }

// Set the configuration of the Kuka iiwa
table KUKAiiwaArmConfiguration {
  name:string; // set the robot name
  /// how commands will be sent to robot
  commandInterface:KUKAiiwaInterface; 
  /// how robot state will be sent to driver
  monitorInterface:KUKAiiwaInterface; 
  /// motion command mode: cartesian, wrench, torque commands
  clientCommandMode:EClientCommandMode; 
  /// The type of commands FRI will use: cartesian, joint
  overlayType:EOverlayType; 
  /// position, cartesian impedence, or joint impedence low level controller adjustments   
  controlMode:EControlMode; 
  controlModeConfig:ControlModeConfig;

  smartServoConfig:SmartServo;
  FRIConfig:FRI;

  tools:[LinkObject];

  /// set kuka tablet "processData" panel UI config strings
  processData:[ProcessData]; 
  currentMotionCenter:string;

  // TODO: add support
  requestMonitorProcessData:bool = false;
}

[ahundt]

@rkojcev here are our earlier conversations via direct email

I am expanding now the possibility to be able to set the Cartesian Impedance via ROS messages :)

I was wondering how can you cast a complex message, for example if I have Cartesian Stiffness which consists of Vec3 translational and Vec3 rotational (ROS geometry_msgs):

cartImpedance->stiffness.translational.x (float64) ...

What would be the best possible way to cast that to the flatbuffer

table CartesianImpedenceControlMode { /// did we changed cartesian impedance values? isCartImpedanceSet:bool = false; /// actual stiffness to set rot:[nm/rad] stiffness:EulerPoseParams; }

Is there any way to cast this in simple way instead of passing all the values... I am sorry I have never worked with flatbuffers before and I wanted to ask you before I make complex things that might not be necessary :D

Explanation of The Message

(“zoomed in” to “zoomed out” order)

It is MoveArmCartesianServo https://github.com/ahundt/grl/blob/master/include/grl/flatbuffer/ArmControlState.fbs#L29

That is part of the ArmState Union: https://github.com/ahundt/grl/blob/master/include/grl/flatbuffer/ArmControlState.fbs#L34

Which is stored in an ArmControlState (single time point arm motion command with several possible modes): https://github.com/ahundt/grl/blob/master/include/grl/flatbuffer/ArmControlState.fbs#L34

That is stored in the main KUKAiiwaState message: https://github.com/ahundt/grl/blob/master/include/grl/flatbuffer/KUKAiiwa.fbs#L266

Where to make Code Changes

C++

You’ll need to change the C++ section that talks to the Java Driver to support these messages:

https://github.com/ahundt/grl/blob/master/include/grl/kuka/KukaJAVAdriver.hpp#L291

add a section that supports: flatbuffer::ArmState::ArmState_MoveArmCartesianServo

Optionally you can also change the FRI driver to support the messages, let me know if you need that.

JAVA

Add another else if like the one linked here for MoveArmCartesianServo https://github.com/ahundt/grl/blob/master/src/java/grl/src/grl/driver/GRL_Driver.java#L343

ROS

You’ll probably need to add another callback like the following, but this time with the standard pose message: https://github.com/ahundt/grl/blob/master/include/grl/ros/KukaLBRiiwaROSPlugin.hpp#L275

Setting it into the cartesian command mode should already be setup (at this layer only, internals need to be updated) by the code at: https://github.com/ahundt/grl/blob/master/include/grl/ros/KukaLBRiiwaROSPlugin.hpp#L248

Risto Kojcev:

Thanks so much for your help.

I have done most of the things you have suggested :D

My idea is to expand the GRL by enabling for the user to set the Caresian Impedance parameters via C++.

For that purpose, I have decided to use new ROS message, which has (for now) the following structure:

MSG for the Damping Parameters https://github.com/rkojcev/majorana/blob/indigo-devel/cartesian_trajectory_msgs/msg/CartesianDamping.msg

MSG for the Stiffness parameters https://github.com/rkojcev/majorana/blob/indigo-devel/cartesian_trajectory_msgs/msg/CartesianStiffness.msg

The overall message I send to the KukaLBRiiwaROSPlugin.hpp is the following https://github.com/rkojcev/majorana/blob/indigo-devel/cartesian_trajectory_msgs/msg/SetCartesianImpedance.msg

The geometry_msgs/Vector3 is a structure as shown here: http://docs.ros.org/api/geometry_msgs/html/msg/Vector3.html

The flatbuffer I want to fill out is the following one:

https://github.com/ahundt/grl/blob/master/include/grl/flatbuffer/KUKAiiwa.fbs#L85

Everything works well and I can recive the newly created SetCartesianImpedance in KukaLBRiiwaROSPlugin.hpp

The thing that I do not understand is how I can fill out the CartesianImpedenceControlMode table via C++ code with the values I get from the new SetCartesianImpedance message while preserving the structure of each already assigned variable in the table.

For example, cartesian stiffness has the following structure:

cartesian_stiffness.translational.x cartesian_stiffness.translational.y cartesian_stiffness.translational.z

cartesian_stiffness.rotational.x cartesian_stiffness.rotational.y cartesian_stiffness.rotational.z

Is there any way how to easily assign the whole structure of cartesian_stiffness to the stiffness:EulerPoseParams?

This will help me quickly add the other parameters as well :D

Thanks so much for your help.

[rkojcev]

1. Do we want to have things set up so it happens less frequently than joint messages/isn't required on every single message.
   • Definitely this will be the case. It will be less frequent compared to the joint messages.
2. Should there be a special ConfigurationUpdate flag/bool set to enable the above?
   • Definitely. The controller needs to know when the impedance parameters have been changed, I guess that should be additional state in which there will be no motion executed.
3. What ROS msgs should we use? Probably use the config msgs from iiwa_stack
   • They are good but only for the iiwa...
4. Are there standard ROS messages we should use instead? (I'm guessing no)
   • I am working togeather with the ROS community to define and set standards for CartesianImpedance and Force control messages. I am working on this based on the following discussion as well as ongoing discussion with the ROS community. The development of the CartesianImpedance ROS msgs and services will be done on this repository forked and more frequently updated is here. Suggestions are welcomed!!

[ahundt]

Cool that's interesting. You may want to consider one of two possible approaches I'll outline below. If I were you I'd go with option 2. However, I outline the pros/cons below and leave the decision up to you, even if it is some option 3 I didn't think of. :-)

1. If you want to follow the ROS best practices

This option tends to take a lot longer than you think it will, but then others will use it too or you can build off stuff that already exists!

• find the correct forum, maybe ros-sig-robot-control
• explain what you're trying to do and ask if that's really the correct forum
• Ask if the links you found are correct (possibly out of date? not updated since 2013?)
• Ask if what you need is already done in terms of messages
  • you'll need to add the kuka implementation part, of course
• explain your design and ask for advice
• May need to implement then reimplement based on feedback

Pros

• may be compatible with other robots now or at some point in the future
• if it works out option 1 has potential to be way more widely useful for many robots

Cons

• plan is challenging
• time conuming
• may never be compatible with non-iiwa robots
  • KUKA doesn't always use real physical units
  • correction would be difficult
  • other robots will have their own implementation specific differences we don't know about here
• will not be compatible with other kuka user's existing iiwa_stack based drivers

2. Use what's in iiwa_stack

Pros

• Will instantly be compatible with many people using the iiwa
  • labs we both collaborate with use iiwa_stack
• More likely to be adopted by people who can use it, i.e. people that actually have an iiwa.

Cons

• may never be compatible with other robots

Conclusion

Those are my thoughts on the options, what do you think?

[rkojcev]

@ahundt I agree that it might be easier to get already defined messages from the iiwa_stack.

However I think that there should be some common effort where we can define ROS msg or API for Cartesian Impedance. Theoretically, Cartesian Impedance is already well defined. The parameters that we need to set (on the iiwa or LBR4+) in my experience are pretty much the same so I think we should define something that could be more general and useful for the community.

@shaun-edwards and @gavanderhoorn what are your thoughts on this one?

[gavanderhoorn]

Quick response based on quick scanning of @ahundt's comments: I think option 1 is exactly what @rkojcev's gsoc project is all about. Ideally, the msg set that is created is abstract enough that it isn't tied to any specific platform or implementation. Platform specifics should then be dealt with by conversion to/from those specifics (such as the 'non-units' used by KUKA).

We want to move the standard msg set forward. If we keep extending custom solutions just because they exist, because a lot of people are already using them, or because of time/effort concerns (or any combination of those three) I don't think we'll ever succeed in doing that. As an example: see how many *Cartestian*.msg variants are out there. It's clearly a need, but hasn't resulted in a standardised set of messages somehow.

@ahundt wrote:

Cons ..

• other robots will have their own implementation specific differences we don't know about here

The 'limited sample space' problem when trying to abstract is certainly something that will make this more difficult. We'll have to make sure any proposed msg set is abstract enough that it doesn't get tied to any specific (reference) platform, and at the same time concrete enough to be useful. A very nice SE problem :).

[ahundt]

I see I didn't know that was one of the goals! Then option1 makes sense.

I've actually spent a lot of time thinking about this. I'm on my phone now so I will keep it short. Essentially, there will always be something specific for every robot, but many commonalities can be abstracted. For example one kuka can have a touch flange on the end while another doesn't. Some robots may require a dead man switch to interact and others may not.

Back to the main topic. I think a message like you discuss makes perfect sense. Keep in mind there are multiple mathematical representations of the same information, think rotation matrix vs quaternion, so it is important to choose the right one for Cartesian impedance. Perhaps using physical units is best and it is up to the user to convert from the implementation specific units?

There are two message sets for ROS with this robot, the ROS messages and the internal driver flatbuffers messages. I tried to design them with a generic non implementation specific base types encapsulated with a message that is implementation specific. Notice the low level data types aren't specific then in the flatbuffers file labeled iiwa that has all the specifics.

Also for C++ driver APIs I suggest understanding tag dispatching and some generic programming, that's an approach that can get the generalizability and the specificity with a single API. If there is interest in hat I can give more detail later.

Let me know what you think about these thoughts, how I can help, and I'm happy to help with the next steps!

[shaun-edwards]

I'm a little late to the discussion. I agree with the direction this seems to be going. I would really like to see @rkojcev's project benefit the broader ROS community. If we can't find common ground, then I would suggest doing our best and creating a ROS-I message set. In reality, ultimate acceptance of the message sets will take time (i.e. adoption by other users/developers). By doing it in ROS-I, we ensure that the messages and the results of this effort are widely promoted. All that being said, I would like to engage the broader ROS community as much as possible.