[gazebo_ros_control] PositionJointInterface not working on gazebo 7 / ros kinetic

[vincentrou]

When I load an urdf in gazebo 7.0 (ros kinetic) with a PositionJointInterface for the joint transmission hardwareInterface, I get a robot floating in the air and I can't control the joint.
If I use a VelocityJointInterface instead, it is working fine.

The same urdf is working fine on gazebo 2.2.3 / ros indigo.

    <transmission name="${wheel_prefix}_steering_trans" type="SimpleTransmission">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${wheel_prefix}_steering_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
      <joint name="${wheel_prefix}_steering_joint">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
      </joint>
    </transmission>

Could it be related to this ? https://bitbucket.org/osrf/gazebo/issues/1138/set-joint-positions-in-jointcontroller

[j4re]

Anything new on this?

[corot]

I have also problems with PositionJointInterface after moving to Kinetic:

Could this arm shacking be related with your problem? note that the servos joints tremble in a different axis, not the servo rotating one.
Funny thing is that this only happens sometimes. Others, all works quite ok. I'm puzzled....

[vincentrou]

Get time to look at it today.
It seems that when you reduce real time update rate, the problem is reduce...

In the file "default_robot_hw_sim.cpp", if you use the POSITION_PID instead of POSITION for the joint_control_methods_ there is no more problem.

So the problem seems to be in gazebo when using SetPosition instead of SetForce.
In gazebo, the problem should come from Joint.cc in "Joint::SetPositionMaximal" function which is use by ode and bullet.
When I comment out the line with "(*li)->MoveFrame(childLinkPose, newChildLinkPose);" there is no more problem...

[corot]

@vincentrou, you are right. Using POSITION_PID instead of POSITION solved the issue. Thanks!

Is there already a PR on this?

[vincentrou]

In fact, I think the problem is in gazebo so I have open an other issue.
You can vote for it if you want to see it resolved.

I have also created a branch to reproduce the problem based on the gazebo ros_control tutorial
Here is the gazebo_ros_demos to reproduce the problem https://github.com/vincentrou/gazebo_ros_demos/tree/position_joint_interface

[vincentrou]

@corot If you want to use POSITION_PID instead of POSITION you just need to load pid in the right rosparam namespace.
Like the commented lines in this commit

[corot]

Thanks again, @vincentrou. Well, what I want is a simulated arm that just executes the trajectories that moveit provides. I'm sock about how difficult is this in Gazebo7!
I already noticed that your fix prevents the controller to execute anything, so your last suggestion makes a lot of sense. but... it somehow interferes with the kobuki base plugin:

[/gazebo  WARN   .... ] Gazebo ROS Kobuki plugin: NaN in d1. Step time: 0.01, WD: 0.07, velocity: -nan

I get this WARNs all the time and tf tree collapses because the base doesn't provide odom -> base_footprint.

Apart from this, I have a question, if you don't mind answering here:
I set the transmissions for my simulated servos in the URDF as:

    <transmission name="${name}_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${name}_joint">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
      </joint>
      <actuator name="${name}_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

And then I define two controllers, one JointTrajectory and one JointPosition:

arm_controller:
  type: position_controllers/JointTrajectoryController
  joints: [arm_shoulder_pan_joint, arm_shoulder_lift_joint, arm_elbow_flex_joint, arm_wrist_flex_joint]
  constraints:
     .....
  ......
# Gripper Position Controller
gripper_controller:
  type: position_controllers/JointPositionController
  joint: gripper_joint

as in the rear robot (turtlebot_arm). This used to work in Gazebo2-indigo. Makes still sense in Gazebo7-Kinetic?
Thanks one more time!

[vincentrou]

@corot I do not use moveit, you should ask it at answers.ros.org
But it seems that Gazebo has problem with PositionJointInterface so maybe you should use EffortJointInterface instead.

[RDaneelOlivav]

What Happened with this issue? Any progress? Have the exact same problem here.

[riksof-germany]

try to use <hardwareInterface>EffortJointInterface</hardwareInterface> instead of PositionJointInterface inside the robot urdf.

[michaeldibono]

I'm experiencing this problem as well. Using <hardwareInterface>EffortJointInterface</hardwareInterface> or <hardwareInterface>VelocityJointInterface</hardwareInterface> does not help. Although the floating is resolved, when I use either I lose the ability to move the arms of my robot. Any updates on a fix?

[Kukanani]

I can also confirm this issue on Kinetic/Gazebo 7.

[vincentrou]

You can fix the problem by fixing manually pid gains for your PositionJointInterface.
Just put those params in a yaml file and load it with rosparam in a launch file.

gazebo_ros_control:
  pid_gains:
    joint2: {p: 100.0, i: 0.01, d: 10.0}

It seems that they will not put back the previous behavior in gazebo : https://bitbucket.org/osrf/gazebo/pull-requests/2598/fix2111/diff

[Maik93]

Hi, still experiencing issues with JointGroupPositionController. I've wrote:

type: "position_controllers/JointGroupPositionController"
joints:
 - joint_1_1
 - joint_1_2
 - joint_1_3
 - joint_2_1
 - joint_2_2
 - joint_2_3
 - joint_3_1
 - joint_3_2
 - joint_3_3
 - joint_4_1
 - joint_4_2
 - joint_4_3
 - joint_5_1
 - joint_5_2
 - joint_5_3
 - joint_6_1
 - joint_6_2
 - joint_6_3

gazebo_ros_control:
  pid_gains:
    joint_1_1: {p: 100.0, i: 0.01, d: 10.0}
    joint_1_2: {p: 100.0, i: 0.01, d: 10.0}
    joint_1_3: {p: 100.0, i: 0.01, d: 10.0}
    joint_2_1: {p: 100.0, i: 0.01, d: 10.0}
    joint_2_2: {p: 100.0, i: 0.01, d: 10.0}
    joint_2_3: {p: 100.0, i: 0.01, d: 10.0}
    joint_3_1: {p: 100.0, i: 0.01, d: 10.0}
    joint_3_2: {p: 100.0, i: 0.01, d: 10.0}
    joint_3_3: {p: 100.0, i: 0.01, d: 10.0}
    joint_4_1: {p: 100.0, i: 0.01, d: 10.0}
    joint_4_2: {p: 100.0, i: 0.01, d: 10.0}
    joint_4_3: {p: 100.0, i: 0.01, d: 10.0}
    joint_5_1: {p: 100.0, i: 0.01, d: 10.0}
    joint_5_2: {p: 100.0, i: 0.01, d: 10.0}
    joint_5_3: {p: 100.0, i: 0.01, d: 10.0}
    joint_6_1: {p: 100.0, i: 0.01, d: 10.0}
    joint_6_2: {p: 100.0, i: 0.01, d: 10.0}
    joint_6_3: {p: 100.0, i: 0.01, d: 10.0}

but seems to have no effect...

Changing in VelocityControl will fix the slow-fall, but I need position controls.
Thanks for help.

[mica-angeli]

@vincentrou I can confirm that adding the PID gain paramters to the gazebo_ros_control configuration solved the issue. Thanks!

[vincentrou]

This issue is resolved since the gazebo issue is fixed and integrated in gazebo9.
But you need to install gazebo9 with this source list :
deb http://packages.osrfoundation.org/gazebo/ubuntu-stable xenial main
Since the default version is gazebo7 for kinetic.

[Maik93]

Sorry, can't make Gazebo9 work. I've run this commands:

sudo add-apt-repository "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable xenial main"
wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -

sudo apt-get install ros-lunar-desktop-full
sudo apt-get install gazebo9

that will produce:

The following packages will be REMOVED:
  gazebo7 gazebo7-common gazebo7-plugin-base libgazebo7 libgazebo7-dev
  libsdformat4 libsdformat4-dev ros-lunar-desktop-full ros-lunar-gazebo-dev
  ros-lunar-gazebo-plugins ros-lunar-gazebo-ros ros-lunar-gazebo-ros-pkgs
  ros-lunar-simulators
The following NEW packages will be installed:
  gazebo9 gazebo9-common gazebo9-plugin-base libavdevice-ffmpeg56 libgazebo9
  libignition-common libignition-fuel-tools libignition-math4 libignition-msgs
  libignition-transport4 libqwt-qt5-6 libsdformat6 libsdl1.2debian libzip4

and now when I try to launch something with ROS+Gazebo through roslauch I've got this error: Resource not found: gazebo_ros. Trying to install manually any of removed package (e.g. ros-lunar-gazebo-ros) will produce a dependency error.

[vincentrou]

You need also to install gazebo9 ros_wrapper ros-$ROS_DISTRO-gazebo9-ros-pkgs and/or ros-$ROS_DISTRO-gazebo9-ros-control
Like mentioned here http://gazebosim.org/tutorials/?tut=ros_wrapper_versions#Gazebo9.xseries

[Maik93]

Perfect, now it works, but on Gazebo 9.0.0-1 (currently latest) the problem is not fixed. I'll wait for the next release.

[corot]

Interesting... I tried 2 weeks ago and it worked with the first gazebo9 released, and stopped working after upgrading. Has been the fix dropped from last release? 😮

[raequin]

@corot when you say gazebo9 worked two weeks ago, was that by setting the preserveWorldVelocity flag to true? Now that fix does not work? I just want to check before replacing gazebo7 with 9.

How does one set the preserveWorldVelocity flag when using roslaunch and urdf? @Maik93 you have experience with this, correct?

[j-rivero]

Interesting... I tried 2 weeks ago and it worked with the first gazebo9 released, and stopped working after upgrading. Has been the fix dropped from last release?

If it helps, there has been only one release of gazebo9, 9.0.0.

[corot]

Ups... sorry, I confused two different issues. What is solved for me in Gazebo9 is the super-inertia in the arm. to make it work, I set preserveWorldVelocity to true on gazebo_ros_control:

diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp
index a295a3e..3ddd29f 100644
--- a/gazebo_ros_control/src/default_robot_hw_sim.cpp
+++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp
@@ -307,7 +307,7 @@ void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period)
 
       case POSITION:
 #if GAZEBO_MAJOR_VERSION >= 4
-        sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
+        sim_joints_[j]->SetPosition(0, joint_position_command_[j], true);
 #else
         sim_joints_[j]->SetAngle(0, joint_position_command_[j]);
 #endif

That was part of my problem shown in the video of my first post. But I still have problems with the trembling arm on Gazebo9. I tried @vincentrou suggestion of adding PID values:

gazebo_ros_control:
  pid_gains:
    arm_shoulder_pan_joint: {p: 100.0, i: 0.01, d: 10.0}
    arm_shoulder_lift_joint: {p: 100.0, i: 0.01, d: 10.0}
    arm_elbow_flex_joint: {p: 100.0, i: 0.01, d: 10.0}
    arm_wrist_flex_joint: {p: 100.0, i: 0.01, d: 10.0}
    gripper_joint: {p: 100.0, i: 0.01, d: 10.0}

but I got something even funnier:

[corot]

When I try @vincentrou's sugestion of using POSITION_PID instead of POSITION in this line I get this curious effect just by unpausing Gazebo

and then the controller fails systematically complaining about path constraint violated:

[/gazebo  WARN 1520786186.316629396, 203.530000000]: Dropping first 1 trajectory point(s) out of 25, as they occur before the current time.
First valid point will be reached in 0.359s.
[/move_group  WARN 1520786186.431446466, 203.630000000]: Controller arm_controller failed with error code PATH_TOLERANCE_VIOLATED
[/move_group  WARN 1520786186.431684779, 203.630000000]: Controller handle arm_controller reports status ABORTED

[corot]

I think I found the problem... but is rather puzzling:

Happens when a simulated servo faces resistance! I'm using joints of type POSITION. As long as I avoid these limit configurations, all works fine. But of course, is terribly brittle!

[TheDash]

Any fix for this? pid_gains addition to my ros control namespace didn't fix anything.

[vincentrou]

To make it work on kinetic with gazebo9 you will need this fix #688
which is present in gazebo_ros_control 2.5.16 and higher 2.5.x

[TheDash]

@vincentrou Would this work with Gazebo 7? with the fix.

Edit: Read your link at #688. I guess the only option is then to use VelocityJointInterface or JointCommand?

[saikishor]

There is currently one issue with Gazebo 7.12.0, when a manipulator is grasping an object with its gripper, then when it is performing a motion the gripper tends to open and close due to some unknown reasons, releasing the grasping object and going to its desired position. So you know the possible reason for this issue?. This didn't happen in Indigo version of gazebo. Any idea?. The problem persists when using with the SetForce

[suraj2596]

This happens when your controller_manager is not launched. https://answers.ros.org/question/214712/gazebo-controller-spawner-warning/

[aniruddhkb]

I would just like to leave this here. In case you are using PositionJointInterface (on Melodic/Gazebo9) and it doesn't work as expected, supply negative PID values. For some reason the Gazebo system provides positive feedback instead of negative.