Joint Torque Controller jumpy behavior

[Thieso]

Hello,

I have a controller plugin that computes the joint torques for my robot manipulator and then in the PreUpdate function applies them to the robot using the JointForceCmd. However, when the robot is only supposed to stay at one particular joint configuration there is already a problem. The computed joint torques by the controller are correctly computed and stay constant. About every few seconds the robots joints seem to jump to a different position and the controller has to correct again to converge back to the static desired joint configuration. I am very unsure why this happens. The controller already worked in Gazebo classic so I think the torques are computed correctly. Could this be a physics integration issue or what might be the problem?

Thanks for any help :)

Environment

• OS Version: Ubuntu 20.04
• Source or binary build? libignition-gazebo3 package 3.15.0-1

Description

• Expected behavior: Controller is working flawlessly
• Actual behavior: Robot is showing jerky behavior and the joints seem to sometimes jump from one position to another. The controller then stabilizes the system again.

Output

Here the blue line is the actual joint value reported by Gazebo from the JointPosition component and the orange is the desired value. As you can see the orange line is constant but the actual value randomly jumps up and then the controller has to stabilize it again.

[jrutgeer]

What I would do is try a simple test case first, e.g.:

• Just one link,
• One rotational joint,
• Have the link hang vertically down, so the controller shouldn't output any torque,
• Run it and see if it stays stable.

Then: do the same but with an initial position horizontal (i.e. without controller the link will oscillate around its rotational joint). See if the controller can hold it stable in the horizontal position.

Then: add a second link. Etc.

In any case: without code and a minimal example, it's a bit hard to reason about this.

[Thieso]

Thanks for the quick reply. I understand that it is hard without a minimal example but it is also kind of hard to provide such an example for me. Maybe just one question concerning the code.

I use

  double t = std::chrono::duration<double>(_info.simTime).count();

  for (uint i = 0; i < joints.size(); i++)
  {
    const auto& joint_pos = _ecm.Component<gz::sim::components::JointPosition>(joints[i]);
    ang_arr.push_back(joint_pos->Data()[0]);
  }

for getting the joint positions and

  for (uint i = 0; i < joints.size() && i < tau.size(); i++)
  {
    auto joint_torque = _ecm.Component<gz::sim::components::JointForceCmd>(joints[i]);
    if (joint_torque == nullptr)
    {
      _ecm.CreateComponent(
          joints[i],
          gz::sim::components::JointForceCmd({tau[i]}));
    }
    else
    {
      const std::vector<double> tauv(1, tau[i]);
      _ecm.SetComponentData<gz::sim::components::JointForceCmd>(joints[i], tauv);
    }
  }

for setting the joint torques. Is that in general correct? There is little documentation about this as far as I know.

Thanks

[azeey]

That seems strange. What you're doing looks good, so I'm not sure what might be causing the jumps. Like @jrutgeer said, it's really hard to debug this without more info.

[jrutgeer]

A few thoughts:

1) > ang_arr.push_back(joint_pos->Data()[0]);

This would be trivial, but nevertheless: I'd rather expect `ang_arr[i] = joint_pos->Data()[0];` here (unless `ang_arr` is local to the scope?).

2) I came accross issue #1926 which probably isn't related, but still relevant background info to know.

3) Not the cause of your issue, but base on this remark of @azeey in #1926:

Now that we have EntityComponentManager::SetComponentData, which creates a component if it doesn't exist, it [...]

I conclude that the 2nd code block could be reduced to just:

  for (uint i = 0; i < joints.size() && i < tau.size(); i++)
  {
       const std::vector<double> tauv(1, tau[i]);
      _ecm.SetComponentData<gz::sim::components::JointForceCmd>(joints[i], tauv);
  }

4) This also seems rather strange i < joints.size() && i < tau.size(); as I would expect it to be a functional error if joints.size() != tau.size(). E.g. if joint2 is removed but not tau2, then tau[i] no longer corresponds to joint[i].

[Thieso]

Thanks for your thoughts.

1. Yes ang_arr is local to the scope
2. This issue is interesting and I will try to see if data gets cleared and does not get set correctly in the same time step.
3. Good point, thank you :)
4. Yes that condition seems rather useless for sure.

Since I use libgazebo3, do you think this would be resolved in newer version of Gazebo? I might try to port all of this to some newer version but not sure if the backend of the simulation is effected by this at all.

[jrutgeer]

Since I use libgazebo3, do you think this would be resolved in newer version of Gazebo?

I don't know, but libignition-gazebo3 seems rather old, I only seem to be able to install libignition-gazebo6 and libignition-gazebo7 through apt-get...

i am running Gazebo Garden built from source per these instructions. The vcs import < collection-garden.yaml command pulls everything from github automatically into the active folder (typically ~/workspace/src/), and all build and install files come in the folder one level down (~/workspace/). So it is easy to run multiple installations concurrently. You only need to source the correct install/setup.bash file. I would give it a try.

If you also need ROS 2 than it's a bit more involved as you also need to compile the ros gazebo bridge, but that's shouldn't be too hard either, except possibly for some minor quircks (I had to comment out some message types as I didn't have those installed).