search

hsr-project / hsr_ros2_doc

6 stars 1 forks source link

HSR in Gazebo hops #9

Open y-masutani opened 4 months ago

y-masutani commented 4 months ago

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 commented 3 months ago

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 commented 3 months ago

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 commented 3 months ago

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 commented 3 months ago

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 commented 3 months ago

Is issue #10 also caused by the same?

y-masutani commented 3 months ago

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

keisuketakeshita commented 3 months ago

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 commented 3 months ago

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

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

y-masutani commented 3 months ago

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 commented 3 months ago

Could you please confirm the following three points?

y-masutani commented 3 months ago

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

k-takeshita commented 3 months ago

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

y-masutani commented 3 months ago

@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 commented 3 months ago

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 commented 3 months ago

@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 commented 3 months ago

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.

Screenshot from 2024-03-24 11-50-21

Screenshot from 2024-03-24 11-57-55

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

Screenshot from 2024-03-24 14-02-01

Screenshot from 2024-03-24 14-05-00

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.

Screenshot from 2024-03-24 16-12-50

y-masutani commented 3 months ago

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

k-takeshita commented 3 months ago

@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 commented 3 months ago

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 commented 3 months ago

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 commented 3 months ago

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 commented 3 months ago

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.

https://github.com/hsr-project/gazebo_ros2_control/pull/1

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

https://github.com/hsr-project/gazebo_ros2_control/pull/2 https://github.com/hsr-project/hsrb_description/pull/6

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