Gazebo: Arm falls on start after switch to Effort Controllers

[ian-chuang]

When launching ur5e_bringup.launch on ur_gazebo on branch melodic-devel-staging, the robot immediately falls when Gazebo starts. The arm can be controlled just fine but needs to be brought back up from where it fell. I think this is due to the recent change to using effort controllers instead of position controllers (https://github.com/ros-industrial/universal_robot/pull/619). When using position controllers (without setting pids in gazebo_ros_control), the robot will start at its designated initial position. However effort controllers are needed for doing realistic grasps. Is there a way to fix this?

I am using ROS noetic on WSL.

[tommyvtran97]

I agree with the original post. I noticed the same issues when using the UR launch files.

[fmauch]

We'll need to look into that...

[stevensu1838]

same issue here.

$ roslaunch ur_gazebo ur5e_bringup.launch $ roslaunch ur5e_moveit_config moveit_planning_execution.launch sim:=true $ roslaunch ur5e_moveit_config moveit_rviz.launch

The robot arm starts at a weird pose and moveit doesn't generate proper movements. Cheers

[stefanscherzinger]

@ian-chuang @tommyvtran97 @stevensu1838

Sorry for the delayed response.

I had a look into the mechanisms of startup and came to the following conclusions:

Background

It seems true, that this issue only appeared after switching to effort_controllers. From my understanding, that's because joint control is now considered in the forward dynamics of the physics engine, i.e. they move according to applied torques and reach equilibrium with constraint forces. This makes sense for grasping and object manipulation. In contrast, the position_controllers type controllers seem to only set joint values (via kinematics) on the lowest level, as e.g. here for the ODE backend. That's why those controllers did not have this problem.

Problem

The issue I see at startup is roughly like this:

1. The controller_manager only starts loading and starting controllers once it receives valid messages on the /clock topic.
2. /clock is published by the gazebo_ros plugin once that starts
3. The plugin starts once the Gazebo simulation starts (via the play button)
4. Once Gazebo's physics engine runs, gravity immediately applies to the robot's joints
5. If the controller_manager is not fast enough in loading a controller that regulates the joints, which it usually isn't, then the robot sinks to the ground.

Workaround

The most reliable approach for me is to start gazebo in paused mode, e.g. with

roslaunch ur_gazebo ur5e_bringup.launch paused:=true

and once Gazebo is up, I set the important parameters via its command line instructions in a new terminal

gz physics -g 0,0,0         # set gravity to zero
gz world -p 0               # unpause the engine. This triggers the controller_manager and the robot should stay where it started.
gz physics -g 0,0,-9.81     # reset gravity to its original value

---

I also experimented with setting /gazebo/gravity/z from the ROS side on startup. There seem to be dynamic_reconfigure options for this, but that did not seem take the wanted effect.

[job-1994]

Hello,

I am having the same issue running on Ubuntu20.04 with Noetic and the universal_robot packages installed via apt (v1.3.1).

The workaround does work, but fixing the launch would be prefereable, is there any update on this?

Would be glad to help set this up if theres any ideas on where to start?