diff --git a/.ci/Dockerfile b/.ci/Dockerfile
index 1846ed32ddb99e38481179408f2d69d328322b09..5f4e55bcf180177d12be6cb5d99567401229fe88 100644
--- a/.ci/Dockerfile
+++ b/.ci/Dockerfile
@@ -1,6 +1,7 @@
FROM osrf/ros:kinetic-desktop-xenial
RUN apt-get update && apt-get install -y \
- clang \
- clang-format \
- clang-tidy \
- ros-kinetic-ros-control
+ clang-5.0 \
+ clang-format-5.0 \
+ clang-tidy-5.0 \
+ ros-kinetic-ros-control \
+ && rm -rf /var/lib/apt/lists/*
diff --git a/CHANGELOG.md b/CHANGELOG.md
index d50473f794c80f5ff771769e99cd6311d5324cc1..6c00a42f9bf23a4f7608171d4c6bb0c57b8b0c51 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -4,15 +4,18 @@
Requires `libfranka` >= 0.5.0
+ * **BREAKING** Fixes for MoveIt, improving robot performance:
+ * Fixed joint velocity and acceleration limits in `joint_limits.yaml`
+ * Use desired joint state for move group
* **BREAKING** Updated joint limits in URDF
* **BREAKING** Fixed velocity, acceleration and jerk limits in `franka_hw`
+ * **BREAKING** Start `franka_gripper_node` when giving `load_gripper:=true` to `franka_control.launch`
* Allow to configure rate limiting, filtering and internal controller in `franka_control_node`
- * **BREAKING** Enabled limiting and low-pass filtering by default (`franka_control_node.yaml`)
+ * **BREAKING** `FrankaHW::FrankaHW` takes additional parameters.
+ * **BREAKING** Enabled rate limiting and low-pass filtering by default (`franka_control_node.yaml`)
* Publish desired joint state in `/joint_state_desired`
* Removed `effort_joint_trajectory_controller` from `default_controllers.yaml`
- * **BREAKING** Fixes for MoveIt:
- * Fixed joint velocity and acceleration limits in `joint_limits.yaml`
- * Use desired joint state for move group
+ * Fixed a bug when switching between controllers using the same `libfranka` interface
## 0.5.0 - 2018-06-28
diff --git a/cmake/ClangTools.cmake b/cmake/ClangTools.cmake
index 7571da91d9af17553432c2c3078033ac4dffbf47..5166474f207d3e63158c83939cf0ab6e100e3377 100644
--- a/cmake/ClangTools.cmake
+++ b/cmake/ClangTools.cmake
@@ -1,11 +1,11 @@
include(CMakeParseArguments)
-find_program(CLANG_FORMAT_PROG clang-format DOC "'clang-format' executable")
+find_program(CLANG_FORMAT_PROG clang-format-5.0 DOC "'clang-format' executable")
if(CLANG_FORMAT_PROG AND NOT TARGET format)
add_custom_target(format)
add_custom_target(check-format)
endif()
-find_program(CLANG_TIDY_PROG clang-tidy DOC "'clang-tidy' executable")
+find_program(CLANG_TIDY_PROG clang-tidy-5.0 DOC "'clang-tidy' executable")
if(CLANG_TIDY_PROG AND NOT TARGET tidy)
if(NOT CMAKE_EXPORT_COMPILE_COMMANDS)
message(WARNING "Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'
@@ -46,7 +46,7 @@ function(add_tidy_target _target)
cmake_parse_arguments(ARG "" "" "FILES;DEPENDS" ${ARGN})
add_custom_target(tidy-${_target}
- COMMAND ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
+ COMMAND ${CLANG_TIDY_PROG} -fix -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
DEPENDS ${ARG_DEPENDS}
COMMENT "Running clang-tidy for ${_target}"
diff --git a/franka_control/config/franka_control_node.yaml b/franka_control/config/franka_control_node.yaml
index 7b9a67825386e3e3012983365758bb2c8173406a..fd2524afc14bc81163d105843f6eddd57436117a 100644
--- a/franka_control/config/franka_control_node.yaml
+++ b/franka_control/config/franka_control_node.yaml
@@ -7,11 +7,9 @@ joint_names:
- panda_joint6
- panda_joint7
arm_id: panda
-# Rate limiter activation flag for each interface [true|false]
-# [Joint position, Joint velocity, Cartesian Pose, Cartesian Velocity, Torque]
-rate_limiting: [true, true, true, true, true]
-# Cutoff frequency of the lowpass filter for each interface. Set to >= 1000 to deactivate.
-# [Joint position, Joint velocity, Cartesian Pose, Cartesian Velocity, Torque]
-cutoff_freq: [100, 100, 100, 100, 100]
+# Activate rate limiter? [true|false]
+rate_limiting: true
+# Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+cutoff_frequency: 100
# Internal controller for motion generators [joint_impedance|cartesian_impedance]
internal_controller: joint_impedance
diff --git a/franka_control/include/franka_control/franka_state_controller.h b/franka_control/include/franka_control/franka_state_controller.h
index b2c8fd49ea13453b92454ab1bf936572504facf4..6fe42947309ba715bbc052c00349d42a78307cbc 100644
--- a/franka_control/include/franka_control/franka_state_controller.h
+++ b/franka_control/include/franka_control/franka_state_controller.h
@@ -26,7 +26,7 @@ class FrankaStateController
/**
* Creates an instance of a FrankaStateController.
*/
- FrankaStateController();
+ FrankaStateController() = default;
/**
* Initializes the controller with interfaces and publishers.
@@ -40,11 +40,11 @@ class FrankaStateController
ros::NodeHandle& controller_node_handle) override;
/**
- * Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it.
- *
- * @param[in] time Current ROS time.
- * @param[in] period Time since the last update.
- */
+ * Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it.
+ *
+ * @param[in] time Current ROS time.
+ * @param[in] period Time since the last update.
+ */
void update(const ros::Time& time, const ros::Duration& period) override;
private:
@@ -55,8 +55,8 @@ class FrankaStateController
std::string arm_id_;
- franka_hw::FrankaStateInterface* franka_state_interface_;
- std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_;
+ franka_hw::FrankaStateInterface* franka_state_interface_{};
+ std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_{};
realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_;
realtime_tools::RealtimePublisher<franka_msgs::FrankaState> publisher_franka_states_;
diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch
index 4ef1f1aaa320db6273f006840b288f550d088c0c..37adda84e725375094308fb5417cf1547b939dc3 100644
--- a/franka_control/launch/franka_control.launch
+++ b/franka_control/launch/franka_control.launch
@@ -6,6 +6,10 @@
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />
+ <include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
+ <arg name="robot_ip" value="$(arg robot_ip)" />
+ </include>
+
<node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
<rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
<param name="robot_ip" value="$(arg robot_ip)" />
diff --git a/franka_control/package.xml b/franka_control/package.xml
index 42b0f05697b1c8eebc5470c50e0cb461a0465b34..026d63157658cdb7d4e2bfe188e49c5d96bbc7cb 100644
--- a/franka_control/package.xml
+++ b/franka_control/package.xml
@@ -31,6 +31,7 @@
<depend>tf</depend>
<exec_depend>franka_description</exec_depend>
+ <exec_depend>franka_gripper</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
diff --git a/franka_control/src/franka_control_node.cpp b/franka_control/src/franka_control_node.cpp
index 2b092411d809d22b389b8ebb9098793f24293dfc..ce116d5e7983749afe4db99afe3fa052e5e66d8b 100644
--- a/franka_control/src/franka_control_node.cpp
+++ b/franka_control/src/franka_control_node.cpp
@@ -40,38 +40,30 @@ int main(int argc, char** argv) {
return 1;
}
- std::vector<bool> limit_rate_vector;
- if (!node_handle.getParam("rate_limiting", limit_rate_vector) || limit_rate_vector.size() != 5) {
- ROS_ERROR("Invalid or no rate_limiting parameters provided");
+ std::array<std::string, 7> joint_names;
+ std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
+
+ bool rate_limiting;
+ if (!node_handle.getParamCached("rate_limiting", rate_limiting)) {
+ ROS_ERROR("Invalid or no rate_limiting parameter provided");
return 1;
}
- std::vector<double> cutoff_freq_vector;
- if (!node_handle.getParam("cutoff_freq", cutoff_freq_vector) || cutoff_freq_vector.size() != 5) {
- ROS_ERROR("Invalid or no cutoff_freq parameters provided");
+ double cutoff_frequency;
+ if (!node_handle.getParamCached("cutoff_frequency", cutoff_frequency)) {
+ ROS_ERROR("Invalid or no cutoff_frequency parameter provided");
return 1;
}
- franka::ControllerMode internal_controller;
- std::string internal_controller_name;
- if (!node_handle.getParam("internal_controller", internal_controller_name)) {
+ std::string internal_controller;
+ if (!node_handle.getParam("internal_controller", internal_controller)) {
ROS_ERROR("No internal_controller parameter provided");
return 1;
}
- std::array<std::string, 7> joint_names;
- std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
- std::array<bool, 5> rate_limiting;
- std::copy(limit_rate_vector.cbegin(), limit_rate_vector.cend(), rate_limiting.begin());
- std::array<double, 5> cutoff_freq;
- std::copy(cutoff_freq_vector.cbegin(), cutoff_freq_vector.cend(), cutoff_freq.begin());
-
- if (internal_controller_name == "joint_impedance") {
- internal_controller = franka::ControllerMode::kJointImpedance;
- } else if (internal_controller_name == "cartesian_impedance") {
- internal_controller = franka::ControllerMode::kCartesianImpedance;
- } else {
- ROS_ERROR("Invalid internal_controller parameter provided");
+ urdf::Model urdf_model;
+ if (!urdf_model.initParamWithNodeHandle("robot_description", public_node_handle)) {
+ ROS_ERROR("Could not initialize URDF model from robot_description");
return 1;
}
@@ -97,29 +89,37 @@ int main(int argc, char** argv) {
std::atomic_bool has_error(false);
- using std::placeholders::_1;
- using std::placeholders::_2;
ServiceContainer services;
services
.advertiseService<franka_control::SetJointImpedance>(
node_handle, "set_joint_impedance",
- std::bind(franka_control::setJointImpedance, std::ref(robot), _1, _2))
+ [&robot](auto&& req, auto&& res) {
+ return franka_control::setJointImpedance(robot, req, res);
+ })
.advertiseService<franka_control::SetCartesianImpedance>(
node_handle, "set_cartesian_impedance",
- std::bind(franka_control::setCartesianImpedance, std::ref(robot), _1, _2))
+ [&robot](auto&& req, auto&& res) {
+ return franka_control::setCartesianImpedance(robot, req, res);
+ })
.advertiseService<franka_control::SetEEFrame>(
node_handle, "set_EE_frame",
- std::bind(franka_control::setEEFrame, std::ref(robot), _1, _2))
+ [&robot](auto&& req, auto&& res) { return franka_control::setEEFrame(robot, req, res); })
.advertiseService<franka_control::SetKFrame>(
- node_handle, "set_K_frame", std::bind(franka_control::setKFrame, std::ref(robot), _1, _2))
+ node_handle, "set_K_frame",
+ [&robot](auto&& req, auto&& res) { return franka_control::setKFrame(robot, req, res); })
.advertiseService<franka_control::SetForceTorqueCollisionBehavior>(
node_handle, "set_force_torque_collision_behavior",
- std::bind(franka_control::setForceTorqueCollisionBehavior, std::ref(robot), _1, _2))
+ [&robot](auto&& req, auto&& res) {
+ return franka_control::setForceTorqueCollisionBehavior(robot, req, res);
+ })
.advertiseService<franka_control::SetFullCollisionBehavior>(
node_handle, "set_full_collision_behavior",
- std::bind(franka_control::setFullCollisionBehavior, std::ref(robot), _1, _2))
+ [&robot](auto&& req, auto&& res) {
+ return franka_control::setFullCollisionBehavior(robot, req, res);
+ })
.advertiseService<franka_control::SetLoad>(
- node_handle, "set_load", std::bind(franka_control::setLoad, std::ref(robot), _1, _2));
+ node_handle, "set_load",
+ [&robot](auto&& req, auto&& res) { return franka_control::setLoad(robot, req, res); });
actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction> recovery_action_server(
node_handle, "error_recovery",
@@ -136,8 +136,31 @@ int main(int argc, char** argv) {
false);
franka::Model model = robot.loadModel();
- franka_hw::FrankaHW franka_control(joint_names, arm_id, internal_controller, rate_limiting,
- cutoff_freq, public_node_handle, model);
+ auto get_rate_limiting = [&]() {
+ node_handle.getParamCached("rate_limiting", rate_limiting);
+ return rate_limiting;
+ };
+ auto get_internal_controller = [&]() {
+ node_handle.getParamCached("internal_controller", internal_controller);
+
+ franka::ControllerMode controller_mode;
+ if (internal_controller == "joint_impedance") {
+ controller_mode = franka::ControllerMode::kJointImpedance;
+ } else if (internal_controller == "cartesian_impedance") {
+ controller_mode = franka::ControllerMode::kCartesianImpedance;
+ } else {
+ ROS_WARN("Invalid internal_controller parameter provided, falling back to joint impedance");
+ controller_mode = franka::ControllerMode::kJointImpedance;
+ }
+
+ return controller_mode;
+ };
+ auto get_cutoff_frequency = [&]() {
+ node_handle.getParamCached("cutoff_frequency", cutoff_frequency);
+ return cutoff_frequency;
+ };
+ franka_hw::FrankaHW franka_control(joint_names, arm_id, urdf_model, model, get_rate_limiting,
+ get_cutoff_frequency, get_internal_controller);
// Initialize robot state before loading any controller
franka_control.update(robot.readOnce());
diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp
index 457e2adce3d0a5afa2e676e32ea78b3a4e1efceb..2e1e510f4bd236a9b63557f9d036d849d350fe61 100644
--- a/franka_control/src/franka_state_controller.cpp
+++ b/franka_control/src/franka_state_controller.cpp
@@ -3,6 +3,7 @@
#include <franka_control/franka_state_controller.h>
#include <cmath>
+#include <memory>
#include <mutex>
#include <string>
@@ -134,16 +135,6 @@ franka_msgs::Errors errorsToMessage(const franka::Errors& error) {
namespace franka_control {
-FrankaStateController::FrankaStateController()
- : franka_state_interface_(nullptr),
- franka_state_handle_(nullptr),
- publisher_transforms_(),
- publisher_franka_states_(),
- publisher_joint_states_(),
- publisher_joint_states_desired_(),
- publisher_external_wrench_(),
- trigger_publish_(30.0) {}
-
bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& controller_node_handle) {
@@ -157,12 +148,11 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
return false;
}
double publish_rate(30.0);
- if (controller_node_handle.getParam("publish_rate", publish_rate)) {
- trigger_publish_ = franka_hw::TriggerRate(publish_rate);
- } else {
+ if (!controller_node_handle.getParam("publish_rate", publish_rate)) {
ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default "
<< publish_rate << " [Hz].");
}
+ trigger_publish_ = franka_hw::TriggerRate(publish_rate);
if (!controller_node_handle.getParam("joint_names", joint_names_) ||
joint_names_.size() != robot_state_.q.size()) {
@@ -173,8 +163,8 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
}
try {
- franka_state_handle_.reset(
- new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id_ + "_robot")));
+ franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+ franka_state_interface_->getHandle(arm_id_ + "_robot"));
} catch (const hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM("FrankaStateController: Exception getting franka state handle: " << ex.what());
return false;
diff --git a/franka_control/src/services.cpp b/franka_control/src/services.cpp
index 0445ea2426e594fa2c03a5f75945319a86155cad..c7a47c91b6f8a98d5e2a76cf86b07e5a4c9cafcf 100644
--- a/franka_control/src/services.cpp
+++ b/franka_control/src/services.cpp
@@ -40,7 +40,7 @@ void setKFrame(franka::Robot& robot,
void setForceTorqueCollisionBehavior(franka::Robot& robot,
const SetForceTorqueCollisionBehavior::Request& req,
SetForceTorqueCollisionBehavior::Response& /* res */
- ) {
+) {
std::array<double, 7> lower_torque_thresholds_nominal;
std::copy(req.lower_torque_thresholds_nominal.cbegin(),
req.lower_torque_thresholds_nominal.cend(), lower_torque_thresholds_nominal.begin());
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
index 84afb873ed47526ea933c7f3da08c238e152e3cd..1d9120adc3f0e5eb0cca2bff66ce58a188cd0a5b 100644
--- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
@@ -3,6 +3,7 @@
#include <franka_example_controllers/cartesian_impedance_example_controller.h>
#include <cmath>
+#include <memory>
#include <controller_interface/controller_base.h>
#include <franka/robot_state.h>
@@ -35,16 +36,15 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
return false;
}
- franka_hw::FrankaModelInterface* model_interface =
- robot_hw->get<franka_hw::FrankaModelInterface>();
+ auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
if (model_interface == nullptr) {
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Error getting model interface from hardware");
return false;
}
try {
- model_handle_.reset(
- new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
+ model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+ model_interface->getHandle(arm_id + "_model"));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Exception getting model handle from interface: "
@@ -52,16 +52,15 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
return false;
}
- franka_hw::FrankaStateInterface* state_interface =
- robot_hw->get<franka_hw::FrankaStateInterface>();
+ auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
if (state_interface == nullptr) {
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Error getting state interface from hardware");
return false;
}
try {
- state_handle_.reset(
- new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot")));
+ state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+ state_interface->getHandle(arm_id + "_robot"));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Exception getting state handle from interface: "
@@ -69,8 +68,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
return false;
}
- hardware_interface::EffortJointInterface* effort_joint_interface =
- robot_hw->get<hardware_interface::EffortJointInterface>();
+ auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
if (effort_joint_interface == nullptr) {
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Error getting effort joint interface from hardware");
@@ -89,9 +87,10 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
dynamic_reconfigure_compliance_param_node_ =
ros::NodeHandle("dynamic_reconfigure_compliance_param_node");
- dynamic_server_compliance_param_.reset(
- new dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>(
- dynamic_reconfigure_compliance_param_node_));
+ dynamic_server_compliance_param_ = std::make_unique<
+ dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>(
+
+ dynamic_reconfigure_compliance_param_node_);
dynamic_server_compliance_param_->setCallback(
boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
@@ -114,9 +113,9 @@ void CartesianImpedanceExampleController::starting(const ros::Time& /*time*/) {
std::array<double, 42> jacobian_array =
model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
// convert to eigen
- Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > dq_initial(initial_state.dq.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > q_initial(initial_state.q.data());
+ Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> dq_initial(initial_state.dq.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> q_initial(initial_state.q.data());
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
// set equilibrium point to current state
@@ -138,11 +137,11 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
// convert to Eigen
- Eigen::Map<Eigen::Matrix<double, 7, 1> > coriolis(coriolis_array.data());
- Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > q(robot_state.q.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > dq(robot_state.dq.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_J_d( // NOLINT (readability-identifier-naming)
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
+ Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming)
robot_state.tau_J_d.data());
Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
Eigen::Vector3d position(transform.translation());
diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
index ae1b787aa97fe89d8ce80a028a02a698600b95d4..af69500f3b79d783fd08f8361c63f42da69c9e2e 100644
--- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
@@ -3,6 +3,7 @@
#include <franka_example_controllers/cartesian_pose_example_controller.h>
#include <cmath>
+#include <memory>
#include <stdexcept>
#include <string>
@@ -31,8 +32,8 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
}
try {
- cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
- cartesian_pose_interface_->getHandle(arm_id + "_robot")));
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot"));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
"CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
index 59e0f6fedbf02a3d85d5884e74c36780694c8d95..67c34a255b04180b5cb4cfb5d48c003b4637705e 100644
--- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
@@ -4,6 +4,7 @@
#include <array>
#include <cmath>
+#include <memory>
#include <string>
#include <controller_interface/controller_base.h>
@@ -31,8 +32,8 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot
return false;
}
try {
- velocity_cartesian_handle_.reset(new franka_hw::FrankaCartesianVelocityHandle(
- velocity_cartesian_interface_->getHandle(arm_id + "_robot")));
+ velocity_cartesian_handle_ = std::make_unique<franka_hw::FrankaCartesianVelocityHandle>(
+ velocity_cartesian_interface_->getHandle(arm_id + "_robot"));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
"CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what());
diff --git a/franka_example_controllers/src/elbow_example_controller.cpp b/franka_example_controllers/src/elbow_example_controller.cpp
index f12137ebd6f75927d16de71ad6a13bb2a47d2290..c988cb502164eb85ef0a803f9347b58a28ab625a 100644
--- a/franka_example_controllers/src/elbow_example_controller.cpp
+++ b/franka_example_controllers/src/elbow_example_controller.cpp
@@ -3,6 +3,7 @@
#include <franka_example_controllers/elbow_example_controller.h>
#include <cmath>
+#include <memory>
#include <stdexcept>
#include <string>
@@ -29,8 +30,8 @@ bool ElbowExampleController::init(hardware_interface::RobotHW* robot_hardware,
}
try {
- cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
- cartesian_pose_interface_->getHandle(arm_id + "_robot")));
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot"));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM("ElbowExampleController: Exception getting Cartesian handle: " << e.what());
return false;
diff --git a/franka_example_controllers/src/force_example_controller.cpp b/franka_example_controllers/src/force_example_controller.cpp
index 4e49c2f6ef78ccc01e65e8c1d8870657c5bbaed0..36cce5da7fb74c1f018240ba22615fd88c75e8eb 100644
--- a/franka_example_controllers/src/force_example_controller.cpp
+++ b/franka_example_controllers/src/force_example_controller.cpp
@@ -3,6 +3,7 @@
#include <franka_example_controllers/force_example_controller.h>
#include <cmath>
+#include <memory>
#include <controller_interface/controller_base.h>
#include <pluginlib/class_list_macros.h>
@@ -30,38 +31,35 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
return false;
}
- franka_hw::FrankaModelInterface* model_interface =
- robot_hw->get<franka_hw::FrankaModelInterface>();
+ auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
if (model_interface == nullptr) {
ROS_ERROR_STREAM("ForceExampleController: Error getting model interface from hardware");
return false;
}
try {
- model_handle_.reset(
- new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
+ model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+ model_interface->getHandle(arm_id + "_model"));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"ForceExampleController: Exception getting model handle from interface: " << ex.what());
return false;
}
- franka_hw::FrankaStateInterface* state_interface =
- robot_hw->get<franka_hw::FrankaStateInterface>();
+ auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
if (state_interface == nullptr) {
ROS_ERROR_STREAM("ForceExampleController: Error getting state interface from hardware");
return false;
}
try {
- state_handle_.reset(
- new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot")));
+ state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+ state_interface->getHandle(arm_id + "_robot"));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"ForceExampleController: Exception getting state handle from interface: " << ex.what());
return false;
}
- hardware_interface::EffortJointInterface* effort_joint_interface =
- robot_hw->get<hardware_interface::EffortJointInterface>();
+ auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
if (effort_joint_interface == nullptr) {
ROS_ERROR_STREAM("ForceExampleController: Error getting effort joint interface from hardware");
return false;
@@ -77,9 +75,10 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
dynamic_reconfigure_desired_mass_param_node_ =
ros::NodeHandle("dynamic_reconfigure_desired_mass_param_node");
- dynamic_server_desired_mass_param_.reset(
- new dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>(
- dynamic_reconfigure_desired_mass_param_node_));
+ dynamic_server_desired_mass_param_ = std::make_unique<
+ dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>(
+
+ dynamic_reconfigure_desired_mass_param_node_);
dynamic_server_desired_mass_param_->setCallback(
boost::bind(&ForceExampleController::desiredMassParamCallback, this, _1, _2));
@@ -89,8 +88,8 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
void ForceExampleController::starting(const ros::Time& /*time*/) {
franka::RobotState robot_state = state_handle_->getRobotState();
std::array<double, 7> gravity_array = model_handle_->getGravity();
- Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
// Bias correction for the current external torque
tau_ext_initial_ = tau_measured - gravity;
tau_error_.setZero();
@@ -101,11 +100,11 @@ void ForceExampleController::update(const ros::Time& /*time*/, const ros::Durati
std::array<double, 42> jacobian_array =
model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
std::array<double, 7> gravity_array = model_handle_->getGravity();
- Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_J_d( // NOLINT (readability-identifier-naming)
+ Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d( // NOLINT (readability-identifier-naming)
robot_state.tau_J_d.data());
- Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
desired_force_torque.setZero();
diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp
index bfb075e2873fc056dc60b65320cf43dcdfb7ac85..607e87da9d919f03cef4925f46b92c8b9ab5e1da 100644
--- a/franka_example_controllers/src/joint_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp
@@ -3,6 +3,7 @@
#include <franka_example_controllers/joint_impedance_example_controller.h>
#include <cmath>
+#include <memory>
#include <controller_interface/controller_base.h>
#include <pluginlib/class_list_macros.h>
@@ -72,16 +73,15 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
<< coriolis_factor_);
}
- franka_hw::FrankaModelInterface* model_interface =
- robot_hw->get<franka_hw::FrankaModelInterface>();
+ auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
if (model_interface == nullptr) {
ROS_ERROR_STREAM(
"JointImpedanceExampleController: Error getting model interface from hardware");
return false;
}
try {
- model_handle_.reset(
- new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
+ model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+ model_interface->getHandle(arm_id + "_model"));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"JointImpedanceExampleController: Exception getting model handle from interface: "
@@ -89,16 +89,15 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
return false;
}
- franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface =
- robot_hw->get<franka_hw::FrankaPoseCartesianInterface>();
+ auto* cartesian_pose_interface = robot_hw->get<franka_hw::FrankaPoseCartesianInterface>();
if (cartesian_pose_interface == nullptr) {
ROS_ERROR_STREAM(
"JointImpedanceExampleController: Error getting cartesian pose interface from hardware");
return false;
}
try {
- cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
- cartesian_pose_interface->getHandle(arm_id + "_robot")));
+ cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+ cartesian_pose_interface->getHandle(arm_id + "_robot"));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"JointImpedanceExampleController: Exception getting cartesian pose handle from interface: "
@@ -106,8 +105,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
return false;
}
- hardware_interface::EffortJointInterface* effort_joint_interface =
- robot_hw->get<hardware_interface::EffortJointInterface>();
+ auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
if (effort_joint_interface == nullptr) {
ROS_ERROR_STREAM(
"JointImpedanceExampleController: Error getting effort joint interface from hardware");
diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp
index 218855945ae47a92a2bab03ecd4b24729060da14..f9ccea429ad6b0c9ebd47c8f4d704c0e6ec47ff2 100644
--- a/franka_example_controllers/src/model_example_controller.cpp
+++ b/franka_example_controllers/src/model_example_controller.cpp
@@ -7,6 +7,7 @@
#include <array>
#include <cstring>
#include <iterator>
+#include <memory>
#include <controller_interface/controller_base.h>
#include <hardware_interface/hardware_interface.h>
@@ -48,8 +49,8 @@ bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
}
try {
- franka_state_handle_.reset(
- new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id + "_robot")));
+ franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+ franka_state_interface_->getHandle(arm_id + "_robot"));
} catch (const hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"ModelExampleController: Exception getting franka state handle: " << ex.what());
@@ -57,8 +58,8 @@ bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
}
try {
- model_handle_.reset(
- new franka_hw::FrankaModelHandle(model_interface_->getHandle(arm_id + "_model")));
+ model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+ model_interface_->getHandle(arm_id + "_model"));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"ModelExampleController: Exception getting model handle from interface: " << ex.what());
diff --git a/franka_gripper/src/franka_gripper_node.cpp b/franka_gripper/src/franka_gripper_node.cpp
index 8b5b5eaeeab8a2e39c67d0b8e0d2623f5267f66c..64a8d254854320e7f348159a5d53d5f3fc1688c6 100644
--- a/franka_gripper/src/franka_gripper_node.cpp
+++ b/franka_gripper/src/franka_gripper_node.cpp
@@ -84,46 +84,56 @@ int main(int argc, char** argv) {
franka::Gripper gripper(robot_ip);
- std::function<bool(const HomingGoalConstPtr&)> homing_handler =
- std::bind(homing, std::cref(gripper), std::placeholders::_1);
- std::function<bool(const StopGoalConstPtr&)> stop_handler =
- std::bind(stop, std::cref(gripper), std::placeholders::_1);
- std::function<bool(const GraspGoalConstPtr&)> grasp_handler =
- std::bind(grasp, std::cref(gripper), std::placeholders::_1);
- std::function<bool(const MoveGoalConstPtr&)> move_handler =
- std::bind(move, std::cref(gripper), std::placeholders::_1);
-
- SimpleActionServer<HomingAction> homing_action_server_(
+ auto homing_handler = [&gripper](auto&& goal) { return homing(gripper, goal); };
+ auto stop_handler = [&gripper](auto&& goal) { return stop(gripper, goal); };
+ auto grasp_handler = [&gripper](auto&& goal) { return grasp(gripper, goal); };
+ auto move_handler = [&gripper](auto&& goal) { return move(gripper, goal); };
+
+ SimpleActionServer<HomingAction> homing_action_server(
node_handle, "homing",
- std::bind(handleErrors<HomingAction, HomingGoalConstPtr, HomingResult>,
- &homing_action_server_, homing_handler, std::placeholders::_1),
+ [=, &homing_action_server](auto&& goal) {
+ return handleErrors<franka_gripper::HomingAction, franka_gripper::HomingGoalConstPtr,
+ franka_gripper::HomingResult>(&homing_action_server, homing_handler,
+ goal);
+ },
false);
- SimpleActionServer<StopAction> stop_action_server_(
- node_handle, "stop", std::bind(handleErrors<StopAction, StopGoalConstPtr, StopResult>,
- &stop_action_server_, stop_handler, std::placeholders::_1),
+ SimpleActionServer<StopAction> stop_action_server(
+ node_handle, "stop",
+ [=, &stop_action_server](auto&& goal) {
+ return handleErrors<franka_gripper::StopAction, franka_gripper::StopGoalConstPtr,
+ franka_gripper::StopResult>(&stop_action_server, stop_handler, goal);
+ },
false);
- SimpleActionServer<MoveAction> move_action_server_(
- node_handle, "move", std::bind(handleErrors<MoveAction, MoveGoalConstPtr, MoveResult>,
- &move_action_server_, move_handler, std::placeholders::_1),
+ SimpleActionServer<MoveAction> move_action_server(
+ node_handle, "move",
+ [=, &move_action_server](auto&& goal) {
+ return handleErrors<franka_gripper::MoveAction, franka_gripper::MoveGoalConstPtr,
+ franka_gripper::MoveResult>(&move_action_server, move_handler, goal);
+ },
false);
- SimpleActionServer<GraspAction> grasp_action_server_(
- node_handle, "grasp", std::bind(handleErrors<GraspAction, GraspGoalConstPtr, GraspResult>,
- &grasp_action_server_, grasp_handler, std::placeholders::_1),
+ SimpleActionServer<GraspAction> grasp_action_server(
+ node_handle, "grasp",
+ [=, &grasp_action_server](auto&& goal) {
+ return handleErrors<franka_gripper::GraspAction, franka_gripper::GraspGoalConstPtr,
+ franka_gripper::GraspResult>(&grasp_action_server, grasp_handler, goal);
+ },
false);
SimpleActionServer<GripperCommandAction> gripper_command_action_server(
node_handle, "gripper_action",
- std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_grasp_epsilon,
- default_speed, &gripper_command_action_server, std::placeholders::_1),
+ [=, &gripper, &gripper_command_action_server](auto&& goal) {
+ return gripperCommandExecuteCallback(gripper, default_grasp_epsilon, default_speed,
+ &gripper_command_action_server, goal);
+ },
false);
- homing_action_server_.start();
- stop_action_server_.start();
- move_action_server_.start();
- grasp_action_server_.start();
+ homing_action_server.start();
+ stop_action_server.start();
+ move_action_server.start();
+ grasp_action_server.start();
gripper_command_action_server.start();
double publish_rate(30.0);
diff --git a/franka_hw/CMakeLists.txt b/franka_hw/CMakeLists.txt
index 5d1469932fb1024322c2d3b2ab777d754e652d96..821f48b5e0a652b4c1487ba52b3142de5cb74a6f 100644
--- a/franka_hw/CMakeLists.txt
+++ b/franka_hw/CMakeLists.txt
@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
hardware_interface
joint_limits_interface
roscpp
+ urdf
)
find_package(Franka 0.5.0 REQUIRED)
@@ -16,7 +17,7 @@ find_package(Franka 0.5.0 REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES franka_hw
- CATKIN_DEPENDS controller_interface hardware_interface joint_limits_interface roscpp
+ CATKIN_DEPENDS controller_interface hardware_interface joint_limits_interface roscpp urdf
DEPENDS Franka
)
diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h
index e563879d3b364b6f073cc60e3021d474b7d6bb46..3950d81fbca89d91b2ef96dff788c26021b5acef 100644
--- a/franka_hw/include/franka_hw/franka_hw.h
+++ b/franka_hw/include/franka_hw/franka_hw.h
@@ -11,17 +11,16 @@
#include <franka/model.h>
#include <franka/robot.h>
#include <franka/robot_state.h>
+#include <franka_hw/control_mode.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <joint_limits_interface/joint_limits_interface.h>
-#include <ros/node_handle.h>
#include <ros/time.h>
-
-#include <franka_hw/control_mode.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/franka_state_interface.h>
+#include <urdf/model.h>
namespace franka_hw {
@@ -34,42 +33,46 @@ class FrankaHW : public hardware_interface::RobotHW {
*
* @param[in] joint_names An array of joint names being controlled.
* @param[in] arm_id Unique identifier for the robot being controlled.
- * @param[in] limit_rate An array of booleans indicating if the rate limiter should be used or not
- * for each interface
- * @param[in] internal_controller Internal controller to be used for control loops using only
- * motion generation
- * @param[in] cutoff_freq An array of doubles indicating the cutoff frequency for the lowpass
- * applied in each interface
- * @param[in] node_handle A node handle to get parameters from.
+ * @param[in] urdf_model A URDF model to initialize joint limits from.
+ * @param[in] get_limit_rate Getter that should return true if the rate limiter
+ * should be used, false otherwise. Defaults to true.
+ * @param[in] get_cutoff_frequency Getter for cutoff frequency for the low-pass filter.
+ * Defaults to franka::kDefaultCutoffFrequency.
+ * @param[in] get_internal_controller Getter for an internal controller to
+ * be used for control loops using only motion generation. Defaults to joint impedance.
*/
FrankaHW(const std::array<std::string, 7>& joint_names,
const std::string& arm_id,
- franka::ControllerMode internal_controller,
- const std::array<bool, 5>& limit_rate,
- const std::array<double, 5>& cutoff_freq,
- const ros::NodeHandle& node_handle);
+ const urdf::Model& urdf_model,
+ std::function<bool()> get_limit_rate = []() { return true; },
+ std::function<double()> get_cutoff_frequency =
+ []() { return franka::kDefaultCutoffFrequency; },
+ std::function<franka::ControllerMode()> get_internal_controller =
+ []() { return franka::ControllerMode::kJointImpedance; });
/**
* Creates an instance of FrankaHW that provides a model interface.
*
* @param[in] joint_names An array of joint names being controlled.
* @param[in] arm_id Unique identifier for the robot being controlled.
- * @param[in] node_handle A node handle to get parameters from.
- * @param[in] limit_rate An array of booleans indicating if the rate limiter should be used or not
- * for each interface
- * @param[in] internal_controller Internal controller to be used for control loops using only
- * motion generation
- * @param[in] cutoff_freq An array of doubles indicating the cutoff frequency for the lowpass
- * applied in each interface
+ * @param[in] urdf_model A URDF model to initialize joint limits from.
* @param[in] model Robot model.
+ * @param[in] get_limit_rate Getter that should return true if the rate limiter
+ * should be used, false otherwise. Defaults to true.
+ * @param[in] get_cutoff_frequency Getter for cutoff frequency for the low-pass filter.
+ * Defaults to franka::kDefaultCutoffFrequency.
+ * @param[in] get_internal_controller Getter for an internal controller to
+ * be used for control loops using only motion generation. Defaults to joint impedance.
*/
FrankaHW(const std::array<std::string, 7>& joint_names,
const std::string& arm_id,
- franka::ControllerMode internal_controller,
- const std::array<bool, 5>& limit_rate,
- const std::array<double, 5>& cutoff_freq,
- const ros::NodeHandle& node_handle,
- franka::Model& model);
+ const urdf::Model& urdf_model,
+ franka::Model& model,
+ std::function<bool()> get_limit_rate = []() { return true; },
+ std::function<double()> get_cutoff_frequency =
+ []() { return franka::kDefaultCutoffFrequency; },
+ std::function<franka::ControllerMode()> get_internal_controller =
+ []() { return franka::ControllerMode::kJointImpedance; });
~FrankaHW() override = default;
@@ -90,7 +93,7 @@ class FrankaHW : public hardware_interface::RobotHW {
* @throw franka::RealtimeException if realtime priority cannot be set for the current thread.
*/
void control(franka::Robot& robot,
- std::function<bool(const ros::Time&, const ros::Duration&)> ros_callback);
+ const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback);
/**
* Updates the controller interfaces from the given robot state.
@@ -126,13 +129,13 @@ class FrankaHW : public hardware_interface::RobotHW {
const std::list<hardware_interface::ControllerInfo>&) override;
/**
- * Prepares switching between controllers (not real-time capable).
- *
- * @param[in] start_list Controllers requested to be started.
- * @param[in] stop_list Controllers requested to be stopped.
- *
- * @return True if the preparation has been successful, false otherwise.
- */
+ * Prepares switching between controllers (not real-time capable).
+ *
+ * @param[in] start_list Controllers requested to be started.
+ * @param[in] stop_list Controllers requested to be stopped.
+ *
+ * @return True if the preparation has been successful, false otherwise.
+ */
bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) override;
@@ -178,7 +181,7 @@ class FrankaHW : public hardware_interface::RobotHW {
const franka::RobotState& robot_state,
franka::Duration time_step) {
robot_state_ = robot_state;
- if (!controller_active_ || ros_callback && !ros_callback(robot_state, time_step)) {
+ if (!controller_active_ || (ros_callback && !ros_callback(robot_state, time_step))) {
return franka::MotionFinished(command);
}
return command;
@@ -201,9 +204,9 @@ class FrankaHW : public hardware_interface::RobotHW {
std::array<std::string, 7> joint_names_;
const std::string arm_id_;
- franka::ControllerMode internal_controller_;
- std::array<bool, 5> limit_rate_;
- std::array<double, 5> cutoff_freq_;
+ std::function<franka::ControllerMode()> get_internal_controller_;
+ std::function<bool()> get_limit_rate_;
+ std::function<double()> get_cutoff_frequency_;
franka::JointPositions position_joint_command_;
franka::JointVelocities velocity_joint_command_;
diff --git a/franka_hw/package.xml b/franka_hw/package.xml
index 78ebaf00519618c18aa597434d5d78302d4276df..0d0b8a1eed3aca6206fb07d706c0023a745e4406 100644
--- a/franka_hw/package.xml
+++ b/franka_hw/package.xml
@@ -13,11 +13,12 @@
<buildtool_depend>catkin</buildtool_depend>
- <depend>libfranka</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>joint_limits_interface</depend>
+ <depend>libfranka</depend>
<depend>roscpp</depend>
+ <depend>urdf</depend>
<test_depend>franka_description</test_depend>
<test_depend>gtest</test_depend>
diff --git a/franka_hw/src/control_mode.cpp b/franka_hw/src/control_mode.cpp
index c8c724fecbb3a8eb4764f2e770973e20f31b6cf9..cfd89c22e0743c98d0b362b3fc9cc2220a7f98b1 100644
--- a/franka_hw/src/control_mode.cpp
+++ b/franka_hw/src/control_mode.cpp
@@ -15,19 +15,19 @@ std::ostream& operator<<(std::ostream& ostream, ControlMode mode) {
} else {
std::vector<std::string> names;
if ((mode & ControlMode::JointTorque) != ControlMode::None) {
- names.emplace_back("JointTorque");
+ names.emplace_back("joint_torque");
}
if ((mode & ControlMode::JointPosition) != ControlMode::None) {
- names.emplace_back("JointPosition");
+ names.emplace_back("joint_position");
}
if ((mode & ControlMode::JointVelocity) != ControlMode::None) {
- names.emplace_back("JointVelocity");
+ names.emplace_back("joint_velocity");
}
if ((mode & ControlMode::CartesianVelocity) != ControlMode::None) {
- names.emplace_back("CartesianVelocity");
+ names.emplace_back("cartesian_velocity");
}
if ((mode & ControlMode::CartesianPose) != ControlMode::None) {
- names.emplace_back("CartesianPose");
+ names.emplace_back("cartesian_pose");
}
std::copy(names.cbegin(), names.cend() - 1, std::ostream_iterator<std::string>(ostream, ", "));
ostream << names.back();
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
index 5a454a0f989f577fd82cbcd5650ac14b8d1aeb3b..50f20d02d8fd24ac68805612a5443f611061acf5 100644
--- a/franka_hw/src/franka_hw.cpp
+++ b/franka_hw/src/franka_hw.cpp
@@ -3,26 +3,39 @@
#include <franka_hw/franka_hw.h>
#include <cstdint>
+#include <ostream>
+#include <utility>
#include <franka/rate_limiting.h>
#include <joint_limits_interface/joint_limits_urdf.h>
-#include <urdf/model.h>
#include "resource_helpers.h"
namespace franka_hw {
+namespace {
+std::ostream& operator<<(std::ostream& ostream, franka::ControllerMode mode) {
+ if (mode == franka::ControllerMode::kJointImpedance) {
+ ostream << "joint_impedance";
+ } else if (mode == franka::ControllerMode::kCartesianImpedance) {
+ ostream << "cartesian_impedance";
+ } else {
+ ostream << "<unknown>";
+ }
+}
+} // anonymous namespace
+
FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
const std::string& arm_id,
- franka::ControllerMode internal_controller,
- const std::array<bool, 5>& limit_rate,
- const std::array<double, 5>& cutoff_freq,
- const ros::NodeHandle& node_handle)
+ const urdf::Model& urdf_model,
+ std::function<bool()> get_limit_rate,
+ std::function<double()> get_cutoff_frequency,
+ std::function<franka::ControllerMode()> get_internal_controller)
: joint_names_(joint_names),
arm_id_(arm_id),
- internal_controller_(internal_controller),
- limit_rate_(limit_rate),
- cutoff_freq_(cutoff_freq),
+ get_internal_controller_(std::move(get_internal_controller)),
+ get_limit_rate_(std::move(get_limit_rate)),
+ get_cutoff_frequency_(std::move(get_cutoff_frequency)),
position_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
velocity_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
effort_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
@@ -51,56 +64,50 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
effort_joint_interface_.registerHandle(effort_joint_handle);
}
- if (node_handle.hasParam("robot_description")) {
- urdf::Model urdf_model;
- if (!urdf_model.initParamWithNodeHandle("robot_description", node_handle)) {
- ROS_ERROR("FrankaHW: Could not initialize urdf model from robot_description");
- } else {
- joint_limits_interface::SoftJointLimits soft_limits;
- joint_limits_interface::JointLimits joint_limits;
-
- for (size_t i = 0; i < joint_names_.size(); i++) {
- const std::string& joint_name = joint_names_[i];
- auto urdf_joint = urdf_model.getJoint(joint_name);
- if (!urdf_joint) {
- ROS_ERROR_STREAM("FrankaHW: Could not get joint " << joint_name << " from urdf");
- }
- if (!urdf_joint->safety) {
- ROS_ERROR_STREAM("FrankaHW: Joint " << joint_name << " has no safety");
- }
- if (!urdf_joint->limits) {
- ROS_ERROR_STREAM("FrankaHW: Joint " << joint_name << " has no limits");
- }
+ joint_limits_interface::SoftJointLimits soft_limits;
+ joint_limits_interface::JointLimits joint_limits;
- if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
- if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
- joint_limits.max_acceleration = franka::kMaxJointAcceleration[i];
- joint_limits.has_acceleration_limits = true;
- joint_limits.max_jerk = franka::kMaxJointJerk[i];
- joint_limits.has_jerk_limits = true;
- joint_limits_interface::PositionJointSoftLimitsHandle position_limit_handle(
- position_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
- position_joint_limit_interface_.registerHandle(position_limit_handle);
-
- joint_limits_interface::VelocityJointSoftLimitsHandle velocity_limit_handle(
- velocity_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
- velocity_joint_limit_interface_.registerHandle(velocity_limit_handle);
-
- joint_limits_interface::EffortJointSoftLimitsHandle effort_limit_handle(
- effort_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
- effort_joint_limit_interface_.registerHandle(effort_limit_handle);
- } else {
- ROS_ERROR_STREAM("FrankaHW: Could not parse joint limit for joint "
- << joint_name << " for joint limit interfaces");
- }
- } else {
- ROS_ERROR_STREAM("FrankaHW: Could not parse soft joint limit for joint "
- << joint_name << " for joint limit interfaces");
- }
+ for (size_t i = 0; i < joint_names_.size(); i++) {
+ const std::string& joint_name = joint_names_[i];
+ auto urdf_joint = urdf_model.getJoint(joint_name);
+ if (!urdf_joint) {
+ ROS_ERROR_STREAM("FrankaHW: Could not get joint " << joint_name << " from urdf");
+ continue;
+ }
+ if (!urdf_joint->safety) {
+ ROS_ERROR_STREAM("FrankaHW: Joint " << joint_name << " has no safety");
+ continue;
+ }
+ if (!urdf_joint->limits) {
+ ROS_ERROR_STREAM("FrankaHW: Joint " << joint_name << " has no limits");
+ continue;
+ }
+
+ if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
+ if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
+ joint_limits.max_acceleration = franka::kMaxJointAcceleration[i];
+ joint_limits.has_acceleration_limits = true;
+ joint_limits.max_jerk = franka::kMaxJointJerk[i];
+ joint_limits.has_jerk_limits = true;
+ joint_limits_interface::PositionJointSoftLimitsHandle position_limit_handle(
+ position_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
+ position_joint_limit_interface_.registerHandle(position_limit_handle);
+
+ joint_limits_interface::VelocityJointSoftLimitsHandle velocity_limit_handle(
+ velocity_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
+ velocity_joint_limit_interface_.registerHandle(velocity_limit_handle);
+
+ joint_limits_interface::EffortJointSoftLimitsHandle effort_limit_handle(
+ effort_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
+ effort_joint_limit_interface_.registerHandle(effort_limit_handle);
+ } else {
+ ROS_ERROR_STREAM("FrankaHW: Could not parse joint limit for joint "
+ << joint_name << " for joint limit interfaces");
}
+ } else {
+ ROS_ERROR_STREAM("FrankaHW: Could not parse soft joint limit for joint "
+ << joint_name << " for joint limit interfaces");
}
- } else {
- ROS_WARN("FrankaHW: No parameter robot_description found to set joint limits!");
}
FrankaStateHandle franka_state_handle(arm_id_ + "_robot", robot_state_);
@@ -123,12 +130,17 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
const std::string& arm_id,
- franka::ControllerMode internal_controller,
- const std::array<bool, 5>& limit_rate,
- const std::array<double, 5>& cutoff_freq,
- const ros::NodeHandle& node_handle,
- franka::Model& model)
- : FrankaHW(joint_names, arm_id, internal_controller, limit_rate, cutoff_freq, node_handle) {
+ const urdf::Model& urdf_model,
+ franka::Model& model,
+ std::function<bool()> get_limit_rate,
+ std::function<double()> get_cutoff_frequency,
+ std::function<franka::ControllerMode()> get_internal_controller)
+ : FrankaHW(joint_names,
+ arm_id,
+ urdf_model,
+ std::move(get_limit_rate),
+ std::move(get_cutoff_frequency),
+ std::move(get_internal_controller)) {
franka_hw::FrankaModelHandle model_handle(arm_id_ + "_model", model, robot_state_);
franka_model_interface_.registerHandle(model_handle);
@@ -144,8 +156,9 @@ bool FrankaHW::controllerActive() const noexcept {
return controller_active_;
}
-void FrankaHW::control(franka::Robot& robot,
- std::function<bool(const ros::Time&, const ros::Duration&)> ros_callback) {
+void FrankaHW::control(
+ franka::Robot& robot,
+ const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback) {
if (!controller_active_) {
return;
}
@@ -184,7 +197,7 @@ bool FrankaHW::checkForConflict(const std::list<hardware_interface::ControllerIn
uint8_t other_claims = 0;
if (map_it->second.size() == 2) {
for (auto& claimed_by : map_it->second) {
- if (claimed_by[2].compare("hardware_interface::EffortJointInterface") == 0) {
+ if (claimed_by[2] == "hardware_interface::EffortJointInterface") {
torque_claims++;
} else {
other_claims++;
@@ -262,8 +275,12 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
ControlMode stop_control_mode = getControlMode(arm_id_, stop_arm_claim_map);
ControlMode requested_control_mode = current_control_mode_;
- requested_control_mode |= start_control_mode;
requested_control_mode &= ~stop_control_mode;
+ requested_control_mode |= start_control_mode;
+
+ auto limit_rate = get_limit_rate_();
+ auto cutoff_frequency = get_cutoff_frequency_();
+ auto internal_controller = get_internal_controller_();
using std::placeholders::_1;
using std::placeholders::_2;
@@ -272,74 +289,74 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
case ControlMode::None:
break;
case ControlMode::JointTorque:
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
std::cref(effort_joint_command_), ros_callback, _1, _2),
- limit_rate_[4], cutoff_freq_[4]);
+ limit_rate, cutoff_frequency);
};
break;
case ControlMode::JointPosition:
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this,
std::cref(position_joint_command_), ros_callback, _1, _2),
- internal_controller_, limit_rate_[0], cutoff_freq_[0]);
+ internal_controller, limit_rate, cutoff_frequency);
};
break;
case ControlMode::JointVelocity:
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this,
std::cref(velocity_joint_command_), ros_callback, _1, _2),
- internal_controller_, limit_rate_[1], cutoff_freq_[1]);
+ internal_controller, limit_rate, cutoff_frequency);
};
break;
case ControlMode::CartesianPose:
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this,
std::cref(pose_cartesian_command_), ros_callback, _1, _2),
- internal_controller_, limit_rate_[2], cutoff_freq_[2]);
+ internal_controller, limit_rate, cutoff_frequency);
};
break;
case ControlMode::CartesianVelocity:
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this,
std::cref(velocity_cartesian_command_), ros_callback, _1, _2),
- internal_controller_, limit_rate_[3], cutoff_freq_[3]);
+ internal_controller, limit_rate, cutoff_frequency);
};
break;
case (ControlMode::JointTorque | ControlMode::JointPosition):
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
std::cref(effort_joint_command_), ros_callback, _1, _2),
std::bind(&FrankaHW::controlCallback<franka::JointPositions>, this,
std::cref(position_joint_command_), ros_callback, _1, _2),
- limit_rate_[4], cutoff_freq_[4]);
+ limit_rate, cutoff_frequency);
};
break;
case (ControlMode::JointTorque | ControlMode::JointVelocity):
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
std::cref(effort_joint_command_), ros_callback, _1, _2),
std::bind(&FrankaHW::controlCallback<franka::JointVelocities>, this,
std::cref(velocity_joint_command_), ros_callback, _1, _2),
- limit_rate_[4], cutoff_freq_[4]);
+ limit_rate, cutoff_frequency);
};
break;
case (ControlMode::JointTorque | ControlMode::CartesianPose):
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
std::cref(effort_joint_command_), ros_callback, _1, _2),
std::bind(&FrankaHW::controlCallback<franka::CartesianPose>, this,
std::cref(pose_cartesian_command_), ros_callback, _1, _2),
- limit_rate_[4], cutoff_freq_[4]);
+ limit_rate, cutoff_frequency);
};
break;
case (ControlMode::JointTorque | ControlMode::CartesianVelocity):
- run_function_ = [this](franka::Robot& robot, Callback ros_callback) {
+ run_function_ = [=](franka::Robot& robot, Callback ros_callback) {
robot.control(std::bind(&FrankaHW::controlCallback<franka::Torques>, this,
std::cref(effort_joint_command_), ros_callback, _1, _2),
std::bind(&FrankaHW::controlCallback<franka::CartesianVelocities>, this,
std::cref(velocity_cartesian_command_), ros_callback, _1, _2),
- limit_rate_[4], cutoff_freq_[4]);
+ limit_rate, cutoff_frequency);
};
break;
default:
@@ -348,7 +365,10 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
}
if (current_control_mode_ != requested_control_mode) {
- ROS_INFO_STREAM("FrankaHW: Prepared switching controllers to " << requested_control_mode);
+ ROS_INFO_STREAM("FrankaHW: Prepared switching controllers to "
+ << requested_control_mode << "with parameters "
+ << "limit_rate=" << limit_rate << ", cutoff_frequency=" << cutoff_frequency
+ << ", internal_controller=" << internal_controller);
current_control_mode_ = requested_control_mode;
controller_active_ = false;
diff --git a/franka_hw/test/franka_hw_controller_switching_test.cpp b/franka_hw/test/franka_hw_controller_switching_test.cpp
index 228d9aaf823fa9375f0afe0c04278af2b35b08fb..58639da26544821748e43a479ebf8e04d27e3c78 100644
--- a/franka_hw/test/franka_hw_controller_switching_test.cpp
+++ b/franka_hw/test/franka_hw_controller_switching_test.cpp
@@ -70,7 +70,7 @@ hardware_interface::ControllerInfo newInfo(
class ControllerConflict :
public ::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > {
public:
- ControllerConflict() : robot_(new FrankaHW(joint_names, arm_id, franka::ControllerMode::kJointImpedance, {}, {}, ros::NodeHandle())) {}
+ ControllerConflict() : robot_(new FrankaHW(joint_names, arm_id, urdf::Model())) {}
bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
return robot_->checkForConflict(info_list);
}
@@ -84,7 +84,7 @@ public:
class NoControllerConflict : public
::testing::TestWithParam<std::list<hardware_interface::ControllerInfo> > {
public:
- NoControllerConflict() : robot_(new FrankaHW(joint_names, arm_id, franka::ControllerMode::kJointImpedance, {}, {}, ros::NodeHandle())) {}
+ NoControllerConflict() : robot_(new FrankaHW(joint_names, arm_id, urdf::Model())) {}
bool callCheckForConflict(const std::list<hardware_interface::ControllerInfo> info_list) {
return robot_->checkForConflict(info_list);
}
diff --git a/franka_hw/test/franka_hw_interfaces_test.cpp b/franka_hw/test/franka_hw_interfaces_test.cpp
index a72d29d6144bf0577429bc4d8e3b8aa9a7cac0c5..7984e1e2f71dfb3f43a00c06b42ceaa05c928a5a 100644
--- a/franka_hw/test/franka_hw_interfaces_test.cpp
+++ b/franka_hw/test/franka_hw_interfaces_test.cpp
@@ -23,7 +23,7 @@ extern std::array<std::string, 7> joint_names;
namespace franka_hw {
TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) {
- std::unique_ptr<FrankaHW> robotptr(new FrankaHW(joint_names, arm_id, franka::ControllerMode::kJointImpedance, {}, {}, ros::NodeHandle()));
+ std::unique_ptr<FrankaHW> robotptr(new FrankaHW(joint_names, arm_id, urdf::Model()));
hardware_interface::JointStateInterface* js_interface =
robotptr->get<hardware_interface::JointStateInterface>();
hardware_interface::PositionJointInterface* pj_interface =
@@ -73,7 +73,10 @@ TEST(FrankaHWTests, InterfacesWorkForReadAndCommand) {
}
TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) {
- std::unique_ptr<FrankaHW> robot_ptr(new FrankaHW(joint_names, arm_id, franka::ControllerMode::kJointImpedance, {}, {}, ros::NodeHandle()));
+ urdf::Model urdf_model;
+ ASSERT_TRUE(urdf_model.initParamWithNodeHandle("robot_description"));
+
+ std::unique_ptr<FrankaHW> robot_ptr(new FrankaHW(joint_names, arm_id, urdf_model));
hardware_interface::PositionJointInterface* pj_interface =
robot_ptr->get<hardware_interface::PositionJointInterface>();
@@ -85,8 +88,6 @@ TEST(FrankaHWTests, JointLimitInterfacesEnforceLimitsOnCommands) {
ASSERT_NE(nullptr, vj_interface);
ASSERT_NE(nullptr, ej_interface);
- urdf::Model urdf_model;
- ASSERT_TRUE(urdf_model.initParamWithNodeHandle("robot_description", ros::NodeHandle()));
std::vector<joint_limits_interface::JointLimits> joint_limits(7);
std::vector<hardware_interface::JointHandle> position_handles(7);
std::vector<hardware_interface::JointHandle> velocity_handles(7);
diff --git a/panda_moveit_config/launch/panda_control_moveit_rviz.launch b/panda_moveit_config/launch/panda_control_moveit_rviz.launch
index 7c1d0bb247432f0530575181d5ec29f22795366e..748f260367162031004d78a3b14e2b57432860dd 100644
--- a/panda_moveit_config/launch/panda_control_moveit_rviz.launch
+++ b/panda_moveit_config/launch/panda_control_moveit_rviz.launch
@@ -2,20 +2,16 @@
<launch>
<arg name="robot_ip" />
<arg name="load_gripper" default="true" />
+ <arg name="launch_rviz" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
- <include if="$(arg load_gripper)" file="$(find franka_gripper)/launch/franka_gripper.launch" >
- <arg name="robot_ip" value="$(arg robot_ip)" />
- </include>
-
<include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
- <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
- </include>
+ <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg launch_rviz)" />
</launch>