From 16a97207684faada24a3768e4d7e93bc8ba96054 Mon Sep 17 00:00:00 2001
From: Kevin Zhang <kevinleezhang@gmail.com>
Date: Fri, 13 Sep 2024 16:01:52 -0400
Subject: [PATCH] Added joint torques example.

---
 examples/execute_joint_torques.py | 13 +++++++++++++
 frankapy/franka_arm.py            | 22 ++++++++++------------
 frankapy/skill_list.py            |  2 +-
 3 files changed, 24 insertions(+), 13 deletions(-)
 create mode 100644 examples/execute_joint_torques.py

diff --git a/examples/execute_joint_torques.py b/examples/execute_joint_torques.py
new file mode 100644
index 0000000..aefca13
--- /dev/null
+++ b/examples/execute_joint_torques.py
@@ -0,0 +1,13 @@
+import argparse
+from frankapy import FrankaArm
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--time', '-t', type=float, default=10)
+    args = parser.parse_args()
+
+    print('Starting robot')
+    fa = FrankaArm()
+    fa.reset_joints()
+    print('Applying 0 joint torque control for {}s'.format(args.time))
+    fa.execute_joint_torques([0,0,0,0,0,3,0], selection=[0,0,0,0,0,1,0], remove_gravity=[0,0,0,0,0,1,0], duration=args.time)
\ No newline at end of file
diff --git a/frankapy/franka_arm.py b/frankapy/franka_arm.py
index 0608d72..5ef011f 100644
--- a/frankapy/franka_arm.py
+++ b/frankapy/franka_arm.py
@@ -852,7 +852,7 @@ def execute_joint_velocities(self,
             if dynamic:
                 skill.add_time_termination_params(buffer_time)
             else:
-                skill.add_joint_threshold_params(buffer_time, FC.DEFAULT_JOINT_THRESHOLDS)
+                print("Unimplemented Joint Velocity Skill")
 
         skill.add_goal_joint_velocities(duration, joint_velocities, joint_accelerations)
         goal = skill.create_goal()
@@ -870,7 +870,7 @@ def execute_joint_torques(self,
                     selection=[0,0,0,0,0,0,0],
                     remove_gravity=[0,0,0,0,0,0,0],
                     duration=5,
-                    dynamic=True,
+                    dynamic=False,
                     buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
                     force_thresholds=None,
                     torque_thresholds=None,
@@ -926,24 +926,22 @@ def execute_joint_torques(self,
         selection = np.array(selection).tolist() 
 
         if dynamic:
-            skill = Skill(SkillType.ImpedanceControlSkill, 
-                          TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,
-                          feedback_controller_type=FeedbackControllerType.PassThroughJointTorqueFeedbackController,
-                          termination_handler_type=TerminationHandlerType.TimeTerminationHandler, 
-                          skill_desc=skill_desc)
             block = False
         else:
-            print("Unimplemented Joint Torque Skill")
+            block = True
+
+        skill = Skill(SkillType.ImpedanceControlSkill, 
+                    TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,
+                    feedback_controller_type=FeedbackControllerType.PassThroughJointTorqueFeedbackController,
+                    termination_handler_type=TerminationHandlerType.TimeTerminationHandler, 
+                    skill_desc=skill_desc)
 
         skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
 
         skill.add_run_time(duration)
 
         if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
-            if dynamic:
-                skill.add_time_termination_params(buffer_time)
-            else:
-                skill.add_joint_threshold_params(buffer_time, FC.DEFAULT_JOINT_THRESHOLDS)
+            skill.add_time_termination_params(buffer_time)
 
         skill.add_joint_torques(joint_torques, selection, remove_gravity)
 
diff --git a/frankapy/skill_list.py b/frankapy/skill_list.py
index 3655b84..8ba1172 100644
--- a/frankapy/skill_list.py
+++ b/frankapy/skill_list.py
@@ -171,7 +171,7 @@ def add_joint_torques(self, joint_torques, selection, remove_gravity):
         joint_torque_controller_msg_proto = \
             JointTorqueFeedbackControllerMessage(joint_torques=joint_torques, selection=selection, remove_gravity=remove_gravity)
 
-        self.add_torque_controller_params(joint_torque_controller_msg_proto.SerializeToString())
+        self.add_feedback_controller_params(joint_torque_controller_msg_proto.SerializeToString())
 
     def add_force_position_params(self, position_kps_cart, force_kps_cart, position_kps_joint, force_kps_joint, S, use_cartesian_gains):
         assert type(position_kps_cart) is list or len(position_kps_cart) == 6, \