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.
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.
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 2).
Robot state initalization
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 to the CMake configuration:
find_package(catkin REQUIRED
COMPONENTS controller_manager ...)
add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})The controller_manager 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.
We will now create the file 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;
}The following steps are carried out successively:
- Define the robot’s ready state in a std::vector container.
- Wait for two seconds in order to make sure that the simulation is ready.
- Create a controller_manager_msgs:SwitchController type message and populate it with the vectors for the controllers that need to be stopped and the ones that should be started. It is important to distinguish between starting a controller and loading it. The controller needs to be loaded by the controller manager in order to be available for starting with this procedure. We will explain how to load it beforehand in the next paragraph within the launch file.
- If controller switching was successful we send a message of type std_msgs::Float64MultiArray populated with the desired robot configuration to our custom joint position controller via the topic /joint_position_controller/command
- We let the node sleep for a second and thereby make sure that the controller has enough time to execute our command
- In a final step we repeat the controller switching procedure but swap the contents of the controllers to be started and stopped with std::swap
We will also extend the launch file simulation.launch for automatic startup of the new node with the simulation:
<!-- 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" />Results
We can now launch our simulation with:
roslaunch panda_simulation simulation.launch
Sometimes the MoveIt! plugin in the RViz window needs to be reset for regular use:
Regular motion planning tasks can now be carried out.
The custom ROS package as well as the modified franka_ros repository that is needed for the simulation are available in my GitHub profile.