diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
index 4b496cfe340a75245913d6064ba789fb93c107af..070328d6f5ed6aa8a37517bdf4544628f5d53e21 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
@@ -31,12 +31,16 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn
void update(const ros::Time&, const ros::Duration& period) override;
private:
- std::string arm_id_;
- ros::NodeHandle node_handle_;
+ // Saturation
+ Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+ const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+ const Eigen::Matrix<double, 7, 1>& tau_J_d, // NOLINT (readability-identifier-naming)
+ const Eigen::Matrix<double, 7, 1>& gravity);
+
std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
std::vector<hardware_interface::JointHandle> joint_handles_;
- std::vector<std::string> joint_names_;
+
double filter_params_{0.005};
double nullspace_stiffness_{20.0};
double nullspace_stiffness_target_{20.0};
@@ -51,12 +55,6 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn
Eigen::Vector3d position_d_target_;
Eigen::Quaterniond orientation_d_target_;
- // Saturation
- Eigen::Matrix<double, 7, 1> saturateTorqueRate(
- const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
- const Eigen::Matrix<double, 7, 1>& tau_J_d, // NOLINT (readability-identifier-naming)
- const Eigen::Matrix<double, 7, 1>& gravity);
-
// Dynamic reconfigure
std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
dynamic_server_compliance_param_;
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
index 6a747dfb933b91dafe21ca1b93d3d0f3bf8e79ed..e66503a5443fc7f073710ccf9e2ba39c51b93107 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
@@ -27,7 +27,6 @@ class CartesianPoseExampleController
void update(const ros::Time&, const ros::Duration& period) override;
private:
- std::string arm_id_;
franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
ros::Duration elapsed_time_;
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
index bbeba97e6e1be0e5e797b7c413bccb41f956de7a..15d4970b63fb46757908e4d96aaa06dfd6c588dd 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
@@ -26,7 +26,6 @@ class CartesianVelocityExampleController : public controller_interface::MultiInt
void stopping(const ros::Time&) override;
private:
- std::string arm_id_;
franka_hw::FrankaVelocityCartesianInterface* velocity_cartesian_interface_;
std::unique_ptr<franka_hw::FrankaCartesianVelocityHandle> velocity_cartesian_handle_;
ros::Duration elapsed_time_;
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
index 148370b553b62b647d8444dcfffec3c0ae51bae8..778cc9b8207ea128ff0a9aefc70906021bb55df8 100644
--- a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
@@ -30,8 +30,8 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf
void update(const ros::Time&, const ros::Duration& period) override;
private:
+ // Saturation
std::array<double, 7> saturateTorqueRate(
- const double delta_tau_max,
const std::array<double, 7>& tau_d_calculated,
const std::array<double, 7>& tau_J_d, // NOLINT (readability-identifier-naming)
const std::array<double, 7>& gravity);
@@ -40,6 +40,7 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf
std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
std::vector<hardware_interface::JointHandle> joint_handles_;
+ const double delta_tau_max_{1.0};
double radius_{0.1};
double acceleration_time_{2.0};
double vel_max_{0.05};
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
index 2527cb33502a3c4df6654a7501df9b4ace44e294..a7c877d175337306cd61f4fe3830b6107310b04c 100644
--- a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
@@ -25,7 +25,6 @@ class JointPositionExampleController : public controller_interface::MultiInterfa
void update(const ros::Time&, const ros::Duration& period) override;
private:
- std::vector<std::string> joint_names_;
hardware_interface::PositionJointInterface* position_joint_interface_;
std::vector<hardware_interface::JointHandle> position_joint_handles_;
ros::Duration elapsed_time_;
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
index d236007beb338a77507d0ae0dbd78267e439900b..1c36f4a971c7a92f58bd184e7a26bc0ad3a34a73 100644
--- a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
@@ -26,7 +26,6 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa
void stopping(const ros::Time&) override;
private:
- std::vector<std::string> joint_names_;
hardware_interface::VelocityJointInterface* velocity_joint_interface_;
std::vector<hardware_interface::JointHandle> velocity_joint_handles_;
ros::Duration elapsed_time_;
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
index fec8a77193e717c59c0ef6868d0f08b51816c1f2..5c6d8ebe00443983cf9bac330e662740680fb4e9 100644
--- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
@@ -17,17 +17,18 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
ros::NodeHandle& node_handle) {
std::vector<double> cartesian_stiffness_vector;
std::vector<double> cartesian_damping_vector;
- node_handle_ = node_handle;
+ std::string arm_id;
+ std::vector<std::string> joint_names;
- sub_equilibrium_pose_ = node_handle_.subscribe(
+ sub_equilibrium_pose_ = node_handle.subscribe(
"/equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
ros::TransportHints().reliable().tcpNoDelay());
- if (!node_handle_.getParam("arm_id", arm_id_)) {
+ if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
return false;
}
- if (!node_handle_.getParam("joint_names", joint_names_) || joint_names_.size() != 7) {
+ if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
ROS_ERROR(
"CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
"aborting "
@@ -44,7 +45,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
}
try {
model_handle_.reset(
- new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id_ + "_model")));
+ new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Exception getting model handle from interface: "
@@ -61,7 +62,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
}
try {
state_handle_.reset(
- new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id_ + "_robot")));
+ new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot")));
} catch (hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Exception getting state handle from interface: "
@@ -78,7 +79,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
}
for (size_t i = 0; i < 7; ++i) {
try {
- joint_handles_.push_back(effort_joint_interface->getHandle(joint_names_[i]));
+ joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
} catch (const hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what());
diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
index f3e96ed13ab87ace19f166846cc48b66e20f6629..6417b3e335d3297b5cae7f1ed0708ab685df34ea 100644
--- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
@@ -18,6 +18,7 @@ namespace franka_example_controllers {
bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& /* controller_node_handle */) {
+ std::string arm_id;
cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
if (cartesian_pose_interface_ == nullptr) {
ROS_ERROR(
@@ -26,14 +27,14 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
return false;
}
- if (!root_node_handle.getParam("arm_id", arm_id_)) {
+ if (!root_node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
return false;
}
try {
cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
- cartesian_pose_interface_->getHandle(arm_id_ + "_robot")));
+ cartesian_pose_interface_->getHandle(arm_id + "_robot")));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
"CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
@@ -47,7 +48,7 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
}
try {
- auto state_handle = state_interface->getHandle(arm_id_ + "_robot");
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
for (size_t i = 0; i < q_start.size(); i++) {
diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
index 4593601e306845186716fbf75bdd5aaed66236e3..afd91ff268905090faa155fc32dcabd00821a352 100644
--- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
@@ -18,7 +18,8 @@ namespace franka_example_controllers {
bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& /* controller_node_handle */) {
- if (!root_node_handle.getParam("arm_id", arm_id_)) {
+ std::string arm_id;
+ if (!root_node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("CartesianVelocityExampleController: Could not get parameter arm_id");
return false;
}
@@ -33,7 +34,7 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot
}
try {
velocity_cartesian_handle_.reset(new franka_hw::FrankaCartesianVelocityHandle(
- velocity_cartesian_interface_->getHandle(arm_id_ + "_robot")));
+ velocity_cartesian_interface_->getHandle(arm_id + "_robot")));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
"CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what());
@@ -47,7 +48,7 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot
}
try {
- auto state_handle = state_interface->getHandle(arm_id_ + "_robot");
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
std::array<double, 7> q_start = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
for (size_t i = 0; i < q_start.size(); i++) {
diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp
index 5a50b76110165029f75b4d7514056f0b832c41fd..d8c9fe75a320a8ab36c722e0951aa1bfab2bf755 100644
--- a/franka_example_controllers/src/joint_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp
@@ -174,9 +174,8 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
// Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is
// 1000 * (1 / sampling_time).
- const double delta_tau_max = 1.0; // NOLINT (readability-identifier-naming)
std::array<double, 7> tau_d_saturated =
- saturateTorqueRate(delta_tau_max, tau_d_calculated, robot_state.tau_J_d, gravity);
+ saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d, gravity);
for (size_t i = 0; i < 7; ++i) {
joint_handles_[i].setCommand(tau_d_saturated[i]);
@@ -205,7 +204,6 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
}
std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate(
- const double delta_tau_max, // NOLINT (readability-identifier-naming)
const std::array<double, 7>& tau_d_calculated,
const std::array<double, 7>& tau_J_d, // NOLINT (readability-identifier-naming)
const std::array<double, 7>& gravity) {
@@ -214,7 +212,7 @@ std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate(
// TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
tau_d_saturated[i] =
- (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max), -delta_tau_max);
+ (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
}
return tau_d_saturated;
}
diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp
index 3d680d18ff660b03a5709f87fc01b68ca5023251..fed70ce2253b6d50d2deee8d4d6640099021f90a 100644
--- a/franka_example_controllers/src/joint_position_example_controller.cpp
+++ b/franka_example_controllers/src/joint_position_example_controller.cpp
@@ -32,11 +32,11 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har
return false;
}
position_joint_handles_.resize(7);
- joint_names_.resize(7);
+ std::array<std::string, 7> joint_names;
for (size_t i = 0; i < 7; ++i) {
- joint_names_[i] = static_cast<std::string>(parameters[i]);
+ joint_names[i] = static_cast<std::string>(parameters[i]);
try {
- position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names_[i]);
+ position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names[i]);
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
"JointPositionExampleController: Exception getting joint handles: " << e.what());
diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp
index 5b701c32f9c5321fb17d01f5d14af85fccac8517..8da0f487d678e914c607be54670f0e29fd16f49e 100644
--- a/franka_example_controllers/src/joint_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp
@@ -32,11 +32,11 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
return false;
}
velocity_joint_handles_.resize(7);
- joint_names_.resize(7);
+ std::array<std::string, 7> joint_names;
for (size_t i = 0; i < 7; ++i) {
- joint_names_[i] = static_cast<std::string>(parameters[i]);
+ joint_names[i] = static_cast<std::string>(parameters[i]);
try {
- velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names_[i]);
+ velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names[i]);
} catch (const hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM(
"JointVelocityExampleController: Exception getting joint handles: " << ex.what());
diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp
index dc165793f04fd88da27e835f421ba0cf8e1a80b0..67ddf05f9415b428ac431e0cbe7a6e8eb6e3034a 100644
--- a/franka_example_controllers/src/model_example_controller.cpp
+++ b/franka_example_controllers/src/model_example_controller.cpp
@@ -32,7 +32,6 @@ namespace franka_example_controllers {
bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& node_handle) {
std::string arm_id;
-
franka_state_interface_ = robot_hw->get<franka_hw::FrankaStateInterface>();
if (franka_state_interface_ == nullptr) {
ROS_ERROR("ModelExampleController: Could not get Franka state interface from hardware");