Controller Tuning for Manipulators Using ft_sensor plugin

[crvogt]

To run:
roslaunch dave_demo_launch dave_manipulator_spring_demo.launch
roslaunch dave_demo_launch dave_manipulator_spring_plot.launch

In the below example, the plotting function is showing the shoulder joint.

Observed Issue:

Oberon7 arm with ft_sensor shows what appears to be noise when plotting joint torques during manipulator operation.

Rexrov with Oberon7	Shoulder Z torque when moving

Without Joint Controller

By removing the joint controller, we see that the ft_sensor generates less "noisy" data and responds reasonably to contact with the environment. However, there are edge cases.

Rexrov with Oberon7 and no controller

By moving the rexrov up and down we see the arm's shoulder torque changing in correspondence. As well, there is some high frequency oscillation that only occurs when the ROV is not moving. The working theory is that it is caused by the arm bouncing off of the ROV body as there is no controller force to hold it up.

Controller Tuning

We find PID values related to the control of the oberon7 in two locations: oberon7_control/launch/joint_control.launch and oberon7_control/launch/joint_effort_controllers.launch

Tuning attempts are ongoing. Initial iterations are based on changing PID values for each joint until oscillations discipate, then moving on to the next joint.

[crvogt]

Commands corresponding to each joint do not reflect what is echoed in the ft_sensor_(joint) topic. Ie, the commands don't seem to be causing the issues as they appear relatively smooth coming from joint_position_controller.py.

I thought it could be a sampling issue with rqt_plot, but echoing rexrov/tf_sensor_(joint)/wrench/torque/* does, as expected, show the observed oscillations.

Currently, I do not think this is a controller tuning issue. Leading theories are:

• collision issue
• joint force implementation issue (not sure what is generating the actual joint forces).

[crvogt]

I changed the joint limits and home pose for the Oberon7 based on detected collisions in the MoveIt setup assistant.No changes to behavior were observed.

Inertia values were also changed a la https://answers.gazebosim.org//question/18895/the-data-are-noisy-when-using-libgazebo_ros_ft_sensorso/ . Values were increased and decreased by an order of magnitude in an attempt to evoke a visibile response, but there were no obvious differences.

[mabelzhang]

I think I'm able to reproduce it. It only happens when the joystick is attached and used, right?

May be related to an existing upstream issue gazebosim/gazebo-classic#2477
Looks like they've tried the inertia tweak and didn't see an effect. They've also considered whether it's noise.
They're using velocity control. They’ve also tried removing the joint controller.

Additional comments link to these posts
https://answers.gazebosim.org//question/19323/gazebo-70-recommended-way-to-get-torque-applied-to-a-joint-by-the-environment-joint-effort-feedback-via-gazebo-api/
https://answers.gazebosim.org//question/1523/drcsim-simulated-force-measurements-in-wrong-coordinates/#1952

One difference is that uuv_manipulators/oberon7/oberon7_control/launch/joint_effort_controllers.launch is already using effort control, which is the recommended way for physics to respond smoothly (as opposed to velocity or position control).

If it's an upstream issue, I don't know how easy it is to fix. It might be that it hasn't been fixed because it's difficult, or because no one internally has needed it. In the latter case, this can be a good enough reason to fix it.

How important are the spikes? Are you relying on them for feedback control?

[crvogt]

Further testing with ros industrial and even the simple rrbot example are showing the same issues with the ft_sensor plugin.

RRBot with plotted joint torques

Unless we're putting a PR in to ROS or Gazebo, I'm going to close this for the time being.

[scpeters]

I opened the gazebo_ros_ft_sensor_demo.world from gazebo_plugins, and I was able to observe significant noise in the force-torque measurement of that double pendulum. My first thought is that this is connected to issues with convergence in the ODE constraint solver, since the default "quick" solver uses an iterative Projected Gauss-Seidel algorithm. I tried switching to the world solver and saw much less noise in the force-torque sensor data. Note that the world solver uses a pivoting algorithm and is generally more accurate, but has poor behavior when the constraint solver does not find a good solution, which is why it is not the default.

Since that demo world is strictly a passive model with no feedback control, I suspect it is not related to gazebosim/gazebo-classic#2477, but it does raise the point that it's important for the physics to be properly tuned. I will test further with the dave_manipulator_spring launch files

[scpeters]

I've recorded screen captures of gazebo_ros_ft_sensor_demo.world showing the reduction in force-torque noise when using the world solver in ros-simulation/gazebo_ros_pkgs#1354

I will try using this solver parameter with the underwater manipulator world to see if it improves that scenario as well.

[scpeters]

One thing to watch out for with the world-step solver is an excessive number of console error messages that include the phrase LCP internal error. We added an ode_quiet parameter in Bitbucket pull request 2512 gazebosim/gazebo-classic@dacd915 that is typically set with an API call from a plugin:

• https://github.com/osrf/gazebo/blob/dacd9151a198562061ce013d0a70776f821a6ea7/test/integration/joint_revolute.cc#L180

[scpeters]

I just loaded this again, and I noticed some console errors during roslaunch that I didn't have all the run-time dependencies installed, and it was complaining that some effort controllers could not be loaded. I resolved that with rosdep install -i -y --rosdistro noetic --from-paths src in my workspace. Now I'm not seeing the noise issues that you were mentioning.

I've also tried using the world-step solver, and it seems to work as well:

diff --git a/models/dave_worlds/worlds/dave_ocean_waves.world b/models/dave_worlds/worlds/dave_ocean_waves.world
index ec56db4..c2ebcb3 100644
--- a/models/dave_worlds/worlds/dave_ocean_waves.world
+++ b/models/dave_worlds/worlds/dave_ocean_waves.world
@@ -22,6 +22,11 @@
       <max_step_size>0.002</max_step_size>
       <real_time_factor>1</real_time_factor>
       <real_time_update_rate>500</real_time_update_rate>
+      <ode>
+        <solver>
+          <type>world</type>
+        </solver>
+      </ode>
     </physics>
     <scene>
       <ambient>0.01 0.01 0.01 1.0</ambient>

[scpeters]

I just loaded this again, and I noticed some console errors during roslaunch that I didn't have all the run-time dependencies installed, and it was complaining that some effort controllers could not be loaded. I resolved that with rosdep install -i -y --rosdistro noetic --from-paths src in my workspace. Now I'm not seeing the noise issues that you were mentioning.

I've also tried using the world-step solver, and it seems to work as well:

diff --git a/models/dave_worlds/worlds/dave_ocean_waves.world b/models/dave_worlds/worlds/dave_ocean_waves.world
index ec56db4..c2ebcb3 100644
--- a/models/dave_worlds/worlds/dave_ocean_waves.world
+++ b/models/dave_worlds/worlds/dave_ocean_waves.world
@@ -22,6 +22,11 @@
       <max_step_size>0.002</max_step_size>
       <real_time_factor>1</real_time_factor>
       <real_time_update_rate>500</real_time_update_rate>
+      <ode>
+        <solver>
+          <type>world</type>
+        </solver>
+      </ode>
     </physics>
     <scene>
       <ambient>0.01 0.01 0.01 1.0</ambient>

no sooner did I type this than I started seeing them the next time I launched the simulation. I am still investigating

[mabelzhang]

Side note:
rosdep error probably related to this #188
Although, that's for a host machine install - was that your setup? If so, that would be resolved in that ticket. Two packages were missing from our host installation tutorial.
If you are using dockwater and had that rosdep problem, I want to know. We need to fix that.

[scpeters]

it was on my host machine setup. I was having too much trouble with dockwater so I have just been using it on my host machine for now

[mabelzhang]

That makes sense. I think it should be resolved now for new host installations.

[scpeters]

I've merged a pull request in gazebo that makes it possible to plot joint velocities for models that use / in joint names and have recorded screen captures of the improvements provided by the world-step solver in the following pull request: #236

[robotsorcerer]

Hi guys,

Not sure if this got fixed on the overall. For static bodies, I found that ft_sensor readings were really tight. But once I start sending position commands to the robot, I found that the ft_sensor readings were pretty much unreliable.

Is there a PR or some fix for this since you last observed this, @scpeters @mabelzhang?

[mabelzhang]

I believe we found changing to the world solver, as in #236, was sufficient?