apply desired joint torque passed from the frankapy client

bash_scripts/start_control_pc.sh

@@ -37,7 +37,7 @@ old_gripper=0
 log_on_franka_interface=0
 stop_on_error=0
 
-while getopts ':h:i:u:p:d:r:s:g:o:l:e' option; do
+while getopts ':h:i:u:p:d:r:a:s:g:o:l:e' option; do
   case "${option}" in
     h) echo "$usage"
        exit

frankapy/franka_arm.py

@@ -89,15 +89,14 @@ def __init__(
         self._with_gripper = with_gripper
         self._old_gripper = old_gripper
         self._last_gripper_command = None
-
         # init ROS
         if init_node:
             rospy.init_node(rosnode_name,
                             disable_signals=True,
                             log_level=ros_log_level)
         self._collision_boxes_pub = BoxesPublisher('franka_collision_boxes_{}'.format(robot_num))
         self._joint_state_pub = rospy.Publisher('franka_virtual_joints_{}'.format(robot_num), JointState, queue_size=10)
-        
+
         self._state_client = FrankaArmStateClient(
                 new_ros_node=False,
                 robot_state_server_name=self._robot_state_server_name,
@@ -115,7 +114,6 @@ def __init__(
                     self._execute_skill_action_server_name, ExecuteSkillAction)
             self._client.wait_for_server()
             self.wait_for_franka_interface()
-
             if self._with_gripper and not self._old_gripper:
                 self._gripper_homing_client = SimpleActionClient(
                     self._gripper_homing_action_server_name, HomingAction)
@@ -171,6 +169,7 @@ def wait_for_franka_interface(self, timeout=None):
             if franka_interface_status.is_ready:
                 return
             sleep(1e-2)
+
         raise FrankaArmCommException('FrankaInterface status not ready for {}s'.format(
             FC.DEFAULT_FRANKA_INTERFACE_TIMEOUT))
 
@@ -550,6 +549,40 @@ def goto_pose_delta(self,
                         block=block,
                         ignore_errors=ignore_errors)
 
+    def apply_joint_torques(self,
+                            desired_torque,
+                            duration=5,
+                            buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,   
+                            skill_desc='ApplyJointTorque'                                                     
+                            ):
+        """
+        Commands Arm to apply desired joint torque
+            desired_torque : :obj:`list` 
+                A list of 7 numbers that correspond to joint joint torques in Nm. 
+            duration : :obj:`float` 
+                How much time this robot motion should take.                  
+            buffer_time : :obj:`float` 
+                How much extra time the termination handler will wait
+                before stopping the skill after duration has passed. 
+            skill_desc : :obj:`str` 
+                Skill description to use for logging on the Control PC.                                    
+        """        
+        skill = Skill(SkillType.JointTorqueSkill, 
+                      TrajectoryGeneratorType.MinJerkJointTrajectoryGenerator,
+                      feedback_controller_type=FeedbackControllerType.TorqueFeedbackController,
+                      termination_handler_type=TerminationHandlerType.TimeTerminationHandler, 
+                      skill_desc=skill_desc)    
+        skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
+        skill.add_time_termination_params(buffer_time)
+        skill.add_run_time(duration)
+        skill.add_goal_joints(duration, desired_torque)
+        goal = skill.create_goal()
+        self._send_goal(goal,
+                        cb=lambda x: skill.feedback_callback(x),
+                        block=False,
+                        ignore_errors=False)
+        sleep(FC.DYNAMIC_SKILL_WAIT_TIME)
+
 
     def goto_joints(self,
                     joints,
@@ -2092,24 +2125,6 @@ def is_joints_reachable(self, joints):
     """
     Unimplemented
     """
-
-    def apply_joint_torques(self, torques, duration, ignore_errors=True, skill_desc='',):
-        """
-        NOT IMPLEMENTED YET
-
-        Commands the arm to apply given joint torques for duration seconds.
-
-        Parameters
-        ----------
-            torques : :obj:`list` 
-                A list of 7 numbers that correspond to torques in Nm.
-            duration : :obj:`float`
-                How much time this robot motion should take.
-            skill_desc : :obj:`str` 
-                Skill description to use for logging on control-pc.
-        """
-        pass
-
     def set_speed(self, speed):
         """
         NOT IMPLEMENTED YET

frankapy/franka_interface_common_definitions.py

@@ -23,6 +23,7 @@ class SkillType:
     GripperSkill = _enum_auto('SkillType')
     ImpedanceControlSkill = _enum_auto('SkillType')
     JointPositionSkill = _enum_auto('SkillType')
+    JointTorqueSkill = _enum_auto('SkillType')
 
 
 class MetaSkillType:
@@ -64,7 +65,7 @@ class FeedbackControllerType:
     NoopFeedbackController = _enum_auto('FeedbackControllerType')
     PassThroughFeedbackController = _enum_auto('FeedbackControllerType')
     SetInternalImpedanceFeedbackController = _enum_auto('FeedbackControllerType')
-
+    TorqueFeedbackController = _enum_auto('FeedbackControllerType')
 
 class TerminationHandlerType:
     ContactTerminationHandler = _enum_auto('TerminationHandlerType')
@@ -91,3 +92,4 @@ class SensorDataMessageType:
     POSE_POSITION_VELOCITY = _enum_auto('SensorDataMessageType')
     POSE_POSITION = _enum_auto('SensorDataMessageType')
     SHOULD_TERMINATE = _enum_auto('SensorDataMessageType')
+    JOINT_TORQUE = _enum_auto('SensorDataMessageType')    

frankapy/proto/sensor_msg.proto

@@ -78,3 +78,19 @@ 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;
+}
+