Added pass through joint torque feedback controller.

franka-interface-common/include/franka-interface-common/definitions.h

@@ -75,6 +75,7 @@ enum class FeedbackControllerType : uint8_t {
     JointImpedanceFeedbackController,
     NoopFeedbackController,
     PassThroughFeedbackController,
+    PassThroughJointTorqueFeedbackController,
     SetInternalImpedanceFeedbackController,
 };
 

franka-interface/CMakeLists.txt

@@ -36,6 +36,7 @@ add_library(franka-interface SHARED
   src/feedback_controller/joint_impedance_feedback_controller.cpp
   src/feedback_controller/noop_feedback_controller.cpp
   src/feedback_controller/pass_through_feedback_controller.cpp
+  src/feedback_controller/pass_through_joint_torque_feedback_controller.cpp
   src/feedback_controller/set_internal_impedance_feedback_controller.cpp
   src/skills/base_meta_skill.cpp
   src/skills/base_skill.cpp

franka-interface/include/franka-interface/feedback_controller/pass_through_joint_torque_feedback_controller.h

@@ -0,0 +1,31 @@
+#ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_JOINT_TORQUE_FEEDBACK_CONTROLLER_H_
+#define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_JOINT_TORQUE_FEEDBACK_CONTROLLER_H_
+
+#include "franka-interface/feedback_controller/feedback_controller.h"
+
+// A passthrough feedback controller that just uses the desired joint 
+// torques from the sensor subscriber and passes it to the robot
+class PassThroughJointTorqueFeedbackController : public FeedbackController {
+ public:
+  using FeedbackController::FeedbackController;
+
+  void parse_parameters() override;
+
+  void initialize_controller(FrankaRobot *robot) override;
+
+  void get_next_step(const franka::RobotState &robot_state, 
+                     TrajectoryGenerator *traj_generator) override;
+
+  void parse_sensor_data(const franka::RobotState &robot_state) override;
+
+ private:
+  JointTorqueFeedbackControllerMessage joint_torque_feedback_params_;
+  JointTorqueControllerSensorMessage joint_torque_sensor_msg_;
+  const franka::Model *model_;
+
+  std::array<double, 7> S_= {};
+  std::array<double, 7> desired_joint_torques_= {};
+
+};
+
+#endif  // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_JOINT_TORQUE_FEEDBACK_CONTROLLER_H_

franka-interface/proto/feedback_controller_params_msg.proto

@@ -30,3 +30,8 @@ message ForcePositionFeedbackControllerMessage {
   repeated double selection = 5;
   required bool use_cartesian_gains = 6;
 }
+
+message JointTorqueFeedbackControllerMessage {
+  repeated double selection = 1;
+  required double joint_torques = 2;
+}

franka-interface/proto/sensor_msg.proto

@@ -92,3 +92,11 @@ message ForcePositionControllerSensorMessage {
   repeated double force_kps_joint = 6;
   repeated double selection = 7;
 }
+
+message JointTorqueControllerSensorMessage {
+  required int32 id = 1;
+  required double timestamp = 2;
+
+  repeated double selection = 3;
+  repeated double joint_torques = 4;
+}

franka-interface/src/feedback_controller/pass_through_joint_torque_feedback_controller.cpp

@@ -0,0 +1,37 @@
+//
+// Created by Kevin on 9/13/24.
+//
+
+#include "franka-interface/feedback_controller/pass_through_joint_torque_feedback_controller.h"
+
+#include "franka-interface/trajectory_generator/impulse_trajectory_generator.h"
+
+void PassThroughJointTorqueFeedbackController::parse_parameters() {
+  // pass
+}
+
+void PassThroughJointTorqueFeedbackController::initialize_controller(FrankaRobot *robot) {
+  model_ = robot->getModel();
+}
+
+void PassThroughJointTorqueFeedbackController::parse_sensor_data(const franka::RobotState &robot_state) {
+  SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readFeedbackControllerSensorMessage(joint_torque_sensor_msg_);
+  if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) {
+    for (int i = 0; i < 7; i++) {
+        S_[i] = std::min(std::max(joint_torque_sensor_msg_.selection(i), 0.), 1.);
+        desired_joint_torques_[i] = joint_torque_sensor_msg_.joint_torques(i);
+    }
+  }
+}
+
+void PassThroughJointTorqueFeedbackController::get_next_step(const franka::RobotState &robot_state,
+                                                  TrajectoryGenerator *traj_generator) {
+
+  std::array<double, 7> gravity = model_->gravity(robot_state);
+
+  for (int i = 0; i < 7; i++) {
+    if (S_[i] == 1){
+        tau_d_array_[i] = desired_joint_torques_[i] - gravity[i];
+    }
+  }
+}

franka-interface/src/feedback_controller_factory.cpp

@@ -15,6 +15,7 @@
 #include "franka-interface/feedback_controller/joint_impedance_feedback_controller.h"
 #include "franka-interface/feedback_controller/noop_feedback_controller.h"
 #include "franka-interface/feedback_controller/pass_through_feedback_controller.h"
+#include "franka-interface/feedback_controller/pass_through_joint_torque_feedback_controller.h"
 #include "franka-interface/feedback_controller/set_internal_impedance_feedback_controller.h"
 
 FeedbackController* FeedbackControllerFactory::getFeedbackControllerForSkill(SharedBufferTypePtr buffer, SensorDataManager* sensor_data_manager){
@@ -53,6 +54,10 @@ FeedbackController* FeedbackControllerFactory::getFeedbackControllerForSkill(Sha
       feedback_controller_type_name = "PassThroughFeedbackController";
       feedback_controller = new PassThroughFeedbackController(buffer, sensor_data_manager);
       break;
+    case FeedbackControllerType::PassThroughJointTorqueFeedbackController:
+      feedback_controller_type_name = "PassThroughJointTorqueFeedbackController";
+      feedback_controller = new PassThroughJointTorqueFeedbackController(buffer, sensor_data_manager);
+      break;
     case FeedbackControllerType::SetInternalImpedanceFeedbackController:
       feedback_controller_type_name = "SetInternalImpedanceFeedbackController";
       feedback_controller = new SetInternalImpedanceFeedbackController(buffer, sensor_data_manager);