diff --git a/CMakeLists.txt b/CMakeLists.txt
index b4e5ea8a21eed5be5fca29974e97be16e83504b8..ebaf3bfd5722a6bccaf4aeef9024eddac3e5b64c 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/README.md b/README.md
index 5a8cb7eea5ac37f0f5040139d76593c078926d34..92c9475b85e95b17055962ef1c946f6425ec1743 100644
--- a/README.md
+++ b/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.
+
+
\ No newline at end of file
diff --git a/assets/robot-state-initializer.gif b/assets/robot-state-initializer.gif
new file mode 100644
index 0000000000000000000000000000000000000000..2a570fd015bfca8a83b30a569abe35cfa1fb7b89
Binary files /dev/null and b/assets/robot-state-initializer.gif differ
diff --git a/launch/simulation.launch b/launch/simulation.launch
index b3471f2a727774c667f00f318667ca4808d38615..2398a2b207be8f532a3f2abe710a654ec4c562f6 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -1,28 +1,28 @@
<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
diff --git a/src/robot_state_initializer.cpp b/src/robot_state_initializer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1fce35bb27c14181ccb0243fb23c8099311f5e3b
--- /dev/null
+++ b/src/robot_state_initializer.cpp
@@ -0,0 +1,75 @@
+#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