Implementation of new skill to apply joint torque

bash_scripts/make_catkin.sh

@@ -1,4 +1,4 @@
 cd catkin_ws
-catkin build
+catkin build --cmake-args -DFranka_DIR=/root/git/franka-interface/libfranka/build
 source devel/setup.bash
 cd ..

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

@@ -26,6 +26,7 @@ enum class SkillType : uint8_t {
     GripperSkill,
     ImpedanceControlSkill,
     JointPositionSkill,
+    JointTorqueSkill,   
 };
 
 // Enum for Meta Skill Types
@@ -70,9 +71,10 @@ enum class FeedbackControllerType : uint8_t {
     NoopFeedbackController,
     PassThroughFeedbackController,
     SetInternalImpedanceFeedbackController,
+    TorqueFeedbackController                
 };
 
-// Enum for Termination Handler Types
+// Enum for Termination Handler Types 
 enum class TerminationHandlerType : uint8_t {
     ContactTerminationHandler,
     FinalJointTerminationHandler,
@@ -106,6 +108,7 @@ enum class SensorDataMessageType : uint8_t {
   POSE_POSITION_VELOCITY,
   POSE_POSITION,
   SHOULD_TERMINATE,
+  JOINT_TORQUE
 };
 
 #endif  // FRANKA_INTERFACE_COMMON_DEFINITIONS_H_

franka-interface/CMakeLists.txt

@@ -28,6 +28,7 @@ include_directories( ${Boost_INCLUDE_DIRS} )
 ## Library
 
 add_library(franka-interface SHARED
+  src/feedback_controller/torque_feedback_controller.cpp
   src/feedback_controller/cartesian_impedance_feedback_controller.cpp
   src/feedback_controller/ee_cartesian_impedance_feedback_controller.cpp
   src/feedback_controller/feedback_controller.cpp
@@ -45,6 +46,7 @@ add_library(franka-interface SHARED
   src/skills/impedance_control_skill.cpp
   src/skills/joint_position_continuous_skill.cpp
   src/skills/joint_position_skill.cpp
+  src/skills/joint_torque_skill.cpp  
   src/termination_handler/contact_termination_handler.cpp
   src/termination_handler/final_joint_termination_handler.cpp
   src/termination_handler/final_pose_termination_handler.cpp

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

@@ -0,0 +1,23 @@
+#ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_TORQUE_FEEDBACK_CONTROLLER_H_
+#define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_TORQUE_FEEDBACK_CONTROLLER_H_
+
+
+#include <Eigen/Dense>
+
+#include "franka-interface/feedback_controller/feedback_controller.h"
+
+class TorqueFeedbackController : public FeedbackController{
+  public:
+    using FeedbackController::FeedbackController;
+    void parse_parameters() override;    
+    void initialize_controller(FrankaRobot *robot) override;
+    void parse_sensor_data(const franka::RobotState &robot_state) override;
+    void get_next_step(const franka::RobotState &robot_state, 
+                        TrajectoryGenerator *traj_generator) override;
+
+  protected: 
+    TorqueControllerSensorMessage torque_feedback_sensor_msg_;
+    Eigen::VectorXd tau_d;    
+};
+
+#endif  // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_TORQUE_FEEDBACK_CONTROLLER_H_

franka-interface/include/franka-interface/skills/joint_torque_skill.h

@@ -0,0 +1,30 @@
+
+#ifndef FRANKA_INTERFACE_SKILLS_JOINT_TORQUE_SKILL_H_
+#define FRANKA_INTERFACE_SKILLS_JOINT_TORQUE_SKILL_H_
+
+#include "franka-interface/skills/base_skill.h"
+
+class JointTorqueSkill: public BaseSkill{
+    public:
+        JointTorqueSkill(int skill_idx, int meta_skill_idx, std::string description):
+                                    BaseSkill(skill_idx, meta_skill_idx, description)
+        {}; 
+        void limit_current_joint_torques(double period);        
+        void execute_skill_on_franka(run_loop* run_loop,
+                                    FrankaRobot* robot, 
+                                    FrankaGripper* gripper,
+                                    RobotStateData* robot_state_data) override;
+    private:
+        bool return_status_{false};
+
+        double safety_factor = 0.1;
+
+        std::array<double, 7> previous_joint_torques_;
+        std::array<double, 7> current_joint_torques_;
+        std::array<double, 7> current_joint_rotatums_;
+
+        // Franka Parameters from https://frankaemika.github.io/docs/control_parameters.html
+        std::array<double, 7> max_joint_torques_{87, 87, 87, 87, 12, 12, 12}; // Nm
+        std::array<double, 7> max_joint_rotatums_{1000, 1000, 1000, 1000, 1000, 1000, 1000}; // Nm / s        
+        }; 
+#endif // FRANKA_INTERFACE_SKILLS_JOINT_TORQUE_SKILL_H_

franka-interface/proto/sensor_msg.proto

@@ -78,3 +78,18 @@ message ForcePositionControllerSensorMessage {
   repeated double force_kps_joint = 6;
   repeated double selection = 7;
 }
+
+message JointImpedanceSensorMessage {
+  required int32 id = 1;
+  required double timestamp = 2;
+
+  repeated double joint_stiffnesses = 3;
+
+}
+
+message TorqueControllerSensorMessage{
+  required int32 id = 1;
+  required double timestamp = 2;
+
+  repeated double joint_torques_cmd = 3;
+}

franka-interface/src/feedback_controller/torque_feedback_controller.cpp

@@ -0,0 +1,25 @@
+#include <exception>
+#include "franka-interface/feedback_controller/torque_feedback_controller.h"
+
+void TorqueFeedbackController::parse_parameters(){
+
+}
+void TorqueFeedbackController::initialize_controller(FrankaRobot *robot) {
+    tau_d.resize(7);
+    tau_d.setZero();
+}
+
+void TorqueFeedbackController::parse_sensor_data(const franka::RobotState &robot_state){
+    tau_d.resize(7);
+    tau_d.setZero();
+    SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readFeedbackControllerSensorMessage(torque_feedback_sensor_msg_);
+    if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) {
+        for(int i = 0; i < 7 ; i++){
+            tau_d(i) = torque_feedback_sensor_msg_.joint_torques_cmd(i);
+        }
+    }
+}
+void TorqueFeedbackController::get_next_step(const franka::RobotState &robot_state,
+                                            TrajectoryGenerator *traj_generator){
+  Eigen::VectorXd::Map(&tau_d_array_[0], 7) = tau_d;
+}

franka-interface/src/feedback_controller_factory.cpp

@@ -16,6 +16,7 @@
 #include "franka-interface/feedback_controller/noop_feedback_controller.h"
 #include "franka-interface/feedback_controller/pass_through_feedback_controller.h"
 #include "franka-interface/feedback_controller/set_internal_impedance_feedback_controller.h"
+#include "franka-interface/feedback_controller/torque_feedback_controller.h"
 
 FeedbackController* FeedbackControllerFactory::getFeedbackControllerForSkill(SharedBufferTypePtr buffer, SensorDataManager* sensor_data_manager){
   FeedbackControllerType feedback_controller_type = static_cast<FeedbackControllerType>(buffer[0]);
@@ -24,7 +25,10 @@ FeedbackController* FeedbackControllerFactory::getFeedbackControllerForSkill(Sha
 
   FeedbackController* feedback_controller = nullptr;
   switch (feedback_controller_type) {
-
+    case FeedbackControllerType::TorqueFeedbackController:
+      feedback_controller_type_name = "TorqueFeedbackController";
+      feedback_controller = new TorqueFeedbackController(buffer, sensor_data_manager);      
+      break;
     case FeedbackControllerType::CartesianImpedanceFeedbackController:
       feedback_controller_type_name = "CartesianImpedanceFeedbackController";
       feedback_controller = new CartesianImpedanceFeedbackController(buffer, sensor_data_manager);

franka-interface/src/run_loop.cpp

@@ -23,6 +23,7 @@
 #include "franka-interface/skills/cartesian_pose_skill.h"
 #include "franka-interface/skills/force_torque_skill.h"
 #include "franka-interface/skills/gripper_skill.h"
+#include "franka-interface/skills/joint_torque_skill.h"
 #include "franka-interface/skills/impedance_control_skill.h"
 #include "franka-interface/skills/joint_position_continuous_skill.h"
 #include "franka-interface/skills/joint_position_skill.h"
@@ -103,7 +104,7 @@ void run_loop::start_new_skill(BaseSkill* new_skill) {
   RunLoopProcessInfo* run_loop_info = shared_memory_handler_->getRunLoopProcessInfo();
   int memory_index = run_loop_info->get_current_shared_memory_index();
   std::cout << string_format("Create skill from memory index: %d\n", memory_index);
-
+  
   SharedBufferTypePtr traj_buffer = shared_memory_handler_->getTrajectoryGeneratorBuffer(memory_index);
   TrajectoryGenerator *traj_generator = traj_gen_factory_.getTrajectoryGeneratorForSkill(
       traj_buffer, sensor_data_manager_);
@@ -112,7 +113,7 @@ void run_loop::start_new_skill(BaseSkill* new_skill) {
       memory_index);
   FeedbackController *feedback_controller =
       feedback_controller_factory_.getFeedbackControllerForSkill(feedback_controller_buffer, sensor_data_manager_);
-
+  
   SharedBufferTypePtr termination_handler_buffer = shared_memory_handler_->getTerminationParametersBuffer(
       memory_index);
   TerminationHandler* termination_handler =
@@ -254,7 +255,11 @@ void run_loop::update_process_info() {
             case SkillType::JointPositionSkill:
               skill_type_name = "JointPositionSkill";
               new_skill = new JointPositionSkill(new_skill_id, new_meta_skill_id, new_skill_description);
-              break;
+              break;   
+            case SkillType::JointTorqueSkill:
+              skill_type_name = "JointTorqueSkill";
+              new_skill = new JointTorqueSkill(new_skill_id, new_meta_skill_id, new_skill_description);
+              break;                        
             default:
               std::cout << "Incorrect skill type: " << 
               static_cast<std::underlying_type<SkillType>::type>(new_skill_type) << 
@@ -603,4 +608,4 @@ RunLoopSharedMemoryHandler* run_loop::get_shared_memory_handler() {
 
 SensorDataManager* run_loop::get_sensor_data_manager() {
   return sensor_data_manager_;
-}
+}

franka-interface/src/skills/joint_torque_skill.cpp

@@ -0,0 +1,130 @@
+#include "franka-interface/skills/joint_torque_skill.h"
+#include <franka/robot.h>
+
+#include "franka-interface/robot_state_data.h"
+#include "franka-interface/run_loop.h"
+#include "franka-interface/run_loop_shared_memory_handler.h"
+#include "franka-interface/feedback_controller/set_internal_impedance_feedback_controller.h"
+#include "franka-interface/trajectory_generator/joint_trajectory_generator.h"
+#include <franka-interface-common/run_loop_process_info.h>
+
+
+
+void JointTorqueSkill::limit_current_joint_torques(double period) {
+  for(int i = 0; i < 7; i++) {
+    if(std::abs(current_joint_torques_[i]) > max_joint_torques_[i] * safety_factor) {
+      if(current_joint_torques_[i] > 0) {
+        current_joint_torques_[i] = max_joint_torques_[i] * safety_factor;
+      } else {
+        current_joint_torques_[i] = -max_joint_torques_[i] * safety_factor;
+      }
+    }
+    current_joint_rotatums_[i] = (current_joint_torques_[i] - previous_joint_torques_[i]) / period;
+
+    if(std::abs(current_joint_rotatums_[i]) > max_joint_rotatums_[i] * safety_factor) {
+      if(current_joint_rotatums_[i] > 0) {
+        current_joint_rotatums_[i] = max_joint_rotatums_[i] * safety_factor;
+      } else {
+        current_joint_rotatums_[i] = -max_joint_rotatums_[i] * safety_factor;
+      }
+    }
+    current_joint_torques_[i] = previous_joint_torques_[i] + current_joint_rotatums_[i] * period;
+  }
+}
+void JointTorqueSkill::execute_skill_on_franka(run_loop* run_loop,
+                                                FrankaRobot* robot,
+                                                FrankaGripper* gripper,
+                                                RobotStateData *robot_state_data) {
+  double time = 0.0; 
+  int log_counter = 0; 
+  std::array<double, 16> pose_desired; 
+
+  RunLoopSharedMemoryHandler* shared_memory_handler = run_loop->get_shared_memory_handler();
+  RunLoopProcessInfo* run_loop_info = shared_memory_handler->getRunLoopProcessInfo();
+  boost::interprocess::scoped_lock<boost::interprocess::interprocess_mutex> lock(
+                                  *(shared_memory_handler->getRunLoopProcessInfoMutex()),
+                                  boost::interprocess::defer_lock);
+  SensorDataManager* sensor_data_manager = run_loop->get_sensor_data_manager();
+
+  std::cout << "Will run the control loop\n";
+
+  std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
+      joint_torque_callback = [&](
+      const franka::RobotState& robot_state,
+      franka::Duration period) -> franka::Torques {
+
+    current_period_ = period.toSec();
+    time += current_period_;        
+    if(time == 0.0 ){
+      for(int i = 0; i < 7; i++) {
+        current_joint_torques_[i] = 0;
+        previous_joint_torques_[i] = 0;
+      }
+
+      try {
+        if (lock.try_lock()) {
+          run_loop_info->set_time_skill_started_in_robot_time(robot_state.time.toSec());
+          lock.unlock();
+        } 
+      } catch (boost::interprocess::lock_exception) {
+            // Do nothing
+      }                
+    }
+
+    if (robot_state_data->mutex_.try_lock()) {
+      robot_state_data->counter_ += 1;
+      robot_state_data->time_ =  period.toSec();
+      robot_state_data->has_data_ = true;
+      robot_state_data->mutex_.unlock();
+    }
+
+    log_counter += 1;            
+    try {
+      sensor_data_manager->getSensorBufferGroupMutex()->try_lock();
+      traj_generator_->parse_sensor_data(robot_state);
+      termination_handler_->parse_sensor_data(robot_state);
+      sensor_data_manager->getSensorBufferGroupMutex()->unlock();
+      } catch (boost::interprocess::lock_exception) {
+    }
+
+    if (log_counter % 1 == 0) {
+      pose_desired = robot_state.O_T_EE_d;
+      robot_state_data->log_robot_state(pose_desired, robot_state, robot->getModel(), time);
+    }            
+
+    feedback_controller_->parse_sensor_data(robot_state);
+    feedback_controller_->get_next_step(robot_state, traj_generator_);
+
+    bool done = termination_handler_->should_terminate(robot_state, model_, traj_generator_);
+    if (done && time > 0.0) {            
+      try{
+        if (lock.try_lock()) {
+          run_loop_info->set_time_skill_finished_in_robot_time(robot_state.time.toSec());
+          lock.unlock();
+        } 
+      } catch (boost::interprocess::lock_exception) {
+        // Do nothing        
+      }
+
+      return franka::MotionFinished(franka::Torques(feedback_controller_->tau_d_array_));
+    }
+
+    for(int i = 0; i < 7; i++) {
+      current_joint_torques_[i] = feedback_controller_->tau_d_array_[i];
+    }
+
+    if (period.toSec() > 0.0){ 
+      limit_current_joint_torques(current_period_);     
+    }
+
+    for(int i = 0; i < 7; i++) {
+      previous_joint_torques_[i] = current_joint_torques_[i];
+    }
+
+    return current_joint_torques_;   
+
+  };
+
+  robot->robot_.control(joint_torque_callback, true);    
+
+}