diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml
index 43f97246de922f0adcdafbca2e4a27768dad9247..b6ffea56a858873c31f77a9faf76ccef9595b5f0 100644
--- a/franka_example_controllers/config/franka_example_controllers.yaml
+++ b/franka_example_controllers/config/franka_example_controllers.yaml
@@ -38,20 +38,20 @@ joint_impedance_example_controller:
- panda_joint6
- panda_joint7
k_gains:
- - 1000.0
- - 1000.0
- - 1000.0
- - 1000.0
- - 500.0
- - 300.0
- - 100.0
+ - 600.0
+ - 600.0
+ - 600.0
+ - 600.0
+ - 250.0
+ - 150.0
+ - 50.0
d_gains:
- - 100.0
- - 100.0
- - 100.0
- - 100.0
- 50.0
- - 30.0
+ - 50.0
+ - 50.0
+ - 20.0
+ - 20.0
+ - 20.0
- 10.0
radius: 0.1
acceleration_time: 2.0
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 c10a7dafdb834c40d21636c4cebe89dee2596836..148370b553b62b647d8444dcfffec3c0ae51bae8 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,22 +30,31 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf
void update(const ros::Time&, const ros::Duration& period) override;
private:
+ 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);
+
std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
std::vector<hardware_interface::JointHandle> joint_handles_;
- std::array<double, 7> last_tau_d_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
- franka_hw::TriggerRate rate_trigger_{1.0};
+
double radius_{0.1};
double acceleration_time_{2.0};
double vel_max_{0.05};
- std::vector<std::string> joint_names_;
- std::vector<double> k_gains_;
- std::vector<double> d_gains_;
double angle_{0.0};
double vel_current_{0.0};
+
+ std::vector<double> k_gains_;
+ std::vector<double> d_gains_;
+ double coriolis_factor_{1.0};
+ std::array<double, 7> dq_filtered_;
std::array<double, 16> initial_pose_;
+
+ franka_hw::TriggerRate rate_trigger_{1.0};
realtime_tools::RealtimePublisher<JointTorqueComparison> torques_publisher_;
- double coriolis_factor_{1.0};
+ std::array<double, 7> last_tau_d_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
};
} // namespace franka_example_controllers
diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp
index 28025282161902b55286acaf7c9e45b7e9238335..5a50b76110165029f75b4d7514056f0b832c41fd 100644
--- a/franka_example_controllers/src/joint_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp
@@ -10,22 +10,13 @@
#include <franka/robot_state.h>
-namespace {
-template <class T, size_t N>
-std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
- ostream << "[";
- std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
- std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
- ostream << "]";
- return ostream;
-}
-} // anonymous namespace
-
namespace franka_example_controllers {
bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& node_handle) {
std::string arm_id;
+ std::vector<std::string> joint_names;
+
if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("JointImpedanceExampleController: Could not read parameter arm_id");
return false;
@@ -49,7 +40,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
<< acceleration_time_);
}
- 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(
"JointImpedanceExampleController: Invalid or no joint_names parameters provided, aborting "
"controller init!");
@@ -125,7 +116,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
}
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(
"JointImpedanceExampleController: Exception getting joint handles: " << ex.what());
@@ -134,6 +125,8 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
}
torques_publisher_.init(node_handle, "torque_comparison", 1);
+ std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0);
+
return true;
}
@@ -162,19 +155,31 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
cartesian_pose_handle_->setCommand(pose_desired);
franka::RobotState robot_state = cartesian_pose_handle_->getRobotState();
- std::array<double, 7> q_desired = robot_state.q_d;
- std::array<double, 7> q_current = robot_state.q;
- std::array<double, 7> dq = robot_state.dq;
std::array<double, 7> coriolis =
model_handle_->getCoriolis(robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
std::array<double, 7> gravity =
model_handle_->getGravity(robot_state.m_total, robot_state.F_x_Ctotal);
- std::array<double, 7> tau_d;
+ double alpha = 0.99;
+ for (size_t i = 0; i < 7; i++) {
+ dq_filtered_[i] = (1 - alpha) * dq_filtered_[i] + alpha * robot_state.dq[i];
+ }
+
+ std::array<double, 7> tau_d_calculated;
for (size_t i = 0; i < 7; ++i) {
- tau_d[i] = k_gains_[i] * (q_desired[i] - q_current[i]) - std::fabs(d_gains_[i]) * dq[i] +
- coriolis_factor_ * coriolis[i];
- joint_handles_[i].setCommand(tau_d[i]);
+ tau_d_calculated[i] = coriolis_factor_ * coriolis[i] +
+ k_gains_[i] * (robot_state.q_d[i] - robot_state.q[i]) +
+ d_gains_[i] * (robot_state.dq_d[i] - dq_filtered_[i]);
+ }
+
+ // 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);
+
+ for (size_t i = 0; i < 7; ++i) {
+ joint_handles_[i].setCommand(tau_d_saturated[i]);
}
if (rate_trigger_() && torques_publisher_.trylock()) {
@@ -195,10 +200,25 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
}
for (size_t i = 0; i < 7; ++i) {
- last_tau_d_[i] = tau_d[i] + gravity[i];
+ last_tau_d_[i] = tau_d_saturated[i] + gravity[i];
}
}
+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) {
+ std::array<double, 7> tau_d_saturated{};
+ for (size_t i = 0; i < 7; i++) {
+ // 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);
+ }
+ return tau_d_saturated;
+}
+
} // namespace franka_example_controllers
PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointImpedanceExampleController,