Updated joint torque control.

frankapy/franka_arm.py

@@ -874,6 +874,8 @@ def execute_joint_torques(self,
                     buffer_time=FC.DEFAULT_TERM_BUFFER_TIME,
                     force_thresholds=None,
                     torque_thresholds=None,
+                    k_gains=None,
+                    d_gains=None,
                     block=True,
                     ignore_errors=True,
                     ignore_virtual_walls=False,
@@ -906,6 +908,14 @@ def execute_joint_torques(self,
             torque_thresholds : :obj:`list` 
                 List of 7 floats corresponding to torque limits on each joint. 
                 Default is None. If None then will not stop on contact.
+            k_gains : :obj:`list` 
+                List of 7 floats corresponding to the k_gains on each joint for 
+                our impedance controller. Default is None. If None then will use 
+                default k_gains.
+            d_gains : :obj:`list` 
+                List of 7 floats corresponding to the d_gains on each joint for 
+                our impedance controller. Default is None. If None then will use 
+                default d_gains.
             block : :obj:`bool` 
                 Function blocks by default. If False, the function becomes
                 asynchronous and can be preempted.
@@ -930,6 +940,15 @@ def execute_joint_torques(self,
         else:
             block = True
 
+        if k_gains is None:
+            k_gains = FC.DEFAULT_K_GAINS
+        else:
+            k_gains = np.array(k_gains).tolist()
+        if d_gains is None:
+            d_gains = FC.DEFAULT_D_GAINS
+        else:
+            d_gains = np.array(d_gains).tolist()
+
         skill = Skill(SkillType.ImpedanceControlSkill, 
                     TrajectoryGeneratorType.StayInInitialJointsTrajectoryGenerator,
                     feedback_controller_type=FeedbackControllerType.PassThroughJointTorqueFeedbackController,
@@ -943,7 +962,7 @@ def execute_joint_torques(self,
         if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds):
             skill.add_time_termination_params(buffer_time)
 
-        skill.add_joint_torques(joint_torques, selection, remove_gravity)
+        skill.add_joint_torques(joint_torques, selection, remove_gravity, k_gains, d_gains)
 
         goal = skill.create_goal()
 

frankapy/proto/feedback_controller_params_msg.proto

@@ -35,4 +35,6 @@ message JointTorqueFeedbackControllerMessage {
   repeated double selection = 1;
   repeated double remove_gravity = 2;
   repeated double joint_torques = 3;
+  repeated double k_gains = 4;
+  repeated double d_gains = 5;
 }

frankapy/proto/sensor_msg.proto

@@ -100,4 +100,6 @@ message JointTorqueControllerSensorMessage {
   repeated double selection = 3;
   repeated double remove_gravity = 4;
   repeated double joint_torques = 5;
+  repeated double k_gains = 6;
+  repeated double d_gains = 7;
 }

frankapy/skill_list.py

@@ -158,18 +158,22 @@ 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, remove_gravity):
+    def add_joint_torques(self, joint_torques, selection, remove_gravity, k_gains, d_gains):
         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 type(k_gains) is list, "Incorrect k_gains type. Should be list."
+        assert type(d_gains) is list, "Incorrect d_gains 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 len(k_gains) == 7, "Incorrect k_gains len. Should be 7."
+        assert len(d_gains) == 7, "Incorrect d_gains 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, remove_gravity=remove_gravity)
+            JointTorqueFeedbackControllerMessage(joint_torques=joint_torques, selection=selection, remove_gravity=remove_gravity, k_gains=k_gains, d_gains=d_gains)
 
         self.add_feedback_controller_params(joint_torque_controller_msg_proto.SerializeToString())