{"id":314,"date":"2020-03-20T09:19:03","date_gmt":"2020-03-20T09:19:03","guid":{"rendered":"https:\/\/erdalpekel.de\/?p=314"},"modified":"2021-02-13T09:22:41","modified_gmt":"2021-02-13T09:22:41","slug":"automatic-robot-state-initialization-in-gazebo","status":"publish","type":"post","link":"https:\/\/erdalpekel.de\/?p=314","title":{"rendered":"Automatic robot state initialization in Gazebo"},"content":{"rendered":"\n

In the last blog post we introduced position based control of the FRANKA EMIKA Panda robot in the Gazebo simulation environment for a more stable movement profile. We also introduced a custom joint position controller that can be used to manually command the joints within the simulation to a desired position in order to prevent motion planning initialization errors that are caused by invalid joint angle configurations after spawning the robot in the Gazebo world.<\/p>\n\n\n\n

In this post we are going to implement a ROS node that will automatically reset the robot state at startup in contrast to the manual terminal commands in the last post and therefore simplify the usage of the simulation package overall.<\/p>\n\n\n\n

A quick side note: The current package configuration causes no p gain specified for pid errors. The gain parameters are not specified intentionally. If we were to specify gains, the gazebo robot hardware initializer would employ a PID controller, which in turn requires parameter tuning for smooth operation. If we however do not specify gain parameters the initialization will fall back to custom position control mechanism which is more stable (see implementation details 1<\/a> 2<\/a>).<\/p>\n\n\n\n

Robot state initalization<\/h2>\n\n\n\n

We will start off by adding the dependencies to the package.xml and CMakeLists.txt file and add the new node robot_state_initializer_node<\/em> to the CMake configuration:<\/p>\n\n\n\n

find_package(catkin REQUIRED\n             COMPONENTS controller_manager ...)\n\nadd_executable(robot_state_initializer_node src\/robot_state_initializer.cpp)\n\ntarget_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})<\/code><\/pre>\n\n\n\n
The controller_manager<\/em> dependency is needed for the custom message types that will be used for programmatic controller switching within the new ROS node. It should also be added to the dependencies in the package.xml file. In our case it is already there.<\/p>\n\n\n\n
We will now create the file robot_state_initializer.cpp:<\/p>\n\n\n\n
#include <controller_manager_msgs\/SwitchController.h>\n#include <ros\/ros.h>\n#include <std_msgs\/Float64MultiArray.h>\n\nint main(int argc, char **argv) {\n  ros::init(argc, argv, \"robot_state_initializer_node\");\n  ros::NodeHandle node_handle;\n\n  std::vector<double> panda_ready_state{0, -0.785, 0, -2.356, 0, 1.571, 0.785};\n\n  \/\/ define variables\n  std::string joint_position_command_topic{\"\/joint_position_controller\/command\"},\n      controller_manager_switch_topic{\"\/controller_manager\/switch_controller\"};\n  ros::ServiceClient switch_controller_client =\n      node_handle.serviceClient<controller_manager_msgs::SwitchController>(controller_manager_switch_topic);\n  ros::Publisher joint_position_publisher =\n      node_handle.advertise<std_msgs::Float64MultiArray>(joint_position_command_topic, 1);\n\n  \/\/ sleep for 2 seconds in order to make sure that the system is up and running\n  ros::Duration(2.0).sleep();\n\n  \/\/ 1. switch from default joint trajectory controller to custom position controller\n  std::string panda_arm_controller{\"panda_arm_controller\"}, panda_hand_controller{\"panda_hand_controller\"},\n      joint_position_controller{\"joint_position_controller\"};\n  std::vector<std::string> stop_controllers{panda_arm_controller, panda_hand_controller};\n  std::vector<std::string> start_controllers{joint_position_controller};\n\n  controller_manager_msgs::SwitchController srv_switch_controller;\n  srv_switch_controller.request.stop_controllers = stop_controllers;\n  srv_switch_controller.request.start_controllers = start_controllers;\n  srv_switch_controller.request.strictness = controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT;\n\n  \/\/ housekeeping for logging\n  std::ostringstream stream_start_controllers, stream_stop_controllers;\n  for (const auto &elem : start_controllers) {\n    stream_start_controllers << elem << \", \";\n  }\n  for (const auto &elem : stop_controllers) {\n    stream_stop_controllers << elem << \", \";\n  }\n\n  if (switch_controller_client.call(srv_switch_controller)) {\n    ROS_INFO_STREAM(\"Success switching controllers from \" << stream_stop_controllers.str() << \" to \"\n                                                          << stream_start_controllers.str());\n  } else {\n    ROS_WARN_STREAM(\"Error switching controllers from \" << stream_stop_controllers.str() << \" to \"\n                                                        << stream_start_controllers.str());\n    return -1;\n  }\n\n  \/\/ 2. publish the desired joint position to the custom controller\n  std_msgs::Float64MultiArray command_msg;\n  command_msg.data = panda_ready_state;\n  joint_position_publisher.publish(command_msg);\n\n  \/\/ sleep for 1 seconds in order to make sure that the controller finishes moving the robot\n  ros::Duration(1.0).sleep();\n\n  \/\/ 3. Restore default controllers\n  std::swap(stop_controllers, start_controllers);\n  srv_switch_controller.request.stop_controllers = stop_controllers;\n  srv_switch_controller.request.start_controllers = start_controllers;\n\n  if (switch_controller_client.call(srv_switch_controller)) {\n    ROS_INFO_STREAM(\"Success switching controllers from \" << stream_stop_controllers.str() << \" to \"\n                                                          << stream_start_controllers.str());\n  } else {\n    ROS_WARN_STREAM(\"Error switching controllers from \" << stream_stop_controllers.str() << \" to \"\n                                                        << stream_start_controllers.str());\n    return -1;\n  }\n\n  ros::spin();\n  return 0;\n}<\/code><\/pre>\n\n\n\n
The following steps are carried out successively:<\/p>\n\n\n\n