Running instruction?

[IanYangChina]

Hi Matthias,

Great contribution to the community you have done :)
I followed your suggestion from your comment in another issue and come have a look.

I have some questions regarding running your codes.

1. Does it directly work with the FRI-ROS driver on ubuntu 18 and ros melodic?
2. Could you elaborate on what scripts are to be run in what orders, and what behaviours are to be expected?
   -- For example, start lbr_server (what option?), rosrun/roslaunch something something...

Thank you very much!

[matthias-mayr]

Great to hear about the interest.

1. I have not used that FRI driver yet. We are using iiwa_ros, but given that it is able to expose a torque (effort) interface, it should be possible to launch this controller
2. This is "only" a controller library, so it can be utilized by compiling it, specifying a controller configuration and starting it, e.g. with the controller_spawner. Since all these things are handled by the respective driver, there are no own launch files, etc. in this repo.

However, we provide an example configuration in the readme. My understanding is that this should go here in the lbr_fri_ros2_stack. It needs to be adapted for the robot configuration. Since this repo names the joints lbr, for example these need to be updated.

Then when starting things up, in lbr_fri_ros.launch these two changes are necessary:

1. Line 5: Change it to EffortJointInterface (or pass that manually when launching)
2. Line 24: As a second controller, specfiy CartesianImpedance_trajectory_controller

That might be everything that's needed, but take it with a grain of salt since I haven't used that iiwa driver yet.

Let me know if there are more questions of issues. I just pushed an update regarding the dependencies.

[anubhav-dogra]

hi, very nice contribution.
I am using KUKA LBR MED14 using iiwa_stack. I followed the above steps to adapt the controller configuration with Gazebo simulation. Everything seems alright as it says controllers are started ( Started controllers: CartesianImpedance_trajectory_controller, joint_state_controller
), However I dont think that the controllers are actually working in my case.

• The first indicator is that robot is crashed down as shown in this picture.

• I also checked with the rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller, there is no controller coming up to choose.
• I again checked with the rosrun rqt_controller_manager rqt_controller_manager and it says that the controller is running, as shown in this picture
  

Can you please help me in this case ?

[matthias-mayr]

We didn't test with iiwa_stack since it wouldn't allow to run this controller on the real robot, but I see the use-case to run it in simulation only and rely on iiwa_stack + KUKA when running on the real system.

The image that you are sharing seems to be from Gazebo. Are there any other messages? E.g. Gazebo complaining about something?

It would also be helpful if you could share a diff with the modifications that were made. Which branch/commit of iiwa_stack are you on?
Is the robot started with the EffortJointInterface? That could be a first starting point.

rqt_joint_trajectory_controller: This rqt plugin needs a specific controller name + topics to recognize a controller. I outlined them in #2. We were not aware of those and currently this controller does not implement them. But if someone wants to send in a PR or it's a frequently requested feature we are happy to have it.

[anubhav-dogra]

Hey thanks for the quick response.
There is not any error or warning on the terminal. But still robot is not in a good position.
If iiwa_stack would not support this package on the real robot, then there is no point of using this package with iiwa_stack. When I will check again for simulation in iiwa_stack and if it will work smoothly, then I will post it here for the solution. :)

So later, I tried it with iiwa_ros and it seems running. The only error which I am facing while running gazebo is this:

[ INFO] [1673016730.978022545]: Received urdf from param server, parsing...
double free or corruption (out)
[iiwa/iiwa_service-2] process has died [pid 32076, exit code -6, cmd /home/terabotics/stuff_ws/devel/lib/iiwa_tools/iiwa_service __name:=iiwa_service __log:=/home/terabotics/.ros/log/b2928640-8dd1-11ed-88e0-7713bf5c47e8/iiwa-iiwa_service-2.log].
log file: /home/terabotics/.ros/log/b2928640-8dd1-11ed-88e0-7713bf5c47e8/iiwa-iiwa_service-2*.log

Also when the iiwa_gazebo.launch is launched, robot is immediately falling on the ground. Is this expected behaviour?
Can I set stiffness values prior to the launch of the controller, so that robot should not fall on the ground and should not break itself or should not exceed the joint limits as sensed in this picture.

I also tried to setup with the moveit, working fine ! but Cannot plan motion because robot falls on the ground, and planner sees this as a large error in the actual and the state of the robot.

[ WARN] [1673016641.389473390, 11.850000000]: Joint 'iiwa_joint_2' from the starting state is outside bounds by a significant margin: [ -2.0955 ] should be in the range [ -1.98968 ], [ 1.98968 ] but the error above the ~start_state_max_bounds_error parameter (currently set to 0.1)
[ERROR] [1673016641.393506923, 11.860000000]: manipulator/manipulator: Motion planning start tree could not be initialized!

I would really appreciate if you would clearify these doubts.
Many thanks.

[matthias-mayr]

The robot should not fall upon being started. The controller starts with stiffness values of 200 & 20 for translation & rotational stiffness. That is sufficient to keep it from falling.

However regarding the dying iiwa_service with

double free or corruption (out)
[iiwa/iiwa_service-2] process has died [pid 32076, exit code -6

This typically happens if some parts of the software stack have been compiled with SIMD instructions. Right now iiwa_ros is not consistent in that aspect. See this PR here. If you have followed the current iiwa_ros installation instructions, the only missing step should be to turn SIMD off in the CMakeLists.txt like in the PR.

The reason why it's falling in your case is that iiwa_ros has a custom Gazebo HW plugin that uses the iiwa_service to do the gravity compensation. It should be fixed as soon as it's not dying anymore.

[matthias-mayr]

Did the instructions work?

[anubhav-dogra]

Yes it worked, however I still need to test it on the real KUKA by today, will update soon.
Thank you very much for your help.

[matthias-mayr]

Yes it worked, however I still need to test it on the real KUKA by today, will update soon. Thank you very much for your help.

I assume that you got the FRI library for iiwa_ros from Konstantinos. This one also needs to be built without SIMD instructions. It didn't fail for us when we didn't do it, but it should be done just to be on the safe side.

This is the patch. It can be applied with git apply patch.txt

From cb623f2d6a9baabf6a232a90b5def1939d32326e Mon Sep 17 00:00:00 2001
From: Matthias Mayr <[email protected]>
Date: Mon, 12 Dec 2022 14:37:38 +0100
Subject: [PATCH] Config: Disables SIMD, march=native by default

iiwa_ros does not use it by default anymore
---
 wscript | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/wscript b/wscript
index 9978455..d4a8fea 100644
--- a/wscript
+++ b/wscript
@@ -22,6 +22,7 @@ def options(opt):
     opt.load('compiler_c')
 
     opt.add_option('--shared', action='store_true', help='build shared library', dest='build_shared')
+    opt.add_option('--native', action='store_true', help='build with march=native', dest='native')
 
 
 def configure(conf):
@@ -33,19 +34,28 @@ def configure(conf):
     if conf.options.build_shared:
         conf.env['lib_type'] = 'cxxshlib'
 
+    native = ''
+    native_icc = ''
+    if conf.options.native:
+        conf.msg('-march=native (AVX support)', 'yes', color='GREEN')
+        native = ' -march=native'
+        native_icc = ' mtune=native'
+    else:
+        conf.msg('-march=native (AVX support)', 'no (optional)', color='YELLOW')
+
     if conf.env.CXX_NAME in ["icc", "icpc"]:
         common_flags = "-Wall -std=c++11"
-        opt_flags = " -O3 -xHost -mtune=native -unroll -g"
+        opt_flags = " -O3 -xHost -unroll -g" + native_icc
     elif conf.env.CXX_NAME in ["clang"]:
         common_flags = "-Wall -std=c++11"
-        opt_flags = " -O3 -march=native -g -faligned-new"
+        opt_flags = " -O3 -g -faligned-new" + native
     else:
         gcc_version = int(conf.env['CC_VERSION'][0]+conf.env['CC_VERSION'][1])
         if gcc_version < 47:
             common_flags = "-Wall -std=c++0x"
         else:
             common_flags = "-Wall -std=c++11"
-        opt_flags = " -O3 -march=native -g"
+        opt_flags = " -O3 -g" + native
         if gcc_version >= 71:
             opt_flags = opt_flags + " -faligned-new"
 
-- 
2.17.1

Maybe I should write a gist about this. All the iiwa people would need to go through this step, but just because of iiwa_ros and not this controller.

[anubhav-dogra]

Alright thanks for the patch information, I will apply it just for the safe side. As of now, it is working without any change in the FRI module.

But before this, I tested the robot, the robot started up with iiwa_bringup.launch and selected the controller:=CartesianImpedance_trajectory_controller.

I noticed one thing that when I select the Torque mode from the SmartPad and selecting value of stiffness other than 0, the robot starts oscilating (from the Cartesian Pose) (not very fast, but at decent slow velocity).

I checked once with selecting the value as 0 and it goes fine, it doesnot oscilate.

I want to use the reference_pose topic as per my application, and I figured the robot moves very slowly and is having an error to the desired pose.
How do I verify the CartesianPose is reached accurately as commanded? Is there a way to get the CartesianPose topic as like in iiwa_stack?
How do I change the velocity of motion ? or trajectories?

[matthias-mayr]

Cool. Great to hear. @majstenmark will be interested learning that it would run on the iiwa med14.

selecting value of stiffness other than 0, the robot starts oscilating (from the Cartesian Pose) (not very fast, but at decent slow velocity)

We never tried that and I am not surprised. There are two things happening here if you select non-zero stiffnesses:

1. When running in torque control mode, iiwa_ros applies some oscillations to the position signal. We need this for better torque control since otherwise the KUKA stack turns off the friction compensation.
2. Now the internal joint impedance controller of KUKA and the Cartesian impedance controller signals overlap. We have never tried that since I didn't see any use-case.

What your seeing sounds mostly like 1.. It's interesting to hear about, I never tried it, because I did not see a point.
Long story short, not select non-zero stiffness.

I want to use the reference_pose topic as per my application, and I figured the robot moves very slowly and is having an error to the desired pose.

I got you covered here since we're working similarly in our research and my research code. Here is a trajectory generator that makes Cartesian linear trajectories. Since this one makes smooth motions, one can turn off the pose filtering of the controller by setting it to 1.0

How do I verify the CartesianPose is reached accurately as commanded? Is there a way to get the CartesianPose topic as like in iiwa_stack?

We typically use tf for that. if verbose state messages are enabled in the configuration, the current pose is also included.

How do I change the velocity of motion ? or trajectories?

With the Cartesian trajectory generator you can define a velocity in the configuration file.

If joint-space trajectories are used, they define the velocity. In both cases, filtering should be turned off. We do not use it in our setup. It's mostly there to provide people a lower entry hurdle without needed to fear about their robot.

[anubhav-dogra]

Super ! Thanks for your ideas on this.

Long story short, not select non-zero stiffness.
NOTED :)

I have some more queries, thanks for helping already. :)

As I read somewhere in the issues that gravity compensation is already handled by the kuka controller. However, if we launch the TorqueControl mode in iiwa_ros with 0 stiffness, the robot starts to fall slowly untill it touches the ground. Ideally, it should be maintaining its position where ever it is, right ?
with Cartesian Impedance Controller we are giving some stiffness and damping values so that robot is able to maintain its position.

How do I verify the CartesianPose is reached accurately as commanded? Is there a way to get the CartesianPose topic as like in iiwa_stack?

I realized that there is a significant error between the commanded position and the actual position of the end-effector. How do I minimize it? Do I need to increase the stifness values of the joints?

We typically use tf for that. if verbose state messages are enabled in the configuration, the current pose is also included.

I was trying to use services from the iiwa_tools such as getFK, getJacobian, etc., as I want to publish the wrench at the end-effector too, but Im very confused on how to use them properly. My code is getting segmentation core dump error as I'm trying to copy joint_states and use it for the service call data.
Do you have any idea how to use these services ? do you have any example codes ?

I got you covered here since we're working similarly in our research and my research code. Here is a trajectory generator that makes Cartesian linear trajectories. Since this one makes smooth motions, one can turn off the pose filtering of the controller by setting it to 1.0

Thankyou so much for these studies and references, I will surely check them. Can I fork your repo?

[matthias-mayr]

As I read somewhere in the issues that gravity compensation is already handled by the kuka controller. However, if we launch the TorqueControl mode in iiwa_ros with 0 stiffness, the robot starts to fall slowly untill it touches the ground. Ideally, it should be maintaining its position where ever it is, right ?
with Cartesian Impedance Controller we are giving some stiffness and damping values so that robot is able to maintain its position.

Yes, that can happen if the robot is not perfectly calibrated. You can re-run the KUKA tool calibration and check whether it got better. Our one usually lifts a bit. I talked to some guys from Stanford and they ran an identification using the external torques and added an imaginary mass at the end-effector.

I realized that there is a significant error between the commanded position and the actual position of the end-effector. How do I minimize it? Do I need to increase the stifness values of the joints?

Yes.

Do you have any idea how to use these services ? do you have any example codes ?

Answered here: epfl-lasa/iiwa_ros#34 (comment)

Can I fork your repo?

Of course. That's what open source is about. Feel free to send pull requests as well. We implemented what we needed ourselves and found generally useful. I am sure there are new interesting and relevant features that could be integrated.

[matthias-mayr]

Small calibration update: It has been discussed in iiwa_ros again..

I am assuming that this issue is handled. If there are future questions, feel free to open new issues.

[anubhav-dogra]

Hi @matthias-mayr,
What I want is: True value of End-effector Force ( computed using $\tau_{ext}$ from the iiwa_ros and Jacobian !)
What I observe: The end-effector Force (in Z) is not zero (ideally it should be) when the robot is static.
There is always a offset to the true set-point reference even if you increase Cartesian Stiffness to high values (1000!)

I did a simple experiment to check the problem.

Current Stiffness configuration is [1000,1000,800,100,100,100]
setpoint wanted is 245 mm

In figure, ignore Time (sec), its just sample points from rosbag.

At 1: Im changing the nullspace configuration of the robot, and I found that wrench is improving significantly.

then it is moved to some random position, and brought back to original position using cartesian_trajectory_generator package you provided.

At 5: The nullspace configuration is similar. But first the wrench value is high, Im gently slapping (twice) the end-effector of the robot, it again comes back to a better state.

I also recorded end-effector position value along with it.

What do you think about this problem? why setpoint is not sticking to the same value even though cartesian stiffness values are same.
Is it a problem of true gravity compensation ? or something else.

[matthias-mayr]

(Unfortunately I missed the notification about your comment in my inbox)
Thanks for bringing this up. It's always great to hear some user stories. A couple of comments:

1. As long as there's a force at the end effector we expect deviations from the reference pose due to the nature of the implemented control laws. If these forces come from some measurement errors, imperfect calibration or actual interaction does not really play a role
2. We always had issues with the calibration of our iiwas and I have not met a single user who did not suffer from this. The Stanford robotics lab has implemented some additional measurement procedure with their own driver and map the deviations to a virtual mass that they are compensating for. That helped them, but afaik it's not publicly available.
3. When a nullspace configuration is applied, this nullspace configuration should have forward kinematics that result in the reference pose, otherwise deviations from the reference pose are to be expected
4. My assumption on why the end-effector wrench gets better in the beginning when applying the nullspace configuration is that this is mostly affected by that joint configuration being more favorable regarding the calibration error
5. Another effect is joint stiction. It's quite high with these arms and it can affect the accuracy in torque control. We have conducted some experiments and there's a thesis online
   • tl;dr: When applying some additional overlay torque the accuracy gets slightly better, but it wasn't enough so it felt to make sense to include it into the controller or the driver

As far as I know about iiwa_ros, this controller and the robot, the reasons for this are a bit outside the individual packages.
But of course the packages could in principle implement some measures, e.g. additional calibration procedures or to overlay torque commands to address the stiction and friction.

[anubhav-dogra]

Hi @matthias-mayr thank you for your reply, and clearfiying these doubts.
I have one more please:
The actual implementation of the controller should contain M matrix in the model like this:
$\tau_c = J^T (q)[M_x (x) \ddot{x}_d +C_x (x, \dot{x}) \dot{x}_d − K_d x_e − D_d \dot{x}_e]$ and

$\tau_{ns} = \left[ I - J^T[JM(q)^{-1}J^T]^{-1}JM(q)^{-1}\right]\tau_0$

but in the code implementation, these are actually ignored, as defined in your paper

Is there any specific reason for that? or assuming quasi-static cases only?

[jsaltducaju]

Hi @anubhav-dogra ,

For the task space torque, the virtual inertia is chosen as the robot inertia to avoid inertia shaping [Ch. 3.2, Ott2008], so that the input force does not require feedback from the external forces.

For the null-space torque, the current implementation assumes quasi-static cases as you mentioned. Please note that M(q) is not the only weighting matrix for the pseudo inverse of the Jacobian matrix that would achieve dynamic consistency. An alternative to M(q) was proposed in [Park1996]. Also, as discussed [Dietrich2015], "the differences between static and dynamic consistency are significantly smaller than expected when real hardware is considered."

---

[Ott2008]: Ott, C. (2008). Cartesian impedance control of redundant and flexible-joint robots. Springer.

[Park1996]: Park, J. (1999). Analysis and control of kinematically redundant manipulators: An approach based on kinematically decoupled joint space decomposition. PhD Thesis, Pohang University of Science and Technology.

[Dietrich2015]: Dietrich, A., Ott, C., & Albu-Schäffer, A. (2015). An overview of null space projections for redundant, torque-controlled robots. The International Journal of Robotics Research, 34(11), 1385-1400.