Added removing gravity.

frankapy/franka_arm.py

@@ -867,7 +867,8 @@ def execute_joint_velocities(self,
 
     def execute_joint_torques(self,
                     joint_torques,
-                    selection,
+                    selection=[0,0,0,0,0,0,0],
+                    remove_gravity=[0,0,0,0,0,0,0],
                     duration=5,
                     dynamic=True,
                     buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
@@ -887,6 +888,9 @@ def execute_joint_torques(self,
             selection : :obj:`list` 
                 A list of 7 numbers that indicate whether to use the joint torques passed in or not. 
                 If 1 is passed in for a specific joint, the robot will use the joint torque for that joint.
+            remove_gravity : :obj:`list` 
+                A list of 7 numbers that indicate whether to remove gravity from that joint or not. 
+                If 1 is passed in for a specific joint, the robot will subtract gravity for that joint.
             duration : :obj:`float` 
                 How much time this robot motion should take.
             dynamic : :obj:`bool` 
@@ -941,7 +945,7 @@ def execute_joint_torques(self,
             else:
                 skill.add_joint_threshold_params(buffer_time, FC.DEFAULT_JOINT_THRESHOLDS)
 
-        skill.add_joint_torques(joint_torques, selection)
+        skill.add_joint_torques(joint_torques, selection, remove_gravity)
 
         goal = skill.create_goal()
 

frankapy/proto/feedback_controller_params_msg.proto

@@ -33,5 +33,6 @@ message ForcePositionFeedbackControllerMessage {
 
 message JointTorqueFeedbackControllerMessage {
   repeated double selection = 1;
-  required double joint_torques = 2;
+  repeated double remove_gravity = 2;
+  repeated double joint_torques = 3;
 }

frankapy/proto/sensor_msg.proto

@@ -98,5 +98,6 @@ message JointTorqueControllerSensorMessage {
   required double timestamp = 2;
 
   repeated double selection = 3;
-  repeated double joint_torques = 4;
+  repeated double remove_gravity = 4;
+  repeated double joint_torques = 5;
 }

frankapy/skill_list.py

@@ -158,16 +158,18 @@ def add_internal_impedances(self, cartesian_impedances, joint_impedances):
 
         self.add_feedback_controller_params(internal_feedback_controller_msg_proto.SerializeToString())
 
-    def add_joint_torques(self, joint_torques, selection):
+    def add_joint_torques(self, joint_torques, selection, remove_gravity):
         assert type(joint_torques) is list, "Incorrect joint_torques type. Should be list."
         assert type(selection) is list, "Incorrect selection type. Should be list."
+        assert type(remove_gravity) is list, "Incorrect remove_gravity type. Should be list."
         assert len(joint_torques) == 7, "Incorrect joint_torques len. Should be 7."
         assert len(selection) == 7, "Incorrect selection len. Should be 7."
+        assert len(remove_gravity) == 7, "Incorrect remove_gravity len. Should be 7."
         assert self._skill_type == SkillType.ImpedanceControlSkill, \
                 "Incorrect skill type. Should be ImpedanceControlSkill"
 
         joint_torque_controller_msg_proto = \
-            JointTorqueFeedbackControllerMessage(joint_torques=joint_torques, selection=selection)
+            JointTorqueFeedbackControllerMessage(joint_torques=joint_torques, selection=selection, remove_gravity=remove_gravity)
 
         self.add_torque_controller_params(joint_torque_controller_msg_proto.SerializeToString())