diff --git a/franka_control/src/franka_control_node.cpp b/franka_control/src/franka_control_node.cpp
index 13ecf1adb3e7b14030d707bd6eb01694a8142cdc..f11565a95457b0c4f6c6d604f2a96369abc73005 100644
--- a/franka_control/src/franka_control_node.cpp
+++ b/franka_control/src/franka_control_node.cpp
@@ -95,12 +95,6 @@ int main(int argc, char** argv) {
{{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
{{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
- robot.setCollisionBehavior(
- {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
- {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
- {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
- {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
-
std::atomic_bool has_error(false);
using std::placeholders::_1;
@@ -142,7 +136,8 @@ 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);
+ franka_hw::FrankaHW franka_control(joint_names, arm_id, internal_controller, rate_limiting,
+ cutoff_freq, public_node_handle, model);
// Initialize robot state before loading any controller
franka_control.update(robot.readOnce());
diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h
index fe35e6b51826169183fe04c859be35f5640c792a..6e395bf7b7c86e2f6b6064f47966ffc2511f756b 100644
--- a/franka_hw/include/franka_hw/franka_hw.h
+++ b/franka_hw/include/franka_hw/franka_hw.h
@@ -48,9 +48,12 @@ 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] 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.
*/
FrankaHW(const std::array<std::string, 7>& joint_names,
@@ -66,9 +69,12 @@ 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] 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] 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] model Robot model.
*/
FrankaHW(const std::array<std::string, 7>& joint_names,
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
index 4756ee9fe9979c8a6fb008ebffa130537e02d10b..e549810677d78d374f2bf7a5c53713bcf786e133 100644
--- a/franka_hw/src/franka_hw.cpp
+++ b/franka_hw/src/franka_hw.cpp
@@ -275,31 +275,36 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
case ControlMode::JointTorque:
run_function_ = [this](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]);
+ std::cref(effort_joint_command_), ros_callback, _1, _2),
+ limit_rate_[4], cutoff_freq_[4]);
};
break;
case ControlMode::JointPosition:
run_function_ = [this](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]);
+ std::cref(position_joint_command_), ros_callback, _1, _2),
+ internal_controller_, limit_rate_[0], cutoff_freq_[0]);
};
break;
case ControlMode::JointVelocity:
run_function_ = [this](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]);
+ std::cref(velocity_joint_command_), ros_callback, _1, _2),
+ internal_controller_, limit_rate_[1], cutoff_freq_[1]);
};
break;
case ControlMode::CartesianPose:
run_function_ = [this](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]);
+ std::cref(pose_cartesian_command_), ros_callback, _1, _2),
+ internal_controller_, limit_rate_[2], cutoff_freq_[2]);
};
break;
case ControlMode::CartesianVelocity:
run_function_ = [this](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]);
+ std::cref(velocity_cartesian_command_), ros_callback, _1, _2),
+ internal_controller_, limit_rate_[3], cutoff_freq_[3]);
};
break;
case (ControlMode::JointTorque | ControlMode::JointPosition):
@@ -307,7 +312,8 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
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]);
+ std::cref(position_joint_command_), ros_callback, _1, _2),
+ limit_rate_[4], cutoff_freq_[4]);
};
break;
case (ControlMode::JointTorque | ControlMode::JointVelocity):
@@ -315,7 +321,8 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
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]);
+ std::cref(velocity_joint_command_), ros_callback, _1, _2),
+ limit_rate_[4], cutoff_freq_[4]);
};
break;
case (ControlMode::JointTorque | ControlMode::CartesianPose):
@@ -323,7 +330,8 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
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]);
+ std::cref(pose_cartesian_command_), ros_callback, _1, _2),
+ limit_rate_[4], cutoff_freq_[4]);
};
break;
case (ControlMode::JointTorque | ControlMode::CartesianVelocity):
@@ -331,7 +339,8 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
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]);
+ std::cref(velocity_cartesian_command_), ros_callback, _1, _2),
+ limit_rate_[4], cutoff_freq_[4]);
};
break;
default: