Various bug fixes

[AndyZe]

Fixes #113. To quote from that issue:

    if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) {
      ...
        *jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
          {this->dataPtr->joints_[i].joint_velocity_cmd});
      }
    }

    if (this->dataPtr->joints_[i].joint_control_method & POSITION) {
      ...
        vel->Data()[0] = targetVel;
      }
    }

    if (this->dataPtr->joints_[i].joint_control_method & EFFORT) {
      ...
        *jointEffortCmd = ignition::gazebo::components::JointForceCmd(
          {this->dataPtr->joints_[i].joint_effort_cmd});
      }
    }
  }

Here are some of the bugs in that code:

• VELOCITY can set a target velocity, then POSITION can set a different velocity target. In other words, POSITION overrides the effect of VELOCITY.
• All of POSITION, VELOCITY, and EFFORT can be active at any time. This does not seem right -- only 1 should be active.
• There's no PID gain specified in calculating the POSITION command. The gain is 1.
• When I send a position command to just one joint (for example), the other joints tend to sag downward with gravity.

Here's a "before" video. It's hard to see but note that the "upper_arm_link" of the robot is always drooping down with gravity although only the elbow joint is actuated.

slow_droop.online-video-cutter.com.mp4

Here's an "after" video showing no droop of other joints besides the actuated elbow.

[AndyZe]

An optional gain parameter. Previously it was hard-coded to 1.0.

[AndyZe]

Small efficiency improvement

[AndyZe]

I think this is what prevented the unactuated joints from drooping with gravity.

[AndyZe]

else-if so the velocity command from this interface doesn't conflict with the one at L597

[AndyZe]

ament_cpp_lint conflicts with one of the other linters:

/home/ros2_ws/src/gz_ros2_control/ign_ros2_control/src/ign_system.cpp:547: If an else has a brace on one side, it should have it on both [readability/braces] [5]

[ahcorde]

@AndyZe do you have any plans to include these fixes in gazebo_ros2_control ? Let me know if you don't have time and I will do it

[AndyZe]

I don't mind making a quick PR to gazebo_ros2_control 👍

I actually wasn't aware of gazebo_ros2_control prior to this. Is that the recommended way?

[ahcorde]

We need to support Gazebo classic too, the code is pretty similar, when something is added here or there I try to keep both repositories up to day. If the contributor is open to do it it's fantastic otherwise I will do it.

By the way, thank you for your contribution.