Improving Panda Gazebo integration with Position based Control

In my recent blog posts I presented a minimal approach to integrating the FRANKA EMIKA Panda robot within the Gazebo simulation environment together with the MoveIt! manipulation framework.

In this post we will alter this 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.

The improvements will be based on the GitHub repository that was also used in the previous posts. You can check it out here.

Position-based Joint Trajectory Execution

We will alter the configurations for the robot in the packages panda_simulation and franka_ros in order to migrate to position based control.

The following files need to be changed:

franka_ros/franka_description/robots/panda.transmission.xacro

We are going to replace every occurrence of EffortJointInterface with PositionJointInterface. This way Gazebo will register the position joint interfaces in the simulation for each of the robot’s joints when spawning the robot (see implementation).

<transmission name="${robot_name}_tran_1">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${robot_name}_joint1">
                <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            </joint>
            <actuator name="${robot_name}_motor_1">
                <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>

panda_simulation/config/panda_control.yaml

In our configuration parameters we are going to increase the publishing rate of the arm and state controllers to 100 and replace effort_controller/JointTrajectoryController with position_controllers/JointTrajectoryController. The last change only needs to be applied to panda_arm_controller. We can also remove the manual gains as they are not needed for the position based joint commands in Gazebo.

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 100

panda_arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - panda_joint1
    - panda_joint2
    - panda_joint3
    - panda_joint4
    - panda_joint5
    - panda_joint6
    - panda_joint7

  constraints:
    goal_time: 2.0

  state_publish_rate: 100

These modifications are enough for migrating the package to position based control but at the next startup the robot will start in an invalid configuration within Gazebo. This will hinder manipulation by MoveIt!:

In order to fix this issue we are going to implement a custom joint position controller that sends a predefined set of joint states to the robot joints in the Gazebo. This will ensure that the robot is in a valid configuration for future attempts of trajectory planning and execution.

Custom Position Control – Fixing invalid Robot States in Gazebo

We are going to implementing a custom controller in our existing ROS package by writing the controller source file and exporting a C++ library which will be used by the controller_manager tool to load the new controller.

We will start our implementation by defining the necessary parameters in the existing configuration file that defines all controllers:

panda_simulation/config/panda_control.yaml

joint_position_controller:
  type: panda_simulation/JointPositionController
  arm_id: panda
  joint_names:
      - panda_joint1
      - panda_joint2
      - panda_joint3
      - panda_joint4
      - panda_joint5
      - panda_joint6
      - panda_joint7
  gains: [
      1, 1, 1, 1, 1, 1, 1
  ]      

We will now define a source file for the controller that loads these parameters through it’s node handle into member variables:

panda_simulation/src/joint_position_controller.cpp

class JointPositionController : public controller_interface::Controller<hardware_interface::PositionJointInterface> {
  bool init(hardware_interface::PositionJointInterface *hw, ros::NodeHandle &n) {
    std::vector<std::string> joint_names;
    if (!n.getParam("joint_names", joint_names)) {
      ROS_ERROR("Could not read joint names from param server");
      return false;
    }

    // retrieve gains
    if (!n.getParam("gains", gains_vec_)) {
      ROS_ERROR("Could not read joint gains from param server");
      return false;
    }

    for (auto &joint_name : joint_names) {
      joint_handles_.push_back(hw->getHandle(joint_name));
    }

    for (auto &joint_handle : joint_handles_) {
      command_.push_back(joint_handle.getPosition());
    }

    sub_command_ = n.subscribe<std_msgs::Float64MultiArray>(std::string("command"), 1,
                                                            &JointPositionController::setCommandCallback, this);

    return true;
  }

  void update(const ros::Time &time, const ros::Duration &period) {
    for (size_t i = 0; i < joint_handles_.size(); i++) {
      double error = command_.at(i) - joint_handles_.at(i).getPosition();
      double commanded_effort = error * gains_vec_.at(i);

      joint_handles_.at(i).setCommand(commanded_effort);
    }
  }

  void setCommandCallback(const std_msgs::Float64MultiArrayConstPtr &msg) { command_ = msg->data; }

  void starting(const ros::Time &time) {}

  void stopping(const ros::Time &time) {}

private:
  std::vector<hardware_interface::JointHandle> joint_handles_;
  std::vector<double> gains_vec_;
  std::vector<double> command_;
  ros::Subscriber sub_command_;
};

PLUGINLIB_EXPORT_CLASS(panda_simulation::JointPositionController, controller_interface::ControllerBase);

A couple of important notes about this class:

• It inherits from an abstract class (controllers_interface) by passing a template type parameter that defines the type of interface that this controller intends to use: hardware_interface::PositionJointInterface
• It overrides the base class methods init, update, starting and stopping
• It subscribes to the topic command and sets the callback setCommandCallback for retrieving the goal angles for all joints of the robot
• The init method is called when the controller_manager loads the controller dynamically at runtime (see below)
• The update method is where the actual work happens: This method is periodically invoked by gazebo_ros_control (see implementation)

Package Configuration

In order to run the controller with controller_manager we need to update the configuration of our ROS package:

panda_simulation/package.xml

We need to add the following dependencies to the package.xml file:

• controller_interface
• hardware_interface
• pluginlib

We also need to define a controller_interface element inside the export tag. This will ensure that controller_manager can find our controller:

<export>
    <controller_interface plugin="${prefix}/controller_plugins.xml" />
</export>

panda_simulation/controller_plugins.xml

This fille lists all controllers with their respective types. This ensures that the controller_manager can load them at runtime with pluginlib by specifying the correct types:

<library path="lib/libpanda_simulation_controllers_lib">
    <class name="panda_simulation/JointPositionController" type="panda_simulation::JointPositionController" base_class_type="controller_interface::ControllerBase" />
</library>

panda_simulation/CMakeLists.txt

In the CMake configuration we need to define a custom library for our custom controller(s) with the respective source files:

# add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)

# Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})

Results

Running the default simulation is straightforward:

source devel/setup.bash
roslaunch panda_simulation simulation.launch

If the robot is in an invalid configuration the MoveIt! motion planning pipeline will not work as expected. In this case, we can load and run our custom controller in order to set valid joint states on the robot’s joints.

First we need to check that the controller can be found by controller_manager:

rospack plugins --attrib=plugin controller_interface | grep panda_simulation

The output should list the following line:

panda_simulation /home/pekel/tmp/ws_panda_simulation/src/panda_simulation/controller_plugins.xml

We can now load the controller. This will call the init method and load the parameters from the ROS parameter server:

rosservice call /controller_manager/load_controller "name: 'joint_position_controller'"

When the response is positive we can switch from the currently running panda_arm_controller to our custom controller. these controllers can’t run simultaneously because this would result in a resource conflict within the simulation because the robot’s joint handle would be acquired by more than one controller.

rosservice call /controller_manager/switch_controller "start_controllers: ['joint_position_controller']
stop_controllers: ['panda_arm_controller']
strictness: 1
start_asap: false
timeout: 0.0"

Now we can send a command to the topic that our controller is subscribed to. The command consists of angles that are copied from the predefined state of the panda robot’s MoveIt! configuration. The controller will append these joint angles to the joints within the update method:

rostopic pub /joint_position_controller/command std_msgs/Float64MultiArray '{data: [0,-0.785,0,-2.356,0,1.571, 0.785]}'

The robot will move to the specified configuration within the Gazebo simulation environment.

We can now switch back to the initial controller panda_arm_controller:

rosservice call /controller_manager/switch_controller "start_controllers: ['panda_arm_controller']
stop_controllers: ['joint_position_controller']
strictness: 1
start_asap: false
timeout: 0.0"

The MoveIt! plugin inside RViz should now be reloaded by clicking the ‘Reset’ button in the bottom-left corner of the window.

It is now possible to plan a new trajectory with the position-based joint trajectory controller:

The custom ROS package as well as the modified franka_ros repository that is needed for the simulation are available in my GitHub profile.