Add automatic robot state initializer.

CMakeLists.txt

@@ -57,9 +57,11 @@ include_directories(${catkin_INCLUDE_DIRS})
 
 add_executable(robot_control_node src/robot_control_node.cpp)
 add_executable(box_publisher_node src/box_publisher.cpp)
+add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
 
 target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
 target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
+target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
 
 # add custom controller as library
 add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)

README.md

@@ -36,14 +36,26 @@ Otherwise, the robot will appear in rviz in a collapsed state.
 
 You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).
 
-## Extension: _MoveIt!_ constraint-aware planning
+## Changelog:
 
-This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _JSON_ files. You can read more about it in the accompanying [blog post](https://erdalpekel.de/?p=123).
+   [_MoveIt!_ constraint-aware planning](https://erdalpekel.de/?p=123)
 
-## Extension: Publishing a box at Panda's hand in _Gazebo_
+   This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _json_ files.
 
-This repository was extended with a node that publishes a simple box object in the _Gazebo_ simulation at the hand of the robot. The position of this box will get updated as soon as the robot moves. You can read more about it in the accompanying [blog post](https://erdalpekel.de/?p=178).
+   [Publishing a box at Panda's hand in _Gazebo_](https://erdalpekel.de/?p=123)
 
-## Extension: Visual Studio Code Remote Docker
+   This repository was extended with a node that publishes a simple box object in the _Gazebo_ simulation at the hand of the robot. The position of this box will get updated as soon as the robot moves.
 
-I have added configuration files and additional setup scripts for developing and using this ROS package within a *Docker* container. Currently user interfaces for Gazebo and RViz are not supported. You can read more about it in the [blog post](https://erdalpekel.de/?p=219).
+   [Visual Studio Code Remote Docker](https://erdalpekel.de/?p=123)
+
+   I have added configuration files and additional setup scripts for developing and using this ROS package within a *Docker* container. Currently user interfaces for Gazebo and RViz are not supported.
+
+   [Position based trajectory execution](https://erdalpekel.de/?p=285)
+
+   The joint specifications in Gazebo were changed from an effort interface to position based interface. Furthermore, the PID controller was substituted with the simple gazebo internal position based control mechanism for a more stable movement profile of the robot.
+
+   [automatic robot state initialization with custom joint position based controller](https://erdalpekel.de/?p=314)
+
+   A custom joint position based controller was implemented in order to set the initial joint states of the robot to a valid configuration. A separate ROS node was implemented that starts this custom controller and initializes the robot with a specific configuration. It switches back to the default controllers after the robot reaches the desired state.
+
+![Panda state initialization in Gazebo](assets/robot-state-initializer.gif?raw=true "Panda state initialization in Gazebo")
\ No newline at end of file

launch/simulation.launch

 <launch>
-    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm_hand.urdf.xacro"/>
+    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
 
     <!-- GAZEBO arguments -->
-    <arg name="paused" default="false"/>
-    <arg name="use_sim_time" default="true"/>
-    <arg name="gui" default="true"/>
-    <arg name="headless" default="false"/>
-    <arg name="debug" default="false"/>
-    <arg name="load_gripper" default="true"/>
+    <arg name="paused" default="false" />
+    <arg name="use_sim_time" default="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="false" />
+    <arg name="load_gripper" default="true" />
 
     <!--launch GAZEBO with own world configuration -->
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
         <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
-        <arg name="debug" value="$(arg debug)"/>
-        <arg name="gui" value="$(arg gui)"/>
-        <arg name="paused" value="$(arg paused)"/>
-        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
-        <arg name="headless" value="$(arg headless)"/>
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="paused" value="$(arg paused)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="headless" value="$(arg headless)" />
     </include>
 
-    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda"/>
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
 
     <!-- Load joint controller configurations from YAML file to parameter server -->
-    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load"/>
+    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
 
 
     <!-- load the controllers -->
@@ -34,7 +34,7 @@
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
     <include file="$(find panda_moveit_config)/launch/planning_context.launch">
-        <arg name="load_robot_description" value="true"/>
+        <arg name="load_robot_description" value="true" />
         <arg name="load_gripper" value="$(arg load_gripper)" />
     </include>
     <include file="$(find panda_moveit_config)/launch/move_group.launch">
@@ -47,6 +47,12 @@
     <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
 
     <!-- launch robot control node for moveit motion planning -->
-    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen"/>
+    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
+
+    <!-- load (not start!) custom joint position controller -->
+    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
+
+    <!-- run custom node for automatic intialization -->
+    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
 
 </launch>
\ No newline at end of file

src/robot_state_initializer.cpp

+#include <controller_manager_msgs/SwitchController.h>
+#include <ros/ros.h>
+#include <std_msgs/Float64MultiArray.h>
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "robot_state_initializer_node");
+  ros::NodeHandle node_handle;
+
+  std::vector<double> panda_ready_state{0, -0.785, 0, -2.356, 0, 1.571, 0.785};
+
+  // define variables
+  std::string joint_position_command_topic{"/joint_position_controller/command"},
+      controller_manager_switch_topic{"/controller_manager/switch_controller"};
+  ros::ServiceClient switch_controller_client =
+      node_handle.serviceClient<controller_manager_msgs::SwitchController>(controller_manager_switch_topic);
+  ros::Publisher joint_position_publisher =
+      node_handle.advertise<std_msgs::Float64MultiArray>(joint_position_command_topic, 1);
+
+  // sleep for 2 seconds in order to make sure that the system is up and running
+  ros::Duration(2.0).sleep();
+
+  // 1. switch from default joint trajectory controller to custom position controller
+  std::string panda_arm_controller{"panda_arm_controller"}, panda_hand_controller{"panda_hand_controller"},
+      joint_position_controller{"joint_position_controller"};
+  std::vector<std::string> stop_controllers{panda_arm_controller, panda_hand_controller};
+  std::vector<std::string> start_controllers{joint_position_controller};
+
+  controller_manager_msgs::SwitchController srv_switch_controller;
+  srv_switch_controller.request.stop_controllers = stop_controllers;
+  srv_switch_controller.request.start_controllers = start_controllers;
+  srv_switch_controller.request.strictness = controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT;
+
+  // housekeeping for logging
+  std::ostringstream stream_start_controllers, stream_stop_controllers;
+  for (const auto &elem : start_controllers) {
+    stream_start_controllers << elem << ", ";
+  }
+  for (const auto &elem : stop_controllers) {
+    stream_stop_controllers << elem << ", ";
+  }
+
+  if (switch_controller_client.call(srv_switch_controller)) {
+    ROS_INFO_STREAM("Success switching controllers from " << stream_stop_controllers.str() << " to "
+                                                          << stream_start_controllers.str());
+  } else {
+    ROS_WARN_STREAM("Error switching controllers from " << stream_stop_controllers.str() << " to "
+                                                        << stream_start_controllers.str());
+    return -1;
+  }
+
+  // 2. publish the desired joint position to the custom controller
+  std_msgs::Float64MultiArray command_msg;
+  command_msg.data = panda_ready_state;
+  joint_position_publisher.publish(command_msg);
+
+  // sleep for 1 seconds in order to make sure that the controller finishes moving the robot
+  ros::Duration(1.0).sleep();
+
+  // 3. Restore default controllers
+  std::swap(stop_controllers, start_controllers);
+  srv_switch_controller.request.stop_controllers = stop_controllers;
+  srv_switch_controller.request.start_controllers = start_controllers;
+
+  if (switch_controller_client.call(srv_switch_controller)) {
+    ROS_INFO_STREAM("Success switching controllers from " << stream_stop_controllers.str() << " to "
+                                                          << stream_start_controllers.str());
+  } else {
+    ROS_WARN_STREAM("Error switching controllers from " << stream_stop_controllers.str() << " to "
+                                                        << stream_start_controllers.str());
+    return -1;
+  }
+
+  ros::spin();
+  return 0;
+}
\ No newline at end of file