Arm behaving strangely under simulation

[Zabot]

There appears to still be something strange going on with the inertial properties of the arm. These two gifs were recorded at the same time scale. I suspect the inertia of some of the arm components is higher than it should be.

[Zabot]

@corot Can you try and reproduce this with your turtlebot model? Both with and without the gazebo_ros_control block in the urdf.

[corot]

Same for me...
with:

without:

[Zabot]

What type of hardware interface are you using in your gazebo transmissions? This appears to be related to this issue ros-simulation/gazebo_ros_pkgs#479. The issue resolves when I use VelocityJointInterfaces.

[corot]

I'm currently using PositionJointInterface

  <xacro:macro name="servo_transmission" params="name">
    <transmission name="${name}_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${name}_joint">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
      </joint>
      <actuator name="${name}_motor">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
      </actuator>
    </transmission>
  </xacro:macro>

After some experiments (explained in issue you mention), that seems to work the best. But still, due to the problem you state here, the arm motions make the full robot glide due to its huge inertia. Also I still have the problem (maybe related) of the arm tilting in an impossible angle.

[Zabot]

I don't think it's an inertia issue anymore. In gazebo the function that the PositionJointInterface calls to set the rotation of a joint used to preserve the velocity of the child frame of the joint in world space. Now it zeros the world velocity of the child joint. Every time a JointPositionInterface is updated it cancels the effect of gravity on the child frame, causing the child frame to hang in midair.

[corot]

Hi @Zabot, any progress on this issue? Your explanation sounds reasonable. Did you open an issue on gazebo? And... can you point me to the code where this set-zero velocity happens? I'm not familiar at all with Gazebo code, sorry.

[Zabot]

I haven't personally done any more work on the issue, there is an open pull request to gazebo, but there hasn't been any activity on it in a while. To full resolve this issue there needs to be a PR to gazebo adding a flag to disable the set-zero behavior, and a PR to gazebo_ros_pkgs to use that flag in gazebo_ros_control.

[corot]

I have read the discussion in the PR, but I must be missing something, as I try what @michael Gray suggest (get velocity, set position, restore velocity in this line) but this code is not even executed until moving the arm, so is useless to fix the weird behavior described on this issue.

By other hand, I have tried to compile Gazebo so I can comment out the speed zeroing, but failed miserably, due to this problem.

[corot]

I can confirm that this is fixed on Gazebo9. But you need to tweak gazebo_ros_control pkg:

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