Added some missing things.

iamlab-cmu · Sep 13, 2024 · 1b1ca2f · 1b1ca2f
1 parent 7ea77ee
commit 1b1ca2f
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git a/franka-interface-common/include/franka-interface-common/definitions.h b/franka-interface-common/include/franka-interface-common/definitions.h
@@ -112,6 +112,7 @@ enum class SensorDataMessageType : uint8_t {
   JOINT_POSITION_VELOCITY,
   JOINT_POSITION,
   JOINT_VELOCITY,
+  JOINT_TORQUE,
   POSE_POSITION_VELOCITY,
   POSE_POSITION,
   SHOULD_TERMINATE,

diff --git a/franka-interface/src/feedback_controller/pass_through_joint_torque_feedback_controller.cpp b/franka-interface/src/feedback_controller/pass_through_joint_torque_feedback_controller.cpp
@@ -7,7 +7,18 @@
 #include "franka-interface/trajectory_generator/impulse_trajectory_generator.h"
 
 void PassThroughJointTorqueFeedbackController::parse_parameters() {
-  // pass
+  int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24));
+
+  bool parsed_params = joint_torque_feedback_params_.ParseFromArray(params_ + 5, data_size);
+
+  if (parsed_params){
+    for (int i = 0; i < 7; i++) {
+        S_[i] = std::min(std::max(joint_torque_feedback_params_.selection(i), 0.), 1.);
+        desired_joint_torques_[i] = joint_torque_feedback_params_.joint_torques(i);
+    }
+  } else {
+    std::cout << "Parsing JointTorqueFeedbackController params failed. Data size = " << data_size << std::endl;
+  }
 }
 
 void PassThroughJointTorqueFeedbackController::initialize_controller(FrankaRobot *robot) {