From 113dd49a385431da0d360acbc08e25a61d46674a Mon Sep 17 00:00:00 2001
From: Simon Gabl <simon.gabl@franka.de>
Date: Wed, 24 Jan 2018 16:16:27 +0100
Subject: [PATCH] Refactoring examples.
---
.../cartesian_impedance_example_controller.h | 16 +++++++---------
.../cartesian_pose_example_controller.h | 1 -
.../cartesian_velocity_example_controller.h | 1 -
.../joint_impedance_example_controller.h | 3 ++-
.../joint_position_example_controller.h | 1 -
.../joint_velocity_example_controller.h | 1 -
.../cartesian_impedance_example_controller.cpp | 15 ++++++++-------
.../src/cartesian_pose_example_controller.cpp | 7 ++++---
.../cartesian_velocity_example_controller.cpp | 7 ++++---
.../src/joint_impedance_example_controller.cpp | 6 ++----
.../src/joint_position_example_controller.cpp | 6 +++---
.../src/joint_velocity_example_controller.cpp | 6 +++---
.../src/model_example_controller.cpp | 1 -
13 files changed, 33 insertions(+), 38 deletions(-)
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 4b496cf..070328d 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 6a747df..e66503a 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 bbeba97..15d4970 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 148370b..778cc9b 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 2527cb3..a7c877d 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 d236007..1c36f4a 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 fec8a77..5c6d8eb 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 f3e96ed..6417b3e 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 4593601..afd91ff 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 5a50b76..d8c9fe7 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 3d680d1..fed70ce 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 5b701c3..8da0f48 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 dc16579..67ddf05 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");
--
GitLab