Gazebo model joints move by themselves

[saalimperator]

Hello!

I'm currently working with your Gazebo model to test my controller. To do this, I modified one of your example controllers so that all joints except the first one keep their current angle. The first one is waiting for input from my controller, which is applied directly to the joint using the ".setCommand(tau)" function.

Now for my problem: when I start Gazebo, it seems to maintain the initial state, but after a few seconds the first joint starts to move slightly counterclockwise without any input from me. Is this normal behavior of this joint when the input is set to zero? Or is there something wrong with the inertia, friction and damping of this joint. When I send some inputs to this joint, it starts to move, but it is not slowed down by friction or damping and just bounces back when it hits the joint limits.

Can you give me a hint where I am doing something wrong?

[rickstaa]

@saalimperator Thanks for creating this issue.

It might not be related to your problem but I also noticed this behaviour even when no controllers are loaded. In that case, one would expect the robot to just fall down and stay there. The behaviour that occurs is however very strange (See gif 1 below). The first time I saw this, I thought it was because of gravity compensation, but there is no code for this the franka_state_controller or franka_gripper controllers.

This behaviour is also present when using the force_example_controller roslaunch franka_gazebo panda.launch controller:=force_example_controller which does contain gravity compensation. Probably, since the PID gains start at 0.

https://github.com/frankaemika/franka_ros/blob/b9b60e8a8316593b6a82d3bfa48a5b89155f42a5/franka_example_controllers/src/force_example_controller.cpp#L115)

The other strange thing is that the path that the arm takes with not controllers looks the same every time the simulation is started.

It could be related to the unstable dynamics that is described in https://github.com/justagist/franka_panda_description/issues/5.

[gollth]

In that case, one would expect the robot to just fall down and stay there.

Not really. The franka_hw_sim plugin simulates the real robot in that it compensates the gravity of arm when you send zero (or no) commands from the controller.

https://github.com/frankaemika/franka_ros/blob/1922fc9d24e37b2a8226c522e736eaa9a65d66ca/franka_gazebo/src/franka_hw_sim.cpp#L310-L317

Imagine this on the real robot. When you have a ROS controller which sends zero torque, the robot won't fall to the ground either but behaves like in "free space" essentially. This is also what happens when no controller is running.

Why the robot is drifting is unclear to me, though. I suspect numerical instabilities somewhere between franka_hw_sim and Gazebo.

when I start Gazebo, it seems to maintain the initial state, but after a few seconds the first joint starts to move slightly counterclockwise without any input from me.

@saalimperator a possible solution would be to have the controller active at all times. So while you are waiting for input, generate commands in your controller that hold the robot at a defined configuration. This should stop the drifting

[rickstaa]

In that case, one would expect the robot to just fall down and stay there.

Not really. The franka_hw_sim plugin simulates the real robot in that it compensates the gravity of arm when you send zero (or no) commands from the controller.

https://github.com/frankaemika/franka_ros/blob/1922fc9d24e37b2a8226c522e736eaa9a65d66ca/franka_gazebo/src/franka_hw_sim.cpp#L310-L317

Imagine this on the real robot. When you have a ROS controller which sends zero torque, the robot won't fall to the ground either but behaves like in "free space" essentially. This is also what happens when no controller is running.

Why the robot is drifting is unclear to me, though. I suspect numerical instabilities somewhere between franka_hw_sim and Gazebo.

when I start Gazebo, it seems to maintain the initial state, but after a few seconds the first joint starts to move slightly counterclockwise without any input from me.

@saalimperator a possible solution would be to have the controller active at all times. So while you are waiting for input, generate commands in your controller that hold the robot at a defined configuration. This should stop the drifting

@gollth At the time of writing, I forgot to check the franka_hw_sim for any gravity compensation. Thanks for pointing me to the relevant code. In that case, numerical instabilities could indeed cause this drifting. I have seen the same behaviour in other simulated robots.

For now, in my experiments, I will use your workaround and try to send control commands to mitigate the drift.

[saalimperator]

@saalimperator a possible solution would be to have the controller active at all times. So while you are waiting for input, generate commands in your controller that hold the robot at a defined configuration. This should stop the drifting

Thanks for the answer. I already expected something like this. Unfortunately, your proposed solution does not work for my project, because I am trying to determine the complete dynamics of the robot by means of an experiment. I have already found a better simulation environment for my case. Much success with the programming!

[rickstaa]

@saalimperator Just out of interest, may I ask which simulation environment you switched?

@gollth As this bug leads to strange behaviour, I was wondering if there are plans to fix this bug in the future?

[saalimperator]

Sorry I got that wrong, I didn't switch to another environment I switched the model. I am now working with an UR in Gazebo. But what I want to do is rather special. My programm communication looks like this: Matlab<->Simulink<->ROS<->Gazebo

[rickstaa]

@saalimperator Thanks for your response! Ah, I see colleagues of my have the same communication scheme.

Since the issue you described above still exists in the franka_gazebo package, maybe let's re-open it for now? In this way, it can serve as a bug report.

[rickstaa]

@gollth I managed to improve the behaviour by passing the gravity vector from the Gazebo simulator to the gravity compensation function. See #177.

After applying this pull request, the behaviour becomes more stable, but the robot still drifts a bit. I, however, could not find anything wrong with the current implementation. Do you maybe know other factors that can cause this drift?

Things I checked

• Is there a delay between the sensor data and the control?: There appears to be no delay between the readSim and writeSim command, since they are both contained in the gazebo_ros_control_plugin.update() method. Also, due to this, the rate of control is equal to the simulation rate.
• Is the floating-point precision of the involved parameters decreased during execution?: To my knowledge, all the parameters are defined as doubles and stay this during the update.
• Are the effort limits defined in the robot description high enough to apply for the gravity compensation?: Since the effort limit is defined as Nm, I think all the joints are well able to perform the gravity compensation.

https://github.com/frankaemika/franka_ros/blob/1922fc9d24e37b2a8226c522e736eaa9a65d66ca/franka_description/robots/panda_arm.xacro#L102

Other causes

• Are there other physics parameters that are needed that are currently hardcoded?
• Are there any round off errors during the gravity compensation?
• Is there any other component that sets joints commands?

[rickstaa]

@gollth Are there any updates on this?

I tried using your suggestion, but for my use case, sending control commands at every time step are not optional. I'm training a RL algorithm to try to control the robot arm. When gravity control is not working, the task becomes a lot harder, since it needs to be able to send control commands and thus do inference at a high frequency.

[gollth]

I'm currently investigating the issue

[rickstaa]

Great! I'm curious about what causes the drift that is still there. Based on the gazebo gravity compensation tutorial I think this drift should not be there. 🤔

[gollth]

Okay I think I have an idea where the problem comes from. It's part of a systematic error in the current calculation of the efforts coming from Gazebo. It's not only related to joints moving on their own without controller, but will affect any controller!

Let me try to explain what I think is going on:

Hypothesis

There is a bug in the way Gazebo calculates torques if the joint origin is rotated. The critical function is gazebo::physics::ODEJoint::GetForceTorque()

Bug Reproduction

Nominal Case

To see more clearly what is going on, let's create a simplified URDF containing only equally long sticks (length 10cm) with equal masses (5kg), where joints have zero rotation (i.e. <origin rpy="0 0 0" />) and bring the robot in the following configuration:

Let's focus on joint6 individually. We would expect a (negative) torque around joint6's Y axis because of gravity of link 6 and 7. If we print some values from during the writeSim() step, we see this confirmed:

Quantity	Value	Source
Effort acting on joint	-0.750765	body2Torque.dot(joint->axis)
KDL Gravity Term	0.750765	model->gravity(q)
Torque calculated by Gazebo	[-0.000793, -0.750765, -0.225921]	GetForceTorque(0).body2Torque
Joint Axis in World frame	[0.000000, 1.000000, 0.000000]	joint->GetGlobalAxis()
Joint Axis in Local frame	[0.000000, 1.000000, 0.000000]	joint->GetLocalAxis()
Joint Axis defined in URDF	[0, 1, 0]	joint->axis_

As expected Gazebo measures a torque around the Y-axis of the joint, which is the degree of freedom, and some stray torques in other directions. The magnitue also seems to match. We have the torque from link6 (5kg × 0.05m ≈ 0.25Nm) plus the torque from link7 (5kg × 0.1m ≈ 0.5Nm). This matches the torque which KDL calculates for its gravity term.

Bugged Case

Now let's do the same thing, but rotate the joint's origin by 90 degrees, i.e. <origin rpy="${pi/2} 0 0" />. Bringing the robot in a similar configuration should now make it experience (negative) torque, this time only coming from the weight of link7 alone, since joint6 is now rotating around Z.

Again we look at the interessting values:

Quantity	Value	Source
Effort acting on joint	-0.507839	body2Torque.dot(joint->axis)
KDL Gravity Term	0.250255	model->gravity(q)
Torque calculated by Gazebo	[-0.000254, -0.507839, 0.000003]	GetForceTorque(0).body2Torque
Joint Axis in World frame	[0.000000, -1.000000, 0.000000]	joint->GetGlobalAxis()
Joint Axis in Local frame	[0.000000, -1.000000, 0.000000]	joint->GetLocalAxis()
Joint Axis defined in URDF	[0, 1, 0]	joint->axis_

Here something strange is happening! KDL correctly calculates that the gravity term acting on the joint should only come from link7 (5kg × 0.05m ≈ 0.25Nm). But the result of GetForceTorque() yields a different result namely 0.5Nm. It seems that this is the result of the torque before rotation? Also the signs don't match.

Panda URDF

For the original Panda URDF this get's of course more complicated. When we plot the difference between the gravity term of KDL and the torque measured by Gazebo over time we see that this is really a systematic error and not (only) related to numerical noise as initially assumed:

Solutions

1. Create a bug report in osrf/gazebo, however there are 1.4k other issues already waiting...
2. Transform meshes/frames/inertias/axis such that the joint in the URDF do not contain any rotation.
3. Hope anybody has an idea how to correctly transform the result of GetForceTorque(). Would appreciate any further ideas or insights 😕

[gavanderhoorn]

1. Create a bug report in osrf/gazebo, however there are 1.4k other issues already waiting...

I would suggest to always do this, if you believe there is no other bug report already open about this.

The osrf/gazebo issue tracker is sort-of also used as a (public) todo-list, so don't let that high nr of open issues dissuade you from reporting something you believe is wrong.

Getting this fixed -- provided the analysis is correct -- would be tremendously valuable.

---

Edit: what would be good though perhaps would be to check whether Ignition has this same problem.

As Ignition has developer focus, finding the same problem there would probably make it easier to dedicate engineering resources to fix it, and back-porting it to Gazebo should then not be too difficult.

---

Edit 2:

Transform meshes/frames/inertias/axis such that the joint in the URDF do not contain any rotation.

no, that would be a work-around. Not a solution.

And would also not be very nice to those other 1000s of users who cannot do this (with their models).

[rickstaa]

@gollth Thanks a lot for finding the root cause of this issue and for creating the steps to reproduce it. Can you maybe push a small example repo to GitHub with the files you used to debug this problem? This will save me time when repeating the steps you describe above.

I agree with @gavanderhoorn this problem is likely affecting multiple repositories and users and should be fixed on osrf/gazebo. I think @gavanderhoorn's suggestion to check if the problem exists in ignition would be an excellent first step. If it does, we can create an issue there and backport the solution. If it does not, we can check what ignition does differently.

I will try to reproduce your example and then check if I can find more information about what causes the issue inside the gazebo/ignition code.

---

Edit1: I will first fix https://github.com/frankaemika/franka_ros/pull/177

[gollth]

Hey @rickstaa, thanks for taking a look. You can find the Stick URDF and the prints in this commit. Also look at its commit message to see how to reproduce the above described behavior.

[rickstaa]

@gollth I will start my investigation of this issue. I will keep this comment as a notepad.

TODOS

• [x] Reproduce the problem in gazebo using the steps given by @gollth.
• [x] Check if the bug is present in the ignition simulator.
• [ ] Inspect what is causing the bug in the Gazebo source code.

Reproduce the bug in gazebo

1. Clone the gazebo-torque-calculation-bug repository using the --recursive. flag.
2. Install the ROS dependencies.
3. Build the catkin workspace.
4. Start the example launch file using roslaunch gazebo_torque_calculation_bug stick.launch load_two_sticks:=true
5. Check the plot to see that the torque of stick 2 is incorrect.

I can not reproduce the bug in the Gazebo v11 simulator.

Check if the bug is present in the ignition simulator

I do not see the bug in the Ignition v6.0 simulator

Possibly related issues

• https://github.com/osrf/gazebo/issues/2476
• https://github.com/osrf/gazebo/issues/2209

[rickstaa]

@gollth quick question. I think

$(calc 'deg2rad(90)')

is a syntax that is specific to your setup? I think calc is referring to the calc-common package but I could not find the deg2rad function. I used hardcoded values for now but I think these methods are very useful when having to deal with non 0/30/45/60/90 degree angles.

[rickstaa]

@gollth Based on your commit, I created a minimal example repository (i.e. no dependency on franka_ros) we can use in our Gazebo issue. This repository can be found here. I will make sure an RQT plot displays the torque, the joints can be controlled through a GUI, and some information is printed to the cmd line. Further, I will also add an Ignition launch file to this repo. I will keep you posted.

[gollth]

@gollth quick question. I think

$(calc 'deg2rad(90)')

is a syntax that is specific to your setup? I think calc is referring to the calc-common package but I could not find the deg2rad function. I used hardcoded values for now but I think these methods are very useful when having to deal with non 0/30/45/60/90 degree angles.

Hi @rickstaa,

thanks for looking into the issue. No calc is way dirtier than that, and comes from my local setup:

calc () {
  expression=$@ 
  python -c "from numpy import *; print($expression)"
}

Feel free to use whatever else you find suitable

[rickstaa]

Ha nice little trick! I mostly write bash scripts but this is a lot faster and versatile.

[rickstaa]

@gollth I investigated your issue, but I'm not able to reproduce it using your steps provided in https://github.com/frankaemika/franka_ros/issues/160#issuecomment-961780423. First, it looks like the figure included in the Bugged Case does not correspond with the data shown in the table. In the figure, the local URDF axis is 0 0 1 where the table shows 0 1 0. Further, I cannot reproduce the error between the KDL GravComp and Total Effort using your example; I also do not see any problems using my example.

Your example

I used a modified version of your example that I included as a submodule in my example repository. The franka_ros submodule that is in there includes your commit. The only thing I did was to add a rotated launch file argument to make it easier to inspect both cases (see c8b153b14dfd27ff513289bdf2f916e343ce66c3) and published the debug info to topics (see 01c8011a2ddd9c402a76c21e31e77b216a4d4037). But the error you described above also doesn't show up on my system when I revert these commits and test your original example.

Results

Nominal case

roslaunch franka_gazebo panda.launch \
  use_gripper:=false \
  headless:=false \
  paused:=true \
  rotated:=false \
  initial_joint_positions:="-J panda_joint6 $(calc 'deg2rad(-90)')"

initial result

panda_joint6: Error:                 0.750765
panda_joint6: Total Effort:          0.000000
panda_joint6: KDL GravComp:          0.750765
panda_joint6: KDL GravComp flipped:  -0.750765
panda_joint6: Gazebo body2Torque:   [0.000000, 0.000000, 0.000000]
panda_joint6: Desired joint command: 0.000000
panda_joint6: Joint command:         0.750765
panda_joint6: Global Axis: [0.000000, 1.000000, 0.000000]
panda_joint6: Local Axis:  [0.000000, 1.000000, 0.000000]
panda_joint6: URDF Axis:   [0, 1, 0]
panda_joint6: Joint velocity:        0.000000

after one step

panda_joint6: Error:                 0.000000
panda_joint6: Total Effort:          -0.750765
panda_joint6: KDL GravComp:          0.750765
panda_joint6: KDL GravComp flipped:  -0.750765
panda_joint6: Gazebo body2Torque:   [-0.000793, -0.750765, -0.225921]
panda_joint6: Desired joint command: 0.000000
panda_joint6: Joint command:         0.750765
panda_joint6: Global Axis: [0.000000, 1.000000, 0.000000]
panda_joint6: Local Axis:  [0.000000, 1.000000, 0.000000]
panda_joint6: URDF Axis:   [0, 1, 0]
panda_joint6: Joint velocity:        0.000277

All looks correct to me:

• Effort is caused by the gravity of both link_6 and link_7 (i.e. 0.5 kg * -9,8 kg/m × 0.1m + 0.5kg * -9.8 kg/m * 0.05m = -0.735 N/m)
• The torques and efforts' signs are also correct when using the right-hand rule.

Bugged case

roslaunch franka_gazebo panda.launch \
  use_gripper:=false \
  headless:=false \
  paused:=true \
  rotated:=true \
  initial_joint_positions:="-J panda_joint6 $(calc 'deg2rad(90)')"

initial result

panda_joint6: Error:                 0.250255
panda_joint6: Total Effort:          0.000000
panda_joint6: KDL GravComp:          0.250255
panda_joint6: KDL GravComp flipped:  -0.250255
panda_joint6: Gazebo body2Torque:   [0.000000, 0.000000, 0.000000]
panda_joint6: Desired joint command: 0.000000
panda_joint6: Joint command:         0.250255
panda_joint6: Global Axis: [-0.000000, -1.000000, -0.000004]
panda_joint6: Local Axis:  [0.000000, 0.000000, 1.000000]
panda_joint6: URDF Axis:   [0, 0, 1]
panda_joint6: Joint velocity:        0.000000

after two steps

panda_joint6: Error:                 0.000000
panda_joint6: Total Effort:          -0.250255
panda_joint6: KDL GravComp:          0.250255
panda_joint6: KDL GravComp flipped:  -0.250255
panda_joint6: Gazebo body2Torque:   [-0.000317, -0.780290, -0.250255]
panda_joint6: Desired joint command: 0.000000
panda_joint6: Joint command:         0.250255
panda_joint6: Global Axis: [0.000000, -1.000000, -0.000004]
panda_joint6: Local Axis:  [0.000000, 0.000000, 1.000000]
panda_joint6: URDF Axis:   [0, 0, 1]
panda_joint6: Joint velocity:        0.000022

It also looks correct to me:

• Effort is caused by the gravity of both link_6 and link_7 (i.e. 0.5 kg * -9,8 kg/m × 0.05m = -0.245 N/m)
• The torques and efforts' signs are also correct when using the right-hand rule.

How to use my example

You can find a guide on using my example in the README. To quickly start, run the following command after building the catkin workspace:

roslaunch gazebo_torque_calculation_bug stick.launch initial_joint_positions:="-J stick_joint1 $(calc 'deg2rad(-90)')"

or for the bugged case

roslaunch gazebo_torque_calculation_bug stick.launch rotated:=true initial_joint_positions:="-J stick_joint1 $(calc 'deg2rad(90)')"

In my example, I got the same results as your example. Maybe I misunderstood your example, or I overlooked something. I will investigate further.

[rickstaa]

@gollth The only two things that I have found so far from Gazebos side that could perturb your gravity compensation are:

• There is a startup delay of 0.2 s for the Gazebo effort calculation. This delay is present in both the GetForce and GetForceTorque(0).body2Torque methods but not for the gravity component that is calculated using the orocos KDL library. As the delay is small and subsides quickly, it should not influence the gravity compensation controller much.

• There also is a minor delay; see the peak in the gif below when we control the robot. As the difference subsides quickly, this shouldn't influence the drift that is seen when the robot is stable.

I also saw these effects in the Ignition simulator.

EDIT:

When we add gravity compensation the initial delay only exists for a one-time step since the compensation applies an effort in the first time step:

Therefore this should not have an effect. Also, the control delay becomes smaller when gravity compensation is enabled

[rickstaa]

I also tried to migrate the example to the Ignition simulator see gazebo-torque-calculation-bug#ignition-gazebo. I, however, think that this requires a bit more work since the ros-simulation/gazebo_ros_pkgs is not ignition compatible. I could not find an Ignition equivalent in the ros_ign.

I think testing only the bug in the ignition simulator requires us to create a small sensor plugin since I saw no ignition topic that contained the joint effort. I will check if we can use the ForceTorque sensor that is contained in the ign-sensors package. The nominal and bugged configurations are now hardcoded into the stick_description xacro file since the ros_ign_gazebo/create.cpp method does not yet take joint angles. I further changed the revolute joint to a fixed joint so that we don't need to implement a control plugin.

As I have no experience with the ignition simulator, I am not sure the easiest way to test this bug. Maybe @gavanderhoorn has some suggestions. However, let's first find the proper steps to reproduce the bug in Gazebo.

[gavanderhoorn]

I'm not sure I completely understand what you're looking for, but perhaps ignitionrobotics/ign_ros2_control#1 helps?

[rickstaa]

@gavanderhoorn Thanks for your answer. I did not see that pull request; it will undoubtedly be helpful in the future!

For now, I ended up coding the configurations directly in the world file while using a rotated launch argument to switch between the nominal and bugged position. I then use the ignition-gazebo-forcetorque-system plugin to get the torque. I then use the ignition::gazebo::systems::JointPositionController plugin to control the joints through the Joint position control ignition GUI.

[rickstaa]

@gollth The ignition example can now also be found under the example repository. Use the following command to start the nominal case:

roslaunch gazebo_torque_calculation_bug stick_ignition.launch

To start the bugged case use:

roslaunch gazebo_torque_calculation_bug stick_ignition.launch rotated:=true

After you started the simulation you can check the joint force and torque by using the following command:

ign topic -e -n1 -t /stick_joint1/force_torque

In both options, you can control the stick joints using the Joint position controller GUI.

Results

The torques/forces below are taken from the child to the parent as expressed in the child frame.

Nominal case

force {
  x: -9.9999906227737618
  y: -1.0384421671310667e-18
  z: 0.012471274055568547
}
torque {
  x: -0.00031178185139543513
  y: -0.74999929510605634
  z: -0.24999976556444289
}

Bugged case

force {
  x: -9.9683132042998146
  y: 2.2186868122886389e-15
  z: -2.775557117473697e-16
}
torque {
  x: -1.5946189417214406e-16
  y: -0.74683136042998111
  z: -3.7261796105162911e-06
}

Current conclusion

It looks like the configurations you provided does not give any problems in the ignition simulator.

[rickstaa]

@gollth I did one last inspection, and it looks like, in my example, the torque and force calculations are working fine in both the Gazebo 11 and Ignition v6.0 simulators (see gif below). I will add a gravity compensator to the stick and check other things that might go wrong. We can reopen the Gazebo investigation if you find the configurations that caused the problems.

Nominal version

Bugged version

[rickstaa]

Gravity compensation investigation

Here I will add the results of my gravity compensation investigation. I will compare my long_stick.launch with the example provided by @gollth. While doing this I will check the following things.

• If there is a drift when we start the system without controllers. The simulation was run for 2 minutes before a trial was assumed to be drift-free.
• If a drift appears when we bring the joint into configurations and disable the control.

Reproduce gravity compensation bug

Check my example

To test this out in my example, we can use the following commands:

roslaunch gazebo_torque_calculation_bug long_stick.launch initial_joint_positions:="-J stick_joint1 $(calc 'deg2rad(-90)')"

or for the bugged case:

roslaunch gazebo_torque_calculation_bug long_stick.launch rotated:=true initial_joint_positions:="-J stick_joint1 $(calc 'deg2rad(90)')"

Results

Nominal

see plots

  

Bugged

see plots

  

• ✔️ No visible drift was observed in both the nominal and bugged case when the simulation was started. There is a small increase in the velocity of joint six but this increase is so small that it has no reasonable effect. I think it is due to some numerical instabilities in the simulator.
• ✔️ Both options can be slowly brought to all configurations without a drift being created when the controllers are disabled

There is a motion when a joint has non-zero velocity when the controller is enabled. This however is expected since the gravity compensator only compensates gravity.

Check Gollths example

To test it out in @gollth example, we can use the following commands:

roslaunch franka_gazebo panda.launch \
  use_gripper:=false \
  headless:=false \
  paused:=true \
  initial_joint_positions:="-J panda_joint6 $(calc 'deg2rad(-90)')"

or for the bugged case:

roslaunch franka_gazebo panda.launch \
  use_gripper:=false \
  headless:=false \
  paused:=true \
  rotated:=true \
  initial_joint_positions:="-J panda_joint6 $(calc 'deg2rad(90)')"

Results

Nominal

see plots

  

Bugged

Same behaviour the spikes below are because the joint reached its limit.

see plots

  

• ❌ A visual drift is observed for the nominal option when the simulation is started (see plots below). No starting drift is observed for the bugged case (see plots below). The velocity increases faster than in my example.
• ❌ Both options can be slowly brought to all configurations without a drift being created when the controllers are disabled

There is a motion when a joint has non-zero velocity when the controller is enabled. This however is expected since the gravity compensator only compensates gravity.

Conclusion

The gravity compensation you implemented contains a small bug that causes the joint to drift. I will continue my investigation into this bug in the next comment.

[rickstaa]

Gravity compensation bug investigation

This comment will show the results of my investigation into the bug that is found in the gravity compensation algorithm in your example:

Bug 1: Wrong gravity vector

The first obvious error has already been discussed above and was fixed when #177 was merged. In the old code, the hardcoded gravity vector was different from the actual gravity vector used by Gazebo. After applying #177 onto the example repository see check_gazebo_gravity_problem-fixed we got the following behaviour:

see figure

  

Conclusion

@gollth, @marcbone I concluded my investigation into the gravity compensator see https://github.com/frankaemika/franka_ros/issues/160#issuecomment-982682096, https://github.com/frankaemika/franka_ros/issues/160#issuecomment-982933376, https://github.com/frankaemika/franka_ros/issues/160#issuecomment-984520245 and https://github.com/frankaemika/franka_ros/issues/160#issuecomment-984604733.

In the end, I could not find any problems with the gravity controller other than the one that I already fixed in #177. I am therefore unsure why the panda robot is drifting when no controllers are enabled but it looks like the gravity compensator is working fine. I have the following hypothesis about why the robot might drift when no controllers are active:

• There are joint configurations that I did not test which still cause problems. @gollth can you please re-check to see which configurations gave you problems.
• Some joints in the robot have a non-zero velocity when the simulation is started. I saw this before in other robots where the gravity controller was started 2-time steps after the robot was launched into a running simulation. Here, since the gravity compensator is found in the FrankaHWSim::writeSim, this should not be the case.
  • [x] I will check to see if there is a non-zero velocity in any of the joints when the panda simulation is started.
• Wrong inertias, masses and joint frictions cause the model to be unstable.
  • [x] Let's first bring the stick joints into the same configuration as the actual panda when it is started.
  • [x] Following let's replace the stick joints with the actual panda joints one by one.

https://github.com/frankaemika/franka_ros/blob/17ea46264f8f982d00d859abe5058d564f9ae3ce/franka_gazebo/src/franka_hw_sim.cpp#L325

[rickstaa]

Investigate panda simulation stability

Results

Let's check several parameters to see if the Gazebo simulator is stable. To test it out, go to the rickstaa/franka_ros/check_gazebo_start_velocity build the catkin workspace and start the simulation without the hand:

roslaunch franka_gazebo panda.launch use_gripper:=false paused:=true

Then take one time in the simulation before plotting the following plots:

rosrun rqt_plot rqt_plot /total_energy

rosrun rqt_plot rqt_plot /total_kinetic_energy

rosrun rqt_plot rqt_plot total_potential_energy

 rosrun rqt_plot rqt_plot /joint_states/velocity[0] /joint_states/velocity[1] /joint_states/velocity[2] /joint_states/velocity[3] /joint_states/velocity[4] /joint_states/velocity[5] /joint_states/velocity[6]

rosrun rqt_plot rqt_plot /panda_joint1/error /panda_joint2/error /panda_joint3/error /panda_joint4/error /panda_joint5/error /panda_joint6/error /panda_joint7/error

 rosrun rqt_plot rqt_plot /velocity_norm

Plots

see plots

      

Conclusion

Although the total energy calculation below is not 100% correct (i.e. I did not account for the work done by the gravity compensator and energy that is lost due to joint friction), it allows us to check if significant energy is added to the system. When inspecting the velocity norm plots, we see that the energy and norm velocity of the system is not increasing over time. We can therefore conclude that the system as a whole is stable. We further see that the error between the gravity compensator and the gravity component is close to zero except where the robot is in a collision, meaning that the compensator works. However, when zooming in on the velocities, we see that the starting velocities are non-zero. It looks like they are responsible for the robot movement, since the gravity compensator only compensates for gravity. The interaction between the different joints probably causes the observed complex movements. The strange thing, however, is that these velocities are increasing.

Try to remove start velocities using joint trajectory controller

This does not work since the controller is imperfect, meaning an initial joint velocity causes the same result as above.

Disable gravity compensation

When the gravity compensation is disabled, the robot falls to the ground and stays as expected. We can see that the drift is only there when we have the gravity compensation enabled.

Effect of the initial position

Changing the initial position of the panda robot does not remove the drift.

[rickstaa]

Migrate stick to panda

Let's slowly transfer the stick into the panda robot. I will do this on the https://github.com/rickstaa/gazebo-torque-calculation-bug/tree/migrate_stick_to_panda branch. Each commit will represent a migration step. The first change is found in 12e4a432e7cf3374d4d1d63288ccf4236b1e3d2b. We will use the stick_long.launch launch file to do so:

roslaunch gazebo_torque_calculation_bug stick_long.launch paused:=true

While doing so, let's take one step in the simulation and plot the following plots:

rosrun rqt_plot rqt_plot /stick/total_energy

rosrun rqt_plot rqt_plot /stick/total_kinetic_energy

rosrun rqt_plot rqt_plot /stick/total_potential_energy

rosrun rqt_plot rqt_plot /stick/joint_states/velocity[0] /stick/joint_states/velocity[1] /stick/joint_states/velocity[2] /stick/joint_states/velocity[3] /stick/joint_states/velocity[4] /stick/joint_states/velocity[5] /stick/joint_states/velocity[6]

rosrun rqt_plot rqt_plot /stick/gazebo_bug/stick_joint1/error /stick/gazebo_bug/stick_joint2/error /stick/gazebo_bug/stick_joint3/error /stick/gazebo_bug/stick_joint4/error /stick/gazebo_bug/stick_joint5/error /stick/gazebo_bug/stick_joint6/error /stick/gazebo_bug/stick_joint7/error

Same configuration

ODE engine

Let's modify the long_stick urdf and launch files such that the initial configuration is the same as the one used in the panda simulation.

Start configuration

see figure


End configuration

see figure


plots

see plots

     

conclusion

The calculated gravity and effort error is close to zero but not entirely zero. This means that the gravity compensation works but has a slight numerical instability. Further, there also appears to be a minimal initial joint velocity for all the joints. The strange thing is that these velocities increase over time. We can also see that there is a slight increase in the total_energy. Maybe this is caused by numerical instabilities in the gravity compensator or Gazebo simulator? The effect of this is most prominent in joint 5.

Nevertheless, as these velocities are small, they take a long time to create a significant configuration change and should not cause any problems in experiments. Of course, these effects go away when the joint damping is increased. I, however, think that increasing the joint damping to fix (instabilities) in the simulation is not wise if it is not kept comparable to the joint damping used on the real robot. @gollth can you please check whether the joint damping you currently use is comparable with the joint damping in the real robot.

Dart engine

Let's try to use the dart physics engine by setting the physics:=dart launch file argument. To do this checkout 89ab71507f76ed646e908d516522f7943892b681 and run the following command:

roslaunch gazebo_torque_calculation_bug stick_long.launch paused:=true physics:=dart

plots

see plot


conclusion

Similar results to ode engine. The error is a bit bigger.

Same joint lengths/positions

Let's make the joint lengths and positions the same while keeping the old collision meshes, masses and inertias. This change is found in 7219024d0cce8f4f873d6a879e238668081321bf. We now have a rather strange robot, but let's look at it for testing sake.

start configuration

see figure


end configuration

see figure


plots

see plots

     

conclusion

Same meshes

start configuration

see figure


end configuration

see figure


plots

see plots

       

conclusion

It looks like adding the collision messes makes the velocity drift and gravity compensation error worse. The exciting thing is that it now also contains a lot of high-frequency signals. Could this be due to the interaction between the different collision meshes? Unfortunately, I think the drift we see after adding the meshes is not acceptable anymore for experiments. Some of the COMs are still outside the robot links so let's check if this drift improves when we adjust the mass positions.

Same mass position

start configuration

see figure


end configuration

see figure


plots

see plots

       

conclusion

The results are similar to when we added the meshes.

Same mass

end configuration

see figure


plots

see plots

       

conclusion

It looks like the velocity drift and gravity compensation error increased a bit while the initial velocity stayed the same. It, however, is tough to conclude since the differences are very small.

Same moment of inertia

The modified gazebo_ros_control/DefaultRobotHWSim does not seem to work with this configuration. The robot is very unstable now (see gif below):

It looks like there is a very big error when the robot reaches a certain configuration.

Same joint properties

Same behaviour as the previous configuration, the robot is still very unstable after a certain configuration has been reached.

[gollth]

Hello @rickstaa,

first of all thanks a lot for your great efforts you put into investigation of this bug. I didn't have yet the time to retrace your whole steps but definitely will in the near future.

Quick comment I noticed right away: I think there is a difference between Gazebo 9 & Gazebo 11. I tried the example from your fork on my machine (Ubuntu 18.04) and got different results than you (I guess you run 20.04?) after a few time steps:

▏ WARN▕ /gazebo ⟩ FrankaHWSim::writeSim() ⟩ L335 ⟫ panda_joint6: Error:                 0.250258
▏ WARN▕ /gazebo ⟩ FrankaHWSim::writeSim() ⟩ L336 ⟫ panda_joint6: Total Effort:          0.000003
▏ WARN▕ /gazebo ⟩ FrankaHWSim::writeSim() ⟩ L337 ⟫ panda_joint6: KDL GravComp:          0.250255
▏ WARN▕ /gazebo ⟩ FrankaHWSim::writeSim() ⟩ L338 ⟫ panda_joint6: KDL GravComp flipped:  -0.250255

Notice the error is not zero. I was trying on ROS Noetic (20.04) and there I get similar values to those you posted above.

I cannot reproduce the error between the KDL GravComp and Total Effort using your example

That might also explain why you couldn't reproduce my bugged case. Could you verify that?

[rickstaa]

@gollth Thanks for working with me on this bug! I suspected a version difference to be the cause of the reproducibility problem. I, however, did not have time to verify this since my experiments are in Gazebo 11. That is handy information since we can probably back-port the fix from Gazebo 11 to Gazebo 9. I will verify this today so that we can open a Gazebo issue.

In the meantime, I found the leading case of the resulting movement observed in Gazebo 11. It looks like two things cause it.

• There are small initial velocities when the simulation is started, causing the robot to drift. The effect of this can be mitigated by increasing the joint damping. I, however, think this is only wise if the joint damping stays close to the actual joint damping.
• Further, it looks like the collision meshes do not work nicely together and amplify this movement. When I add them to the robot, the drift is intensified.

I, however, am still performing some experiments before I can conclude. I will update https://github.com/frankaemika/franka_ros/issues/160#issuecomment-985620934 and let you know.

[rickstaa]

Investigate error blowup

Let's quickly investigate what goes wrong when we see the gravity compensation error explode. We investigate this using the fully migrated stick (see rickstaa/gazebo-torque-calculation-bug#29adee633458e29ee344f25b8690cb794d588acf).

Check the behaviour around the explosion

show plots

     Let's zoom in on the error a bit to see how fast the explosion goes:     ### Check initial error/velocity Also, let's check the initial error and velocity:   

After some seconds:

show plots

     

Conclusion

First, we see that the initial velocities of the joints are more prominent than before we added the moments of inertia. Further, the initial error looks slightly smaller. Finally, we see that around 20 seconds, the explosion happens. We see that from 19.9 s, the gravity compensation torque begins to converge from the actual joint effort. For most of the joints, this error decreases again after some time. However, for joint 4, the gravity controller oscillates and does not converge to the actual effort again. Maybe because of some singularity that is reached in the KDL calculation or a bug in my code? One configuration in which the gravity component from orocos KDL is wrong is found at 20.634 s:

position: [1.0239263566109518, 0.5960596715412674, -1.269196156791847, -1.3912509739159447, 0.6797047110628416, 2.132753824903997, 1.1042482208897306]

roslaunch gazebo_torque_calculation_bug stick_long.launch paused:=true initial_joint_positions:="-J stick_joint1 1.0239263566109518 -J stick_joint2 0.5960596715412674 -J stick_joint3 -1.269196156791847 -J stick_joint4 -1.3912509739159447 -J stick_joint5 0.6797047110628416 -J stick_joint6 2.132753824903997 -J stick_joint7 1.1042482208897306"

Investigate bad configuration

Let's try to start from the bad configuration:

roslaunch gazebo_torque_calculation_bug stick_long.launch paused:=true initial_joint_positions:="-J stick_joint1 0.629052096614803 -J stick_joint2 1.7260168916830132 -J stick_joint3 -2.431480455925368 -J stick_joint4 -0.4789728572990013 -J stick_joint5 1.4027805093682248 -J stick_joint6 2.352817784556791 -J stick_joint7 1.1476455418911584"

From this configuration, the error on joint4 starts to increase and becomes eventually unstable (see figure below):

Conclusion

Most of the joints do have a significant gravity compensation error when the robot is at the following configuration:

position: [0.09654970403330143, 1.5832436223876893, 1.8650816451372636, -1.101766637453536, 2.7164232834515225, 1.198539552336471, 1.0820332807641506]

However, the strange thing is that the error explosion does not occur when we start from this configuration. I'm not sure what is going on, but I can think of several possible causes:

• The KDL calculation contains a bug. Maybe some singularity is reached inside the calculation?
• Maybe my gravity calculation contains a bug.
• Maybe applying the gravity compensation at the joint limits is problematic.
• Maybe the parameters in the safety controller are not correct and energy is added to the system when the soft joint limits are reached.

Investigate the effect of safety_controller

Let's check if disabling the safety controller removes the strange gravity compensation error explosion problem. This can be done using the safety_controller:=false launch file argument:

roslaunch gazebo_torque_calculation_bug stick_long.launch paused:=true initial_joint_positions:="-J stick_joint1 1.0239263566109518 -J stick_joint2 0.5960596715412674 -J stick_joint3 -1.269196156791847 -J stick_joint4 -1.3912509739159447 -J stick_joint5 0.6797047110628416 -J stick_joint6 2.132753824903997 -J stick_joint7 1.1042482208897306" safety_controller:=false

show plots

     

Conclusion

As the unstable behaviour goes away when we disable the safety_controller, it looks like the unstable behaviour is caused by the interaction between the safety_controller and the gravity compensation. This conclusion is strengthened since the strange behaviours go away when we disable the gravity compensation.

Check the behaviour at the hard joint limits

Let's also quickly check if the strange error spikes coincide with the times when the hard joint_limits are reached. This can be done using the plot_joint_positions_and_limits.launch launch file. This launch file plots the joint positions and the joint limits. When we do this, we get the following plots:

show plots

             

Conclusion

It looks indeed that the error spikes are because the hard joint limits are reached.

End conclusion

The error explosion that was seen in my previous investigation is caused by the interaction between the hard joint limits, the safety_controller and the gravity compensation. I, therefore, think the gravity compensation should account for the instances when the soft or hard joint limits are being reached.

[rickstaa]

Compare different physics engine

Let's quickly try to use a different physics engine while launching the panda simulation. I added 4c7d16536b86102f849ad9d94ff9c4455b543882 onto the develop branch to do this (see rickstaa/franka_ros/develop-gazebo-bug-test). This commit adds the physics argument to the panda.launch file. Let's also check what happens to the starting drift when the safety_controller is disabled. I added 96d769b31d3edc087faf11a040f2aaffdb85bc1d to provide a way to turn the safety_controller on and off.

ODE

see result

**no gripper with the safety_controller** ```bash roslaunch franka_gazebo panda.launch use_gripper:=false physics:=ode ```  **no gripper without the safety_controller** ```bash roslaunch franka_gazebo panda.launch use_gripper:=false physics:=ode safety_controller:=false ```  **gripper with safety_controller**  **gripper without safety_controller** 

Conclusion

There appears to be a large starting drift both with and without the safety controller enabled.

DART

see result

**no gripper with the safety_controller**  **no gripper without the safety_controller**  **gripper with safety_controller**  **gripper without safety_controller** 

Conclusion

The simulation now is a lot more stable. There still is a slight starting drift, but it is significantly reduced by using the Dart engine. After 3 minutes, the arm will have lowered a bit because joint four has drifted:

see result


Bullet

see result


Simbody

see result


Conclusion

It looks like the drift is less significant when we use the dart physics engine compared to the ODE physics engine. Disabling the safety controller does not affect this starting drift. Most drift is introduced when the hand is added to the robot.

[rickstaa]

@gollth, @marcbone I concluded my investigation into the motion drift (i.e. for Gazebo 11); you can check the comments above. I added the plots and figures under a collapsable <details> section to remove noise. I summarized my results below:

Conclusions

• There appears to be a small startup and command delay for the effort coming from Gazebo (see https://github.com/frankaemika/franka_ros/issues/160#issuecomment-982933376). This delay only seems to be present when no effort control commands are sent. It becomes neglectable when the gravity compensation is enabled and should not be a problem.
• Gazebo appears to have a slight joint drift when your 7DOF stick is loaded. This drift is likely due to numerical instabilities in the simulator or the orocos KDL library but is very small and should therefore not significantly influence simulation experiments in which the robot is controlled (see https://github.com/frankaemika/franka_ros/issues/160#issuecomment-985620934). Further, this drift can be removed by increasing the joint damping, but this should only be done if it is kept close to the real robot.
• Both the original long stick and the fully migrated stick have non-zero joint velocities when the Gazebo simulation is started. Altough these velocities are small, I don't think they need to be there. I have not yet found what causes these initial velocities (https://github.com/frankaemika/franka_ros/issues/160#issuecomment-985593607).
• The robot simulation shows some very unstable behaviours when it reaches the soft joint limits. This is probably caused by the interaction between the gravity compensator and the safety_controller. I, therefore, think the gravity compensation should account for the instances when the soft joint limits are reached (see https://github.com/frankaemika/franka_ros/issues/160#issuecomment-988621107).
• Another error in the gravity compensation is that it does not account for the hard joint limits (see https://github.com/frankaemika/franka_ros/issues/160#issuecomment-988621107). This creates an impulse when these joint limits are reached.
• It looks that your gravity compensation is not taking into account the physical properties of the attached hand (see https://github.com/frankaemika/franka_ros/issues/160#issuecomment-992776684). This should be fixed to use the panda simulation with the hand attached.
• The panda Gazebo simulation seems to have less drift when using the DART physics engine (https://github.com/frankaemika/franka_ros/issues/160#issuecomment-989173918).
• The starting drift is only significant in the DART simulator when the hand is attached (See https://github.com/frankaemika/franka_ros/issues/160#issuecomment-989173918).

Next steps

The starting drift can be reduced by switching to the DART physics engine from the conclusions above. Changing the ODE parameters might yield a similar result. After this is done, the drift that is still left when the hand is not attached can be removed by taking the soft and hard joint limits into account when applying for gravity compensation. This can be done by adding a simple check before applying for the gravity compensation. The drift experienced with the attached hand is caused by the gravity compensation bot taking into account the physical properties of the hand.

I will leave these changes up to you since there are many ways to implement this. I think solving this will significantly improve the gravity compensation.

Another thing I noticed while migrating from the stick to the panda was that the drift becomes problematic when the collision geometries are added. I did not yet find why this is the case. The drift is too big to be caused by the small error found between the KDL gravity component calculation. Maybe the drift can also be reduced by improving the link, joint and physics engine properties?

Since I have been working on this issue for a long time, I think a fresh pair of eyes is needed to verify my conclusions. I am looking forward to hearing your take on this problem. Feel free to let me know if anything is unclear or needs clarification.

[rickstaa]

@gollth I used the singularity recipe below to verify that the bug is present in Gazebo 9. The only result that is different from your initial example is that Gazebo's effort is 0.0. My result, however, is similar to the values shown in https://github.com/frankaemika/franka_ros/issues/160#issuecomment-987682342. Nevertheless, in the new example, Gazebo's torque and joint effort are incorrect. Maybe you can open an issue on osrf/gazebo_models repository if you Franka also wants to support ROS melodic?

My result

I get the following result when using:

roslaunch franka_gazebo panda.launch   use_gripper:=false   headless:=false   paused:=true   rotated:=true initial_joint_positions:="-J panda_joint6 1.5707963267948966"

[ WARN] [1639081235.413943968, 2.107000000]: panda_joint6: Error:                 0.250003
[ WARN] [1639081235.413970640, 2.107000000]: panda_joint6: Total Effort:          0.000003
[ WARN] [1639081235.414014090, 2.107000000]: panda_joint6: KDL GravComp:          0.250000
[ WARN] [1639081235.414040982, 2.107000000]: panda_joint6: KDL GravComp flipped:  -0.250000
[ WARN] [1639081235.414069299, 2.107000000]: panda_joint6: Gazebo body2Torque:   [0.000007, -0.500000, 0.000003]
[ WARN] [1639081235.414095318, 2.107000000]: panda_joint6: Desired joint command: 0.000000
[ WARN] [1639081235.414120661, 2.107000000]: panda_joint6: Joint command:         0.250000
[ WARN] [1639081235.414149246, 2.107000000]: panda_joint6: Global Axis: [-0.000001, -1.000000, -0.000001]
[ WARN] [1639081235.414176269, 2.107000000]: panda_joint6: Local Axis:  [0.000000, -1.000000, 0.000000]
[ WARN] [1639081235.414204518, 2.107000000]: panda_joint6: URDF Axis:   [0, 0, 1]
[ WARN] [1639081235.414230392, 2.107000000]: panda_joint6: Joint velocity:        -0.000009

Singularity recipe

I used the following recipe to build my ubuntu 18.04 container. For more information see https://sylabs.io/guides/latest/user-guide. Please make sure you use the --nv flag and added your user name to the xhost control access list when you can not open GUI applications like RVIZ and Gazebo (see apptainer/singularity#3800).

# Ubuntu 18.04 - Ros melodic Franka gazebo gravity compensation test container
Bootstrap: docker
From: osrf/ros:melodic-desktop-full

%help
    A small container used for testing the problems explained in
    https://github.com/frankaemika/franka_ros/issues/160#issuecomment-961780423.
%labels
    Maintainer: Rick Staa
    Github: https://github.com/frankaemika/franka_ros/issues/160#issuecomment-961780423
    Version: v0.0.0
    Type: Public
%environment
    # Set language options
    export LANG=C.UTF-8 LC_ALL=C.UTF-8

    # Set ROS vars
    export ROS_DISTRO=melodic
%post
    # Install extra system package dependencies
    apt update --fix-missing
    apt install -q -y git

    # Install ros build tools
    apt install -q -y python-catkin-tools

    # Install libfranka from source
    # NOTE: Uncomment if you want to install from source
    apt update
    apt install -q -y \
       cmake \
       libpoco-dev \
       libeigen3-dev
    bash -c "cd / \
       && apt remove -y \"*libfranka*\" \
       && git clone --recursive https://github.com/frankaemika/libfranka.git \
       && cd libfranka \
       && mkdir build \
       && cd build \
       && cmake -DCMAKE_BUILD_TYPE=Release .. \
       && cmake --build ."

    # Build the gazebo gravity compensation test catkin workspace
    mkdir -p /gazebo_gravity_comp_test_ws; cd /gazebo_gravity_comp_test_ws
    git clone -b melodic-devel --recursive https://github.com/rickstaa/gazebo-torque-calculation-bug.git src
    rosdep install --from-paths src --ignore-src --rosdistro melodic -y --skip-keys libfranka
    . /opt/ros/melodic/setup.sh; catkin build -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=/libfranka/build

[rickstaa]

@golth, @marco I just updated my conclusion since I noticed that the drift I removed entirely when the safety_controller is disabled in the DART physics engine. I now have a clear picture of what caused this drift and how we can solve it.

[rickstaa]

@gollth, @marcbone The first part of the solution is implemented in #211.

[rickstaa]

@gollth, @marcbone I just noticed that I made an error while looking at the safety controller. It is not the safety controller that causes the starting drift but the addition of the hand. It however does cause unstable behaviour (see https://github.com/frankaemika/franka_ros/issues/160#issuecomment-988621107).

I updated https://github.com/frankaemika/franka_ros/issues/160#issuecomment-989175979 so that it reflects the right conclusion.

[rickstaa]

@gollth, @marcbone, I think I found the cause of the drift experienced when the hand is attached. It looks that your gravity compensation is not taking into account the physical properties of the attached hand:

https://github.com/frankaemika/franka_ros/blob/cbe64f9e9499dcb192c152f5a6a9473fefc1ff91/franka_gazebo/src/model_kdl.cpp#L277

The currently used KDL::Chain starts from panda_link0 and ends at panda_link8. Can you verify that this is indeed the case?

I did not create a pull request since it requires me to rewrite a significant part of your codebase. I, therefore, first wanted to discuss the issue with you.

I updated https://github.com/frankaemika/franka_ros/issues/160#issuecomment-989175979 to include the new cause. To summarize, the causes I found for the drift, ordered from most serious to less severe, are:

• The hand physical properties are not considered when performing the gravity compensation (cause a starting drift when the hand is attached).
• Physics engine inaccuracies (i.e. most minor in the dart physics engine).
• The soft and hard limits are not considered when performing the gravity compensation (lead to instabilities).
• Other model physical property inaccuracies.
• Other numerical instabilities.

[rickstaa]

@gollth, @marcbone sorry for the enormous amount of information, but I just noticed I forgot something in my previous conclusion https://github.com/frankaemika/franka_ros/issues/160#issuecomment-992776684. It did not include the fact that although the starting drift is not caused by the safety_controller it does cause some unstable behaviour at the soft joint limits. I hope I have given you a good picture of all the problems that are currently present in the gravity compensation. I will leave the further investigation to you now. Please let me know when things are unclear.

[gollth]

It looks that your gravity compensation is not taking into account the physical properties of the attached hand:

Yes it does (kind of), directly with the line after your snippets:

https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/src/model_kdl.cpp#L277-L278

This appends a load frame to the end of the chain. This load frame is defined by the mass of the EE (m_total) and the CoM (F_x_Ctotal) which both come ultimately from the URDF (unless the user sets custom values via ROS parameters):

https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/src/franka_hw_sim.cpp#L440-L455

Although you are right, the inertia tensor of this augmented link is still identity

[gollth]

Thanks for all your investigation @rickstaa

[rickstaa]

@gollth Thanks a lot for your response. You are right thanks for clarifying that! Based on the naming (i.e. load), I assumed that the augmented frame only accounted for the external load. I quickly checked the code, and overall, it looks correct. I, however, have some concerns about the current implementation:

• First, it looks like your not getting the hand and finger masses from the URDF (see franka_hw_sim.cpp#L392-L452). Instead, you set the m_ee parameter in the panda.launch file. It would make more sense to remove this line and get the mass of both the fingers and hand directly from the URDF.

https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/launch/panda.launch#L54

• Second, when the m_ee parameter is not supplied, you only retrieve the mass of the hand and ignore the mass of the fingers:

https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/src/franka_hw_sim.cpp#L421

• Same for the inertias and COM's:

https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/src/franka_hw_sim.cpp#L431-L434

https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/src/franka_hw_sim.cpp#L445-L447

I think adding the fingers to the chain might be more correct. I, however, am not sure if KDL allows for parallel links in a chain. I vaguely remember it does not, but I haven't looked at it for a while. If adding parallel links is not allowed, we could use a single link that contains the combined COM and inertia matrix of both the hand and the two fingers. Lastly

• Lastly, it looks that you are using the initialized value of F_x_Ctotal (i.e. 0, 0, 0) and do not update it anywhere in the code. Maybe this is as intended, or I overlooked the part where you set this parameter, but I would have expected it to be equal to the hand and arm's combined COM position.

https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/src/franka_hw_sim.cpp#L315

https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/src/model_kdl.cpp#L271

I hope we can find the cause of this drift. Please let me know if you need help from my side.

[rickstaa]

@gollth I created an issue on the gazebo repository for the drift difference that is observed between the ODE and DART physics engine (see https://github.com/osrf/gazebo/issues/3149). Did you already create an issue for the ROS melodic torque calculation problem? In my issue, I quickly mention the melodic torque problem but I currently link it to https://github.com/frankaemika/franka_ros/issues/160#issuecomment-987682342.

[justagist]

Although franka_hw_sim_plugin handles the gravity compensation for the arm, there is no gravity compensation in franka_gripper_sim. This causes the gripper commands to not work correctly when the gripper fingers are not in the same horizontal plane. I had to completely disable gravity in the simulation to make the gripper work in such configurations.

[rickstaa]

@gollth, @marcbone do you have any updates on what goes wrong with the gravity compensation when the gripper is attached?

I think using the DART simulator is a good workaround for now when the gripper is not attached. I can successfully train my RL algorithm using the DART simulator as the gravity compensation seems stable. I am unsure what causes the drift when the ODE physics engine is used. I did not have time to dive deeper into the Gazebo codebase, and the gazebo team has not yet responded to the issue I opened about this drift. I, therefore, think for now it might be best to merge https://github.com/frankaemika/franka_ros/pull/211 since this gives users the ability to use the DART physics engine when using effort control.

I, however, found one problem when using the DART physics engine. See the section below. Nevertheless, the gravity compensation does not seem to work both in the ODE and DART physics engine, the panda robot keeps drifting, so I think this should be fixed first.

Grasp action DART crash

When the gripper is attached and the franka_gripper/grasp/goal, franka_gripper/move/goal or franka_gripper/gripper_action/goal actions are used the simulator sometimes crashes with the following error message:

Show error message

```bash Error [BoxedLcpConstraintSolver.cpp:291] [BoxedLcpConstraintSolver] The solution of LCP includes NAN values: -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan nan nan nan nan nan nan nan -nan -nan. We're setting it zero for safety. Consider using more robust solver such as PGS as a secondary solver. If this happens even with PGS solver, please report this as a bug. [ WARN] [1643023521.425695656, 90.305000000]: Command for panda_joint1is NaN, won't send to robot [ WARN] [1643023521.425777956, 90.305000000]: Command for panda_joint2is NaN, won't send to robot [ WARN] [1643023521.425795056, 90.305000000]: Command for panda_joint3is NaN, won't send to robot [ WARN] [1643023521.425808456, 90.305000000]: Command for panda_joint4is NaN, won't send to robot [ WARN] [1643023521.425855257, 90.305000000]: Command for panda_joint5is NaN, won't send to robot [ WARN] [1643023521.426084258, 90.305000000]: Command for panda_joint6is NaN, won't send to robot [ WARN] [1643023521.426113458, 90.305000000]: Command for panda_joint7is NaN, won't send to robot [ WARN] [1643023522.488360928, 90.307000000]: Command for panda_joint1is NaN, won't send to robot [ WARN] [1643023522.488449328, 90.307000000]: Command for panda_joint2is NaN, won't send to robot [ WARN] [1643023522.488477629, 90.307000000]: Command for panda_joint3is NaN, won't send to robot [ WARN] [1643023522.488506729, 90.307000000]: Command for panda_joint4is NaN, won't send to robot [ WARN] [1643023522.488532129, 90.307000000]: Command for panda_joint5is NaN, won't send to robot [ WARN] [1643023522.488556229, 90.307000000]: Command for panda_joint6is NaN, won't send to robot [ WARN] [1643023522.488580929, 90.307000000]: Command for panda_joint7is NaN, won't send to robot [ WARN] [1643023523.534044389, 90.307000000]: Command for panda_joint1is NaN, won't send to robot [ WARN] [1643023523.534135489, 90.307000000]: Command for panda_joint2is NaN, won't send to robot [ WARN] [1643023523.534164089, 90.307000000]: Command for panda_joint3is NaN, won't send to robot [ WARN] [1643023523.534194190, 90.307000000]: Command for panda_joint4is NaN, won't send to robot [ WARN] [1643023523.534219690, 90.307000000]: Command for panda_joint5is NaN, won't send to robot [ WARN] [1643023523.534244590, 90.307000000]: Command for panda_joint6is NaN, won't send to robot [ WARN] [1643023523.534268890, 90.307000000]: Command for panda_joint7is NaN, won't send to robot Segmentation fault (core dumped) franka_gripper_sim_test: /usr/include/boost/smart_ptr/shared_ptr.hpp:734: typename boost::detail::sp_member_access::type boost::shared_ptr::operator->() const [with T = const sensor_msgs::JointState_ >; typename boost::detail::sp_member_access::type = const sensor_msgs::JointState_ >*]: Assertion `px != 0' failed. ```

See the problem in action

**Branch used for the test:** https://github.com/rickstaa/franka_ros/tree/dart_engine_tests  

Funny enough, this issue showed that the new tests you introduced are working (see https://github.com/frankaemika/franka_ros/runs/4920586653?check_suite_focus=true) 🚀 .

This only seems to happen when the gripper is controlled to the fully closed and fully opened position (i.e. 0.0 and 0.08). I, therefore, think this is caused by a bug or the instability of the gripper control logic, which causes some numbers to become singular. It might be closely related to the problems I pointed out in https://github.com/frankaemika/franka_ros/pull/173#issuecomment-979223037. However, I did not yet investigate since the gravity compensator for when the gripper is attached is not working yet, which might also cause the crash. However, this hypothesis is strengthened since it does not seem to happen when I stabilize the panda arm by putting it down on the ground (see figure below). I know that getting the gripper to work in simulation is not easy, but I just wanted to make you aware of all the problems I found.

See stabilized position


[rickstaa]

Although franka_hw_sim_plugin handles the gravity compensation for the arm, there is no gravity compensation in franka_gripper_sim. This causes the gripper commands to not work correctly when the gripper fingers are not in the same horizontal plane. I had to completely disable gravity in the simulation to make the gripper work in such configurations.

@justagist Thanks for your comment! @marcbone I think this is closely related to my comment in https://github.com/frankaemika/franka_ros/pull/173#issuecomment-979223037. The gripper PID gains might be easier to tune when the gravity of the fingers is also compensated! I however do not think that this is the major cause of the drift we observe when the hand is attached when using the DART physics engine (see figure below).

See gif


[gollth]

For reference, the movement of the joints should now be gone also in Melodic with the new feature from @rickstaa & @marcbone: https://github.com/frankaemika/franka_ros/pull/181#issuecomment-1064120809 (although it does only mask the underlying real problem in Gazebo).

@rickstaa what do you think, shall we close this ticket or do you want to keep it open for reference?