Prismatic joint for lift door having strange behaviors when its parent lift model is loaded with heavy robots

edit

Hi there! I am working on lift models to transport robots in RMF with ROS2 Eloquent and Gazebo 9.13.2. But my lift models are misbehaving when loaded with heavy robots.

Background

The basic structure of the lift model consists of a parent lift cabin model, driven by a vertical prismatic joint controlled by a lift plugin. Then there are one or more lift door models nested in the lift cabin model, driven by prismatic joints controlled by door plugins. Click here for the sdf of a lift model.

The general workflow of a robot taking the lift is: The robot stops in front of the lift and sends a LiftRequest message to request the lift to open on its level. It waits until the lift is sending out the desired LiftState message, and enters the lift. The procedure is the same when exiting the lift.

The problem

The lifts work fine when just moving without any loads. But when my robot that has a mass of 70 enters or tries to exit the lift, the lift doors start to have weird behaviors like closing/opening unexpectedly or moving back and forth when they try to open/close. However, if I lower the mass of the robot model to 20, the lift behaves normally again. Here is a video showing what is happening.

Things I have tried

I have eliminated possibilities that the lift or door plugins are causing the problem. As when I print out the position, velocity, acceleration, etc., I can see that the door plugin is doing the right thing to try to accelerate the door in the correct direction. But the velocity of the misbehaving door is just mysteriously increasing in the opposite direction.

The issue is also not caused by collisions between models, as when I change the whole lift cabin's collision bitmask to 0x00, the same thing happens.

I have also tried changing the f_max parameter for both the lift and door plugin. The f_max of lift plugin doesn't seem to be affecting. But changes in f_max of door plugin makes the doors behave slightly differently in terms of velocity and acceleration, but I couldn't find a value that eliminates or even ease the problem.

So the only direct factor seems to be the mass of the robot entering, and I really have no idea now why this is happening. Appreciate if anyone could suggest any possible sources of the problem.

Thanks!

To reproduce the behavior:

1. Clone the following branches of the repos into the RMF workspace:
2. rmf_core: Kevinskwk:one_to_one
3. rmf_schedule_visualizer: master
4. traffic_editor: master
5. rmf_demos: bug/heavy_robot_taking_lift
6. Source ros eloquent first, then do colcon build
7. Source the setup.bash and run ros2 launch demos lift_test.launch.xml
8. In the RViZ RMF panel, select fleet mir100 and send a loop request from mir100_1_charger to L1_door