Added joint torques example.

iamlab-cmu · Sep 13, 2024 · 16a9720 · 16a9720
1 parent 55e2115
commit 16a9720
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 13 deletions.
diff --git a/examples/execute_joint_torques.py 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)
diff --git 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
@@ -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, \