Gravity compensation

[captain-yoshi]

Problem description

An alternative is to use the force controller and move the robot by hand. If it starts drifting after rotation, then the compensation does not yet work. If it drifts only a little, but otherwise seems to improve things, then maybe the specification of the weight is not accurate enough.

This is a continuation of #73 regarding gravity compensation. Starting with the force_controller, I have adjusted the pid gains for rotation to 1.0. This produces the effect seen on the video below (the tcp starts to rotate as falling !). With the previous default gains 0.01, the robot does not move, but trying to force a rotation by moving by hand does not seem to work. Moving by hand for translation works as expectedt. As you said in a previous commit, maybe the default values where too low to have a significant effect.

Should I try adjusting the pid gains for rotation to be stable, or this could be the effect of the gravity compensation which may be wrong as #39 seems to suggest. In the mean time I will try the #91 PR.

out.mp4

Software versions

• Commit: a115bfd
• OS: Ubuntu 20.04
• ROS version: ROS Noetic
• Robot: UR3 (CB3)

To Reproduce
My controller config

my_cartesian_force_controller:
  type: "position_controllers/CartesianForceController"
  robot_base_link: "ur3_base_link"
  end_effector_link: "robotiq_2f140_tcp"
  #end_effector_link: "pipette_tcp"
  ft_sensor_ref_link: "robotiq_ft300_frame_id"
  joints: *robot_joints

  solver:
    error_scale: 0.5

  pd_gains:
    trans_x: {p: 0.05}
    trans_y: {p: 0.05}
    trans_z: {p: 0.05}
    rot_x: {p: 1}  # worked with previous default values of 0.01
    rot_y: {p: 1}
    rot_z: {p: 1}

  # Gravity should point towards the center of earth.
  # In robot_base_link.
  gravity:
    x: 0
    y: 0
    z: -9.81

  tool:
    # Mass of everything that's mounted after the sensor.
    mass: 2.23

    # The center of mass in the ft_sensor_ref_link.
    # Take a measure and try to estimate that as good as possible.
    com_x: -0.036
    com_y: 0.0
    com_z: 0.088

and how I initialise and tare the signal.

constexpr char CONTROLLER_TYPE_STR[] = "force";  // force or compliance controller
constexpr double ERROR_SCALE = 0.5;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "dummy");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  ros::NodeHandle nh;

  // initialization
  std::string cartesian_ctrl_name = "my_cartesian_" + std::string(CONTROLLER_TYPE_STR) + "_controller";
  std::string ctrl_signal_taring_topic = "/" + cartesian_ctrl_name + "/signal_taring";
  std::string ctrl_solver_topic = "/" + cartesian_ctrl_name + "/solver";

  ros::ServiceClient sensor_client =
      nh.serviceClient<robotiq_ft_sensor::sensor_accessor>("/robotiq_ft_sensor/robotiq_ft_sensor_acc");
  ros::ServiceClient controller_client = nh.serviceClient<std_srvs::Trigger>(ctrl_signal_taring_topic);
  ros::ServiceClient ctrl_manager_client =
      nh.serviceClient<controller_manager_msgs::SwitchController>("/controller_manager/switch_controller");

  // dynamic reconfigure
  dynamic_reconfigure::Client<ControllerConfig> dyn_reconf_ctrl_client(ctrl_solver_topic);

  sensor_client.waitForExistence(ros::Duration(1));
  controller_client.waitForExistence(ros::Duration(1));
  ctrl_manager_client.waitForExistence(ros::Duration(1));

  // reset fts
  robotiq_ft_sensor::sensor_accessor req;
  robotiq_ft_sensor::sensor_accessor resp;
  req.request.command_id = req.request.COMMAND_SET_ZERO;

  if (!sensor_client.call(req) && !req.response.success)
  {
    ROS_ERROR("Failed to reset sensor");
    return 0;
  }

  // reconfigure the cartesian controller
  ControllerConfig ctrl_dyn_config;
  ctrl_dyn_config.error_scale = ERROR_SCALE;

  if (!dyn_reconf_ctrl_client.setConfiguration(ctrl_dyn_config))
  {
    ROS_ERROR("Failed to reconfigure the cartesian %s configuration", CONTROLLER_TYPE_STR);
    return 0;
  }

  // tare signal from force controller
  std_srvs::Trigger trigger;
  if (!controller_client.call(trigger) && !trigger.response.success)
  {
    ROS_ERROR("Failed to tare signal from %s controller", CONTROLLER_TYPE_STR);
    return 0;
  }

  // start force controller
  controller_manager_msgs::SwitchController switch_ctrl;
  switch_ctrl.request.start_controllers = { cartesian_ctrl_name };
  switch_ctrl.request.timeout = 1.0;
  switch_ctrl.request.strictness = switch_ctrl.request.STRICT;

  if (!ctrl_manager_client.call(switch_ctrl) && !switch_ctrl.response.ok)
  {
    ROS_ERROR("Failed to start the %s controller '%s'", CONTROLLER_TYPE_STR,
              switch_ctrl.request.stop_controllers[0].c_str());
    return 0;
  }

  ROS_INFO("Successfully started !");
  ros::waitForShutdown();
}

[captain-yoshi]

Testing with #91, the falling seems to have stopped.

Here you can view the video where the controller seems to be in equilibrium at the start and then I try to rotate the tcp with my hand. Based on the video, do you think the rotational pid gains are too high for my setup ?

Will have to do more tests, but the PR seems promising.

[captain-yoshi]

Another question : If I tune my force controller with some parameters, does the same parameters can be applied to the compliance controller as is? E.g the compliance should be in equilibrium given the same parameters (scale error, pid gains) and with no target frame.

[stefanscherzinger]

@captain-yoshi

Thanks for continuously dedicating time to this!

Should I try adjusting the pid gains for rotation to be stable, or this could be the effect of the gravity compensation which may be wrong as #39 seems to suggest.

In general, very low rotational gains may hide the effect of incorrect gravity compensation for some time, but eventually, the robot will drift if not balanced exactly in all rotated poses. Ideally, the controller's stability should be somewhat disjoint from the gravity thing, but I'm not sure at the moment.

Concerning the videos, what happens in test-fzi-master.mp4 at around 1:10 when the robot suddenly drifts over from its equilibrium condition? Did you trigger something special? It does not seem as if this was a steadily increasing drift, but seems more like a sudden thing.

In the second video test-kkstb-master.mp4, the arm seems to behave a little strange, especially the sudden movement at 0:50 is not normal. I can't say right now if that's because of too high rotational gains or something with the gravity compensation. The controller gains don't seem too high, though. When you grab the whole Robotiq gripper, does moving the robot by hand still feel natural? If the gravity compensation worked, you shouldn't feel any drift while rotating the robot by hand.

I'll see if I can setup an integration test for this. That's probably the best approach to check mechanical plausibility in a systematic way.

---

Another question : If I tune my force controller with some parameters, does the same parameters can be applied to the compliance controller as is?

Mostly, yes. The default suggestions are the same. The compliance controller has the additional stiffness to parameterize. When that is zero, the compliance controller should move as fast as would the force controller with the same controller gains. Increasing the stiffness will, of course, generate the offset-proportional restoring forces, but the controller's sensitivity should be the same.

[captain-yoshi]

Thanks for continuously dedicating time to this!

Your controller is really good !

Concerning the videos, what happens in test-fzi-master.mp4 at around 1:10 when the robot suddenly drifts over from its equilibrium condition? Did you trigger something special? It does not seem as if this was a steadily increasing drift, but seems more like a sudden thing.

I forgot to crop the video to remove the waiting time. At around 1:10, this is where I start the script which starts the force controller. Yup it's a sudden thing. Did not happen before because of the low rotational gains of 0.01.

In the second video test-kkstb-master.mp4, the arm seems to behave a little strange, especially the sudden movement at 0:50 is not normal. I can't say right now if that's because of too high rotational gains or something with the gravity compensation.

Yeah I'm not sure either, it could also be that the robotiq FTS sensed high torques. I will try to replicate and save the FTS data. Not sure if their were a lot of drift caused by me FTS...

When you grab the whole Robotiq gripper, does moving the robot by hand still feel natural? If the gravity compensation worked, you shouldn't feel any drift while rotating the robot by hand.

It feels natural when moving but I will recheck to confirm this.

I will also recheck my COG parameters. Seems fine with the UR free drive mode but they might compensate up to a certain tolerance...

[captain-yoshi]

@stefanscherzinger

This is related to the COM parameters for the cartesian controller.

The center of mass in the ft_sensor_ref_link.

The parameters should be referenced from the FTS frame given by the product manual, e.g for the robotiq ft300 which is located at the end of the tool side.

Mass of everything that's mounted after the sensor.

The mass of everything that is mounting after the sensor or the sensor frame ? So my sensor of 300g should not be added in the COM mass. Also should I recalculate the COM (com_x, com_y and com_z) without the FTS ?

Theoretically, for a very long FTS, as in the sketch below, what compensates the FTS weight and size ?

---

EDIT: The fts frame reference should be located at the end of the sensor to match my robotiq ft-300 sensor.

[captain-yoshi]

My fts reference was the same as the robot eef which was erroneous. Now the frame is on the end of the sensor tool side. I have removed the sensor mass and rectified the COM parameters.

  tool:
    # Mass of everything that's mounted after the sensor.
    mass: 1.93

    # The center of mass in the ft_sensor_ref_link.
    # Take a measure and try to estimate that as good as possible.
    com_x: -0.0325
    com_y: 0.0
    com_z: 0.067

I have restested the arm and put screnshots and force and torque plots here (alternate-com folder). The behavior is mostly the same as before, but was not able to trigger the sudden movement.

I will try to test with my FTS and the gripper only because the COM parameters are given by the manufacturer and should be more precise then my custom dual gripper. This will help me pinpoint if my problem comes from my COM parameters or the gravity compensation algorithm.

fzi-master [pd gain = 1.5]

The robot starts failling from a rotation point of view as the first video posted. The Y-axis (green-up in rviz and pink torque in the plot) seems to not be well compensated for gravity. At this point it can be either my COM parameters or the gravity compensation algorithm or the pd gains too high or my FTS ... Wish I had an ATI product :)

kkstb-master [pd gain = 1.5]

The robot does not fall, but it slowly drifts but kept in equilibrium. Notice that the torques are kept between -0.1 and 0.1 N compared to the fzi-master, but the Z-axis force drifts up to -1 N then is kept into equilibrium.

But when I rotate with my hand it starts drifting. @KKSTB Do you have this behavior with your robot and your PR ?

kkstb-master [pd gain = 0.01]

With the old default pd rotational gains, the robot is in equilibrium, but I can't force the robot to rotate by hand because of the gains are so small.

[captain-yoshi]

To simplify my use case, the robot now has a FTS + 2F-140 Gripper. The COM parameters have been adjusted from the manufacturer table (see below).

I see the same behavior as my dual gripper setup. I think the problem is more with the gravity compensation algorithms or some parameters that needs to be tuned.

ft_sensor_ref_link

my_cartesian_force_controller:
  type: "position_controllers/CartesianForceController"
  robot_base_link: "ur3_base_link"
  end_effector_link: "robotiq_2f140_tcp"
  ft_sensor_ref_link: "robotiq_ft300_frame_id"
  joints: *robot_joints

  solver:
    error_scale: 0.5
    iterations: 100

  pd_gains:
    trans_x: {p: 0.05}
    trans_y: {p: 0.05}
    trans_z: {p: 0.05}
    rot_x: {p: 1.5}
    rot_y: {p: 1.5}
    rot_z: {p: 1.5}

  # Gravity should point towards the center of earth.
  # In robot_base_link.
  gravity:
    x: 0
    y: 0
    z: -9.81

  tool:
    # Mass of everything that's mounted after the sensor.
    mass: 1.025

    # The center of mass in the ft_sensor_ref_link.
    # Take a measure and try to estimate that as good as possible.
    com_x: 0.0
    com_y: 0.0
    com_z: 0.073

FZI-master

The controller starts at around 6 seconds. The robot drifts instantly.

IMG_0497.MOV

KKSTB-master

The controller starts when the robot does a small sudden jump (around 1 second). Drifts a little, then when I rotate the gripper it drifts more.

IMG_ok.MOV

[captain-yoshi]

Redid some of the tests with a real time kernel and without the use of an Ethernet switch. The end result is the same.

At the moment I don't need the gravity compensation but at some point I'm sure I will do. Happy to test things whenever you need!

[captain-yoshi]

@stefanscherzinger I will finally need the gravity compensation, so I will try to debug this further. If you have any suggestions feel free to comment.

• Would having a graph of all compensation and errors help ?
• Checking each rotation independantly (high gain for one axis, low gains for other axis).
• Do you have a high level diagram that shows the gravity compensation algorithm ?

[KKSTB]

But when I rotate with my hand it starts drifting. @KKSTB Do you have this behavior with your robot and your PR ?

Yes I got this behavior as well. This is due to some deviation in actual mass+cog and yaml mass+cog, causing a sideway drift in tared force after rotating the effector. The effect is just like you tare a weighting scale and rotate the weighting scale, causing drift in weighting scale reading. You may also experiment with this effect by reducing yaml mass, i.e. reduce the effect of gravity compensation. With less gravity compensation, the drift should become more significant.
As a side note, you can measure the exact mass by comparing the FTS reading of the effector in upward and downward posture.

The controller starts when the robot does a small sudden jump (around 1 second)

Could this be because there is not enough time delay after taring, causing the last FTS reading before taring to sneak into the controller?

[captain-yoshi]

@KKSTB Thanks for the detailed info !

Yes I got this behavior as well. This is due to some deviation in actual mass+cog and yaml mass+cog, causing a sideway drift in tared force after rotating the effector.

Were you able to remove the drift when rotating the effector with your calibrated mass+cog ?

I also got a small drift after starting the controller. Do you see a small drift with your calibrated mass+cog at equilibrium ?

Could this be because there is not enough time delay after taring, causing the last FTS reading before taring to sneak into the controller?

Maybe, will have to check. Do you zero the FTS before or after taring the signal from the controller ?

[stefanscherzinger]

@captain-yoshi

Thanks again for your detailed reports in this thread!

Would having a graph of all compensation and errors help ?

I personally would prefer a qualitative experiment. Unfortunately, I haven't found the time so far, but an integration test with a simple ft sensor dummy should let us check if the gravity compensation (both old and new proposed fix) work as intended.

Checking each rotation independantly (high gain for one axis, low gains for other axis).

I would leave the gains high for this test and test only two rotations. My envisioned setup is something like this:

In which (1) is the zero-ed ft-sensor setup with signaled gravity compensation in the force controller, and (2) and (3) are rotations (e.g. done manually by hand) to check if the algorithm works. As mentioned earlier (also by @KKSTB) these are things to keep in mind:

1. Low controller gains will just hide the drift. But they are not the solution to this problem
2. A minimal drift might still occur if the user's estimation of the mass and com is not 100% correct
3. This can be provoked by deliberately under- (or over) estimating the specified mass (in the controller .yaml)
4. The mass of the test object should be sufficiently big to clearly differentiate the effect from 2.. I would say approx. 3-5 Kilo is fine.
5. If the gravity algorithm works as intended, the robot stays where it is in (2) and (3)

I'll see to put this via simulation into an integration test..

---

Do you have a high level diagram that shows the gravity compensation algorithm ?

Unfortunately no. That's only in code for now.

---

Edit: Your second video from here (KKSTB-master) is already close to testing (2). But it's difficult for me to distinguish a faulty algorithm from effect 2. from the list above.

[Mohatashem]

Out of curiosity has there been any progress on this?

[stefanscherzinger]

@Mohatashem

Not from my side, unfortunately.

[Mohatashem]

Hello @stefanscherzinger and others,

I’ve been meaning to post here earlier. I have been keenly following this discussion, but didn't get the time to ask. I was wondering if I could have you opinion on this, as I've been stuck with gravity compensation for a while now. Apologies in advance for the long post. I will divide it into two parts.

So, I tried testing this on a UR10e in the lab last week, but the robot kept sinking. UR10e's internal FT sensor exhibitied ridiculously weird behaviour. I decided to test this first in a Gazebo simulation instead.

Setup

Here are the key aspects of my Gazebo setup:

• Running in headless mode

• Ceiling Mounted UR10e with an FT sensor plugin specified in the xacro as follows:

    <gazebo reference="ceiling_arm_wrist_3_joint">
        <provideFeedback>true</provideFeedback>
    </gazebo>
    <gazebo>
        <plugin name="ft_sensor_plugin" filename="libgazebo_ros_ft_sensor.so">
            <updateRate>200</updateRate>
            <topicName>/ceiling_arm/force_torque_wrench</topicName>
            <gaussianNoise>0.0</gaussianNoise>
            <jointName>ceiling_arm_wrist_3_joint</jointName>
            
        </plugin>
    </gazebo> 

• The WrenchStamped information from the FT Sensor is relayed to the my_cartesian_force_controller/ft_sensor_wrench.

• Gravity in Gazebo is specified as -9.8 m/s^2

• Gravity is specified as +9.8 m/s^2 for gravity compensation in the yaml file (based on direction of the robot base link, see video attached)

• Since, the FT sensor is associated with the wrist_3_joint (couldn't get to associate it to a fixed joint/link as gazebo seems to lump them together). Under the influence of gravity, it depicts the weight of the wrist_3_link plus everything else after. I tested this by adjusting gravity parameter in gazebo from -9.8 to 0 and back.

• Attached a payload cube of 1kg with 0.1 m side (with COM at 0.0, 0.0, 0.05 m) when considered in the ft_sensor_ref_link

• Assumptions on the force sensor: The force sensor is somewhat ideal (no drift, hysteresis etc), only shows the weight of the object attached to it.

• This is the config yaml file:

$(arg ceiling_prefix)my_cartesian_force_controller:
    type: "position_controllers/CartesianForceController"
    end_effector_link: "$(arg ceiling_prefix)cube_link"
    robot_base_link: "$(arg ceiling_prefix)base_link"
    ft_sensor_ref_link: "$(arg ceiling_prefix)wrist_3_link"
    joints: 
    - $(arg ceiling_prefix)shoulder_pan_joint
    - $(arg ceiling_prefix)shoulder_lift_joint
    - $(arg ceiling_prefix)elbow_joint
    - $(arg ceiling_prefix)wrist_1_joint
    - $(arg ceiling_prefix)wrist_2_joint
    - $(arg ceiling_prefix)wrist_3_joint

    pd_gains:
        trans_x: {p: 1.0}
        trans_y: {p: 1.0}
        trans_z: {p: 1.0}
        rot_x: {p: 1.0}
        rot_y: {p: 1.0}
        rot_z: {p: 1.0}
    
    solver:
        error_scale: 1.0
        iterations: 1

    gravity:
         x: 0.0
         y: 0.0 
         z: 9.8 #  because + z-axis of robot base_link is pointing in the gravity direction

    tool:
         mass: 1.615 #  (wrist_3_link+cube_link = 0.615+1)
         com_x: 0.0 # 
         com_y: 0.0 # 
         com_z: 0.024 # (wrist_3_link+cube_link = -0.026+0.05)

Note: The reason why tool's com and mass specified are not of the cube alone is because gazebo seems to lump fixed links together thereby combining the masses and changing the com. And since, the F/T sensor in gazebo is associated with wrist_3_joint, it motivates the choice of ft_sensor_ref_link: "$(arg ceiling_prefix)wrist_3_link"

Observations:

I've been reading the discussions on gravity compensation. Kindly, correct me if my understanding is wrong.
I think, I have been mixing up "compensating gravity" with "taring signal". If I've understood correctly gravity compensation is going to happen all the time, regardless of the FT sensor orientation. And signal taring is required/called only if there arises drift in the sensor readings or if there is some sort of zero-bias (i.e. if sensor has non-zero readings even when no load is attached). Is this line of reasoning correct? Am I missing out on something?

Assuming it is correct:

In the computeForceError(), we call compensateGravity()
Within compensateGravity() compensate for the weight_force i.e. remove the forces/moments due to the weight of the payload on the sensor.
And as long as computeForceError() is being called, gravity is being compensated.

What I can't seem to understand is the behaviour here

I ran a few experiments in the simulation based on the setup described above:

While, the controller was running, I record data for different steps in the `computeForceError() viz:

• Before calling compensateGravity()
• the tmp variable inside compensateGravity()
• post compensation
• value assigned to m_grav_comp_during_taring on signal taring
• final force/torque value before being transformed and expressed in base-link

Experiment 1:

I started as follows:

1. Bringup the gazebo and load interaction controllers
2. Ran this:

rosrun topic_tools relay /ceiling_arm/force_torque_wrench /ceiling_arm_my_cartesian_force_controller/ft_sensor_wrench

3. Call the service:

rosservice call /ceiling_arm_my_cartesian_force_controller/signal_taring "{}" 

4. Started force controller via rqt_controller_manager.

Observed Behaviour:

experiment_1.video-converter.com.mp4

The data plots are here:

Based on this, (see compensated Compensated Gravity Force/Torque in the figure above). Compensation seems to happen properly (as these values seem to be low/close to zero). However, this causes the forces to be as depicted in Final Force/Torque plot above (Fz > 15 N).

I was wondering the following and what you guys think:

1. My setup differs from the real robotic setup in the sense that I cannot reset the FT sensor, before calling signal taring, which causes the sensor readings to be imparted to the controller as is. That should make things different, right?

2. Does this have to be added to the compensated values at all times?

Note: I ran a second experiment, the only thing I did different this time was that I did not call the signal taring service this time. However, there seemed to be practically no difference between the plots/video I obtained in the second experiment and the ones posted above. Note sure why is that yet.

Results from another experiment follow in the next comment.

[Mohatashem]

Experiment 2

Before running this experiment, I commented out this. This is the only change I made. Everything else stayed the same.

I ran the setup as previously, without signal taring.

Observed behaviour

experiment_4.video-converter.com.mp4

Here the robot seems to drift downwards, but not as abruptly as in the previous experiment. As you can see in the video. I also published desired force values to the target_wrench topic in the end_effector_link as follows:

1. Applied a non-zero force along Z-axis, then applied zero force. The robot seems to stop but drifts slightly.
2. Negative torque about x
3. Positve torque about y
4. Positive torque about z
5. Reset to 0 torques about z and x, the cube continues to rotate about y.
6. Reset to 0 about y-axis

The robot seems to hold the final position and orientation until I turned the controller off.
The plots from this experiments are here:

Obviously, the final values, that are eventually displayed in base link (not depicted here) are pretty similar to the compensated values (since they do not contain the tared term). Again, I am not sure if it was a realistic to do this. What do you guys reckon?

Apart from this, I have another question. If an FT Sensor comes with in-built functionalities of resetting and gravity compensation, do we still require gravity compensation and signal taring features from this controller?

Thank you!!

[stefanscherzinger]

@Mohatashem Many thanks for your experiments and thoughts about this! I have been out-of-office for three weeks and would like to continue fixing gravity compensation once and for all.

I'll need some time to get back into this topic and follow your investigations. Until then I can respond to your brief questions:

Is this line of reasoning correct?

Yes, that's correct and how I remember it. Gravity compensation is always active when used, albeit not changing anything if the end-effector stays upright without orientations after switching the system on and starting with a previously zero-biased sensor.

Am I missing out on something?

No, I guess that's it. Here's the principal idea that I found in a different issue.

My setup #73 (comment) from the real robotic setup in the sense that I cannot reset the FT sensor, before calling signal taring, which causes the sensor readings to be imparted to the controller as is. That should make things different, right?

Yes, that's different. The current approach assumes that the sensor measures zero forces/torques when the controller starts.

If an FT Sensor comes with in-built functionalities of resetting and gravity compensation, do we still require gravity compensation and signal taring features from this controller?

No, we wouldn't. If every sensor had a built-in gravity compensation (and preferably dynamics-compensation) of everything that's attached to it, then I would drop that part from the controllers.