{"id":285,"date":"2020-02-29T20:01:32","date_gmt":"2020-02-29T20:01:32","guid":{"rendered":"https:\/\/erdalpekel.de\/?p=285"},"modified":"2020-03-20T08:02:14","modified_gmt":"2020-03-20T08:02:14","slug":"improving-panda-gazebo-integration-with-position-based-control","status":"publish","type":"post","link":"https:\/\/erdalpekel.de\/?p=285","title":{"rendered":"Improving Panda Gazebo integration with Position based Control"},"content":{"rendered":"\n<p>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.<\/p>\n\n\n\n<p>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.<\/p>\n\n\n\n<p>The improvements will be based on the GitHub repository that was also used in the previous posts. You can check it out here.<\/p>\n\n\n\n<h2 class=\"wp-block-heading\">Position-based Joint Trajectory Execution<\/h2>\n\n\n\n<p>We will alter the configurations for the robot in the packages panda_simulation and franka_ros in order to migrate to position based control.<\/p>\n\n\n\n<p>The following files need to be changed:<\/p>\n\n\n\n<h4 class=\"wp-block-heading\">franka_ros\/franka_description\/robots\/panda.transmission.xacro<\/h4>\n\n\n\n<p>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&#8217;s joints when spawning the robot (see <a href=\"https:\/\/github.com\/ros-simulation\/gazebo_ros_pkgs\/blob\/070cf5b3b3d528d438a5a925b0592eadf03f780b\/gazebo_ros_control\/src\/default_robot_hw_sim.cpp#L161\">implementation<\/a>).<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"xml\" class=\"language-xml line-numbers\">&lt;transmission name=\"${robot_name}_tran_1\">\n            &lt;type>transmission_interface\/SimpleTransmission&lt;\/type>\n            &lt;joint name=\"${robot_name}_joint1\">\n                &lt;hardwareInterface>hardware_interface\/PositionJointInterface&lt;\/hardwareInterface>\n            &lt;\/joint>\n            &lt;actuator name=\"${robot_name}_motor_1\">\n                &lt;hardwareInterface>hardware_interface\/PositionJointInterface&lt;\/hardwareInterface>\n                &lt;mechanicalReduction>1&lt;\/mechanicalReduction>\n            &lt;\/actuator>\n        &lt;\/transmission><\/code><\/pre>\n\n\n\n<h4 class=\"wp-block-heading\">panda_simulation\/config\/panda_control.yaml<\/h4>\n\n\n\n<p>In our configuration parameters we are going to increase the publishing rate of the arm and state controllers to <em>100<\/em> and replace <em>effort_controller\/JointTrajectoryController<\/em> with <em>position_controllers\/JointTrajectoryController<\/em>. The last change only needs to be applied to <em>panda_arm_controller<\/em>. We can also remove the manual gains as they are not needed for the position based joint commands in Gazebo.<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"yaml\" class=\"language-yaml line-numbers\">joint_state_controller:\n  type: joint_state_controller\/JointStateController\n  publish_rate: 100\n\npanda_arm_controller:\n  type: position_controllers\/JointTrajectoryController\n  joints:\n    - panda_joint1\n    - panda_joint2\n    - panda_joint3\n    - panda_joint4\n    - panda_joint5\n    - panda_joint6\n    - panda_joint7\n\n  constraints:\n    goal_time: 2.0\n\n  state_publish_rate: 100<\/code><\/pre>\n\n\n\n<p>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!:<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img fetchpriority=\"high\" decoding=\"async\" width=\"871\" height=\"765\" src=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture2.png\" alt=\"\" class=\"wp-image-294\" srcset=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture2.png 871w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture2-300x263.png 300w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture2-768x675.png 768w\" sizes=\"(max-width: 871px) 100vw, 871px\" \/><figcaption>Invalid robot configuration in Gazebo seen by colliding links.<\/figcaption><\/figure>\n\n\n\n<figure class=\"wp-block-image size-large\"><img decoding=\"async\" width=\"1024\" height=\"783\" src=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture-1024x783.png\" alt=\"\" class=\"wp-image-293\" srcset=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture-1024x783.png 1024w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture-300x229.png 300w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture-768x587.png 768w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture.png 1192w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><figcaption>Invalid robot configuration highlighted by the motion planning framework in RViz.<\/figcaption><\/figure>\n\n\n\n<p>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.<\/p>\n\n\n\n<h2 class=\"wp-block-heading\">Custom Position Control &#8211; Fixing invalid Robot States in Gazebo<\/h2>\n\n\n\n<p>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 <a href=\"http:\/\/wiki.ros.org\/controller_manager\"><em>controller_manager<\/em> <\/a>tool to load the new controller.<\/p>\n\n\n\n<p>We will start our implementation by defining the necessary parameters in the existing configuration file that defines all controllers:<\/p>\n\n\n\n<h4 class=\"wp-block-heading\"> panda_simulation\/config\/panda_control.yaml<\/h4>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"yaml\" class=\"language-yaml line-numbers\">joint_position_controller:\n  type: panda_simulation\/JointPositionController\n  arm_id: panda\n  joint_names:\n      - panda_joint1\n      - panda_joint2\n      - panda_joint3\n      - panda_joint4\n      - panda_joint5\n      - panda_joint6\n      - panda_joint7\n  gains: [\n      1, 1, 1, 1, 1, 1, 1\n  ]      <\/code><\/pre>\n\n\n\n<p>We will now define a source file for the controller that loads these parameters through it&#8217;s node handle into member variables:<\/p>\n\n\n\n<h4 class=\"wp-block-heading\">panda_simulation\/src\/joint_position_controller.cpp<\/h4>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"cpp\" class=\"language-cpp line-numbers\">class JointPositionController : public controller_interface::Controller&lt;hardware_interface::PositionJointInterface> {\n  bool init(hardware_interface::PositionJointInterface *hw, ros::NodeHandle &amp;n) {\n    std::vector&lt;std::string> joint_names;\n    if (!n.getParam(\"joint_names\", joint_names)) {\n      ROS_ERROR(\"Could not read joint names from param server\");\n      return false;\n    }\n\n    \/\/ retrieve gains\n    if (!n.getParam(\"gains\", gains_vec_)) {\n      ROS_ERROR(\"Could not read joint gains from param server\");\n      return false;\n    }\n\n    for (auto &amp;joint_name : joint_names) {\n      joint_handles_.push_back(hw->getHandle(joint_name));\n    }\n\n    for (auto &amp;joint_handle : joint_handles_) {\n      command_.push_back(joint_handle.getPosition());\n    }\n\n    sub_command_ = n.subscribe&lt;std_msgs::Float64MultiArray>(std::string(\"command\"), 1,\n                                                            &amp;JointPositionController::setCommandCallback, this);\n\n    return true;\n  }\n\n  void update(const ros::Time &amp;time, const ros::Duration &amp;period) {\n    for (size_t i = 0; i &lt; joint_handles_.size(); i++) {\n      double error = command_.at(i) - joint_handles_.at(i).getPosition();\n      double commanded_effort = error * gains_vec_.at(i);\n\n      joint_handles_.at(i).setCommand(commanded_effort);\n    }\n  }\n\n  void setCommandCallback(const std_msgs::Float64MultiArrayConstPtr &amp;msg) { command_ = msg->data; }\n\n  void starting(const ros::Time &amp;time) {}\n\n  void stopping(const ros::Time &amp;time) {}\n\nprivate:\n  std::vector&lt;hardware_interface::JointHandle> joint_handles_;\n  std::vector&lt;double> gains_vec_;\n  std::vector&lt;double> command_;\n  ros::Subscriber sub_command_;\n};\n\nPLUGINLIB_EXPORT_CLASS(panda_simulation::JointPositionController, controller_interface::ControllerBase);<\/code><\/pre>\n\n\n\n<p>A couple of important notes about this class:<\/p>\n\n\n\n<ul class=\"wp-block-list\"><li>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: <em>hardware_interface::PositionJointInterface<\/em><\/li><li>It overrides the base class methods <em>init, update, starting <\/em>and <em>stopping<\/em><\/li><li>It subscribes to the topic command and sets the callback  <em>setCommandCallback<\/em> for retrieving the goal angles for all joints of the robot<\/li><li>The <em>init <\/em>method is called when the <em>controller_manager <\/em>loads the controller dynamically at runtime (see below)<\/li><li>The <em>update <\/em>method is where the actual work happens: This method is periodically invoked by <em>gazebo_ros_control<\/em> (see <a href=\"https:\/\/github.com\/ros-simulation\/gazebo_ros_pkgs\/blob\/070cf5b3b3d528d438a5a925b0592eadf03f780b\/gazebo_ros_control\/src\/gazebo_ros_control_plugin.cpp#L251\">implementation<\/a>)<\/li><\/ul>\n\n\n\n<h3 class=\"wp-block-heading\">Package Configuration<\/h3>\n\n\n\n<p>In order to run the controller with <em>controller_manager<\/em> we need to update the configuration of our ROS package:<\/p>\n\n\n\n<h4 class=\"wp-block-heading\">panda_simulation\/package.xml<\/h4>\n\n\n\n<p>We need to add the following dependencies to the <em>package.xml<\/em> file:<\/p>\n\n\n\n<ul class=\"wp-block-list\"><li>controller_interface<\/li><li>hardware_interface<\/li><li>pluginlib<\/li><\/ul>\n\n\n\n<p>We also need to define a controller_interface element inside the export tag. This will ensure that controller_manager can find our controller:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"xml\" class=\"language-xml line-numbers\">&lt;export>\n    &lt;controller_interface plugin=\"${prefix}\/controller_plugins.xml\" \/>\n&lt;\/export><\/code><\/pre>\n\n\n\n<h4 class=\"wp-block-heading\">panda_simulation\/controller_plugins.xml<\/h4>\n\n\n\n<p>This fille lists all controllers with their respective types. This ensures that the <em>controller_manager <\/em>can load them at runtime with <em><a href=\"http:\/\/wiki.ros.org\/pluginlib\">pluginlib <\/a><\/em>by specifying the correct types:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"xml\" class=\"language-xml line-numbers\">&lt;library path=\"lib\/libpanda_simulation_controllers_lib\">\n    &lt;class name=\"panda_simulation\/JointPositionController\" type=\"panda_simulation::JointPositionController\" base_class_type=\"controller_interface::ControllerBase\" \/>\n&lt;\/library><\/code><\/pre>\n\n\n\n<h4 class=\"wp-block-heading\">panda_simulation\/CMakeLists.txt<\/h4>\n\n\n\n<p>In the CMake configuration we need to define a custom library for our custom controller(s) with the respective source files:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"vim\" class=\"language-vim line-numbers\"># add custom controller as library\nadd_library(${PROJECT_NAME}_controllers_lib src\/joint_position_controller.cpp)\n\n# Specify libraries to link a library or executable target against\ntarget_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})<\/code><\/pre>\n\n\n\n<h2 class=\"wp-block-heading\">Results<\/h2>\n\n\n\n<p>Running the default simulation is straightforward:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"bash\" class=\"language-bash\">source devel\/setup.bash\nroslaunch panda_simulation simulation.launch<\/code><\/pre>\n\n\n\n<p>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&#8217;s joints.<\/p>\n\n\n\n<p>First we need to check that the controller can be found by <em>controller_manager<\/em>:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"bash\" class=\"language-bash\">rospack plugins --attrib=plugin controller_interface | grep panda_simulation<\/code><\/pre>\n\n\n\n<p>The output should list the following line:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"vim\" class=\"language-vim\">panda_simulation \/home\/pekel\/tmp\/ws_panda_simulation\/src\/panda_simulation\/controller_plugins.xml<\/code><\/pre>\n\n\n\n<p>We can now load the controller. This will call the init method and load the parameters from the ROS parameter server:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"bash\" class=\"language-bash\">rosservice call \/controller_manager\/load_controller \"name: 'joint_position_controller'\"<\/code><\/pre>\n\n\n\n<p>When the response is positive we can switch from the currently running panda_arm_controller to our custom controller. these controllers can&#8217;t run simultaneously because this would result in a resource conflict within the simulation because the robot&#8217;s joint handle would be acquired by more than one controller.<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"bash\" class=\"language-bash\">rosservice call \/controller_manager\/switch_controller \"start_controllers: ['joint_position_controller']\nstop_controllers: ['panda_arm_controller']\nstrictness: 1\nstart_asap: false\ntimeout: 0.0\"<\/code><\/pre>\n\n\n\n<p>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&#8217;s <a href=\"https:\/\/github.com\/erdalpekel\/panda_moveit_config\/blob\/a145fc96ff906f03c5bf818b32108d0422c2cf3c\/config\/panda_arm.xacro#L13\">MoveIt! configuration<\/a>. The controller will append these joint angles to the joints within the update method:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"bash\" class=\"language-bash\">rostopic pub \/joint_position_controller\/command std_msgs\/Float64MultiArray '{data: [0,-0.785,0,-2.356,0,1.571, 0.785]}'<\/code><\/pre>\n\n\n\n<p>The robot will move to the specified configuration within the Gazebo simulation environment.<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img decoding=\"async\" width=\"795\" height=\"766\" src=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture3.png\" alt=\"\" class=\"wp-image-301\" srcset=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture3.png 795w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture3-300x289.png 300w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture3-768x740.png 768w\" sizes=\"(max-width: 795px) 100vw, 795px\" \/><figcaption>Panda robot commanded to a valid state by custom controller.<\/figcaption><\/figure>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"853\" src=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture4-1024x853.png\" alt=\"\" class=\"wp-image-302\" srcset=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture4-1024x853.png 1024w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture4-300x250.png 300w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture4-768x640.png 768w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture4.png 1088w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><figcaption> Panda robot commanded to a valid state by custom controller &#8211; <em>MoveIt!<\/em> accepts new configurations.<\/figcaption><\/figure>\n\n\n\n<p>We can now switch back to the initial controller <em>panda_arm_controller<\/em>:<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code lang=\"bash\" class=\"language-bash\">rosservice call \/controller_manager\/switch_controller \"start_controllers: ['panda_arm_controller']\nstop_controllers: ['joint_position_controller']\nstrictness: 1\nstart_asap: false\ntimeout: 0.0\"<\/code><\/pre>\n\n\n\n<p>The MoveIt! plugin inside RViz should now be reloaded by clicking the &#8216;Reset&#8217; button in the bottom-left corner of the window.<\/p>\n\n\n\n<p>It is now possible to plan a new trajectory with the position-based joint trajectory controller:<\/p>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"1024\" height=\"450\" src=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture5-1024x450.png\" alt=\"\" class=\"wp-image-303\" srcset=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture5-1024x450.png 1024w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture5-300x132.png 300w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture5-768x338.png 768w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture5.png 1272w\" sizes=\"(max-width: 1024px) 100vw, 1024px\" \/><figcaption> Panda robot commanded to a valid state by custom controller &#8211; <em>MoveIt!<\/em> motion planning possible. <\/figcaption><\/figure>\n\n\n\n<figure class=\"wp-block-image size-large\"><img loading=\"lazy\" decoding=\"async\" width=\"751\" height=\"704\" src=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture6.png\" alt=\"\" class=\"wp-image-304\" srcset=\"https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture6.png 751w, https:\/\/erdalpekel.de\/wp-content\/uploads\/2020\/02\/Capture6-300x281.png 300w\" sizes=\"(max-width: 751px) 100vw, 751px\" \/><figcaption> Panda robot commanded to a valid state by custom controller &#8211; <em> <\/em>Robot executed new trajectory.<\/figcaption><\/figure>\n\n\n\n<p>The custom <a rel=\"noreferrer noopener\" href=\"https:\/\/github.com\/erdalpekel\/panda_simulation\" target=\"_blank\">ROS package<\/a> as well as the modified <a rel=\"noreferrer noopener\" href=\"https:\/\/github.com\/erdalpekel\/franka_ros\" target=\"_blank\">franka_ros<\/a> repository that is needed for the simulation are available in my <a rel=\"noreferrer noopener\" href=\"https:\/\/github.com\/erdalpekel\" target=\"_blank\">GitHub profile<\/a>.<\/p>\n","protected":false},"excerpt":{"rendered":"<p>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.<\/p>\n","protected":false},"author":1,"featured_media":286,"comment_status":"closed","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1],"tags":[],"class_list":["post-285","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-uncategorized"],"_links":{"self":[{"href":"https:\/\/erdalpekel.de\/index.php?rest_route=\/wp\/v2\/posts\/285","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/erdalpekel.de\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/erdalpekel.de\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/erdalpekel.de\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/erdalpekel.de\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=285"}],"version-history":[{"count":21,"href":"https:\/\/erdalpekel.de\/index.php?rest_route=\/wp\/v2\/posts\/285\/revisions"}],"predecessor-version":[{"id":317,"href":"https:\/\/erdalpekel.de\/index.php?rest_route=\/wp\/v2\/posts\/285\/revisions\/317"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/erdalpekel.de\/index.php?rest_route=\/wp\/v2\/media\/286"}],"wp:attachment":[{"href":"https:\/\/erdalpekel.de\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=285"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/erdalpekel.de\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=285"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/erdalpekel.de\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=285"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}