HSR in Gazebo hops

[y-masutani]

I am trying the MoveIt demo by using simulator. HSR in Gazebo hops abnormally.
Is there something wrong? How can I fix it?

Please see the linked video.

https://drive.google.com/file/d/1Hdsg1Qb3QVCVd3vdNc9-IqY4Cps98m-Y/view?usp=sharing

[keisuketakeshita]

Are you using gazebo_ros2_control provided by hsr_project?
As far as I have tried, gazebo_ros2_control obtained through apt causes hopping behavior.

[y-masutani]

I include the following repository in my workspace
https://github.com/hsr-project/gazebo_ros2_control.git (foxy branch)
So after source install/setup.bash, it should be used.

I tried sudo apt remove ros-foxy-gazebo-ros2-control just to be sure, but the result was the same.

[keisuketakeshita]

You can determine whether you are using gazebo_ros2_control provided by hsr_project by checking if there are PID parameters.
If parameters like wrist_roll_joint.p exist when you execute "ros2 param list /gazebo_ros2_control", then you are using the package prvided by us.

[y-masutani]

After launching the Gazebo simulator, I executed the following command.

ros2 param list /gazebo_ros2_control 

The output was only one line.

  use_sim_time

I redid the whole build and the result was the same.

How can I resolve this situation?
Any advice would be appreciated.

[y-masutani]

Is issue #10 also caused by the same?

[y-masutani]

I am trying to read the source code of gazebo_ros2_control provided by hsr_project. I am using the following commits as clues.
ros-controls/gazebo_ros2_control@410068c
I understand that hsr_project has added the control method named POSITION_PID. I have the following questions.

• Where is the setting regarding POSITION_PID?
• Where is the function DeclarePIDControl() called from?

[keisuketakeshita]

Is issue #10 also caused by the same?

I think so.

I am trying to read the source code of gazebo_ros2_control provided by hsr_project. I am using the following commits as clues. ros-controls/gazebo_ros2_control@410068c I understand that hsr_project has added the control method named POSITION_PID. I have the following questions.

• Where is the setting regarding POSITION_PID?
• Where is the function DeclarePIDControl() called from?

It appears to be a mistake made during the release process.
Could you try the following branch, which contains fixes for the issue?
It might improve the behavior to some extent.

https://github.com/hsr-project/gazebo_ros2_control/tree/bugfix/activate-pid-control

[y-masutani]

I tried the bugfix/activate-pid-control branch.

• ros2 param list /gazebo_ros2_control now shows many parameters.
• The angular velocity command for the /omni_base_controller/command_velocity topic now rotates the base_link with respect to the floor.
• The hop problem in moveit_fk_demo is still there.
• New problem? HSR rotates gradually even when no command is given. See the screencast below.

https://drive.google.com/file/d/1OTXw3h-Xx4kd6ZZtYbjjUlQZuD7QLl2c/view?usp=sharing

[y-masutani]

I found that the problem has not yet been solved at all.
The phenomenon of friction between the wheel and the floor is obviously wrong.
Compare the two screencasts.

ROS1 Gazebo
https://drive.google.com/file/d/1vLe2RRVifZC4rVfW_kg_8Mdpuuen7GOZ/view?usp=drive_link

ROS2 Gazebo (with bugfix/activate-pid-control branch)
https://drive.google.com/file/d/1SOtaX9-RXhgkgLNoDtVoUjScQq1G0ld7/view?usp=drive_link

How can I resolve this situation?
Any advice would be appreciated.

[k-takeshita]

Could you please confirm the following three points?

• Is the automatic startup of ROS1 disabled?
• Is the stop buton released?
• Can you read the hash value with the following command?
  ros2 run hsrb_servomotor_protocol exxx_read_hash /dev/ttyCTI2 11

[y-masutani]

This issue is not about the actual robot, but about the simulation.

[k-takeshita]

I'm sorry. I replied to the wrong place. It was meant for #11.

[y-masutani]

@k-takeshita I would like to know if the phenomenon I point out in this issue happens only in my environment or also in yours.

[k-takeshita]

We recognize the "hopping" and "slipping" phenomena as known issues.
While transitioning to PID control similar to ROS1 has reduced the severity of the issues, they haven't been completely resolved.

[y-masutani]

@k-takeshita In your environment, does the HSR in Gazebo gradually turns even when no commands is given? Is this phenomenon the same as the "slip" you said? First of all I would like to prevent it from occurring.

[y-masutani]

Using the Plot in Gazebo and rosbag , I observed a time series of values for each joint. All joints oscillate violently, especially arm_roll_joint and wrist_roll_joint (their velocity amplitude is 24 rad/s). See the following screenshots.

The same thing was observed in ROS 1. The oscillations are also occurring, but their amplitude is much smaller. See the following screenshots.

I suspect that the oscillations are caused by a time delay in the controller's processing. It is difficult for me to find out, so I added damping to the joints as an alternative.

hsrb_description/urdf/common.xacro :

   <xacro:arg name="implicit_damping" default="true" />

hsrb_description/urdf/arm_v0/arm.urdf.xacro:

        <joint name="${prefix}_roll_joint" type="revolute">
            <origin xyz="0.005 0.0 0.345" rpy="0.0 0.0 0.0" />
            <axis xyz="0.0 0.0 1.0" />
            <limit effort="100.0" velocity="2.0" lower="-2.09" upper="3.84" />
            <parent link="${prefix}_flex_link" />
            <child link="${prefix}_roll_link" />
            <dynamics damping="1.0"/>
        </joint>

With these two changes, the arm_roll_joint and wrist_roll_joint vibrations are slightly reduced and the main body no longer rotates with respect to the floor when no command is given. However, I am not sure if this is an adequate solution. In addition, the problem of slip against rotation remains.

[y-masutani]

@k-takeshita Doesn't the HSR model need a PID controller for velocity control of the driving wheels?

[k-takeshita]

@k-takeshita Doesn't the HSR model need a PID controller for velocity control of the driving wheels?

It hasn't been tried yet, but there's a possibility that the HSR model could benefit from a PID controller for velocity control of the driving wheels.

The hopping issue seems to occur more frequently when there is a difference between the current value and the desired value of the arm_lift_joint at the start of its movement.
However, the cause and solution for this issue are still unknown.

[y-masutani]

I found that the cause of the HSR main body's rotation without command and joint vibration is wrist_roll_joint. Observing the wrist_roll_joint's effort in rosbag, I found that it oscillates.

Even without changing the damping, by changing the D-gain of wrist_roll_joint from 10 to 2, the oscillation stopped and the HSR main body stopped rotating. I am not sure if this is the optimal cause.

@k-takeshita How did you determine the PID gain of the joint?

[y-masutani]

However, the slip phenomenon has not been eliminated at all. Friction is effective for translation, but not for rotation. This is a serious problem because changing the value of the coefficient of friction has no effect.

I have seen other problems in Gazebo where the friction settings do not work and the objects slip. In fact, I experienced this with a gripper of some robot arm.

@k-takeshita My biggest question is that the HSR simulator you provide for ROS 1 does not have the above problem. Do you use any special techniques (or cheats)? Please let me know them.

[k-takeshita]

How did you determine the PID gain of the joint?

I didn't make the gain adjustments, so I don't know how they were determined.
However, I think the current gains are too high, so I agree with lowering them.
If you find good gains, please submit a pull request.

Do you use any special techniques (or cheats)? Please let me know them.

The only special modification in ROS1 is that the "read" function is performed every cycle.
This change has been added to the foxy branch.
https://github.com/hsr-project/hsrb_gazebo_plugins/blob/master/src/hsrb_gazebo_plugins/hsrb_gazebo_ros_control_plugin.cpp#L62
https://github.com/hsr-project/gazebo_ros2_control/blob/foxy/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp#L375

[y-masutani]

I may have fixed the bug.

I found that in GazeboSystem::write(), SetForce() and SetPosition() were being executed simultaneously.
I don't know if this is an appropriate fix, but I changed that part and the behavior was noticeably improved.

hsr-project/gazebo_ros2_control#1

In addition, I introduced VELOCITY_PID with reference to POSITION_PID. The behavior was further improved.

hsr-project/gazebo_ros2_control#2
hsr-project/hsrb_description#6

I compared them in the response to the backward command. See the three screencasts below.

• ROS1
  https://drive.google.com/file/d/1jmMcqYyEryqQ-j4fJ3DDtNQ0lZyM-LOl/view?usp=sharing

• ROS2 POSITION_PID with bug fix
  https://drive.google.com/file/d/1iYNk_h86e0q9SaN8vVwOV9d3FijzeJv6/view?usp=sharing

• ROS2 POSITION_PID with bug fixesa nd VELOCITY_PID
  https://drive.google.com/file/d/1G6uGhmIlCGy0lgAzrkuZmmM08U7xlY-B/view?usp=sharing