In this post we implement a ROS node that automatically initializes the robot state in Gazebo to a valid configuration . This is achieved by means of a custom joint position controller and the controller switching mechanism provided by the controller manager.
In this post we will alter the integration by replacing the effort controllers with joint position based controllers and thereby improve the stability of the robot during trajectory execution. We will also fix issues when starting the robot in Gazebo where it gets initialized with invalid joint angles by implementing a custom controller that commands the joints to fixed state.