Adding KST param to deactivate joints, similar to controller

Author: stephenmcc

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java

@@ -224,6 +224,13 @@ public IKStreamingRTThread(String robotName,
                                                                                               fullRobotModelFactory,
                                                                                               yoGraphicsListRegistry,
                                                                                               registry);
+
+         List<String> inactiveJoints = parameters.getInactiveJoints();
+         for (int i = 0; i < inactiveJoints.size(); i++)
+         {
+            kinematicsStreamingToolboxController.getActiveOptimizationSettings().deactivateJoint(desiredFullRobotModel.getOneDoFJointByName(inactiveJoints.get(i)));
+         }
+
          kinematicsStreamingToolboxController.setCollisionModel(collisionModel);
 
          MessageUnpacker<WholeBodyStreamingMessage> wholeBodyStreamingMessageUnpacker = MessageUnpackingTools.createWholeBodyStreamingMessageUnpacker();

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java

@@ -8,6 +8,7 @@
 import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicsToolboxModule;
 import us.ihmc.avatar.networkProcessor.kinemtaticsStreamingToolboxModule.KinematicsStreamingToolboxParameters.ClockType;
 import us.ihmc.avatar.networkProcessor.modules.ToolboxController;
+import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseKinematics.InverseKinematicsOptimizationSettingsCommand;
 import us.ihmc.commons.Conversions;
 import us.ihmc.communication.controllerAPI.CommandInputManager;
 import us.ihmc.communication.controllerAPI.StatusMessageOutputManager;
@@ -253,6 +254,11 @@ public void setRobotStateUpdater(IKRobotStateUpdater robotStateUpdater)
       tools.setRobotStateUpdater(robotStateUpdater);
    }
 
+   public InverseKinematicsOptimizationSettingsCommand getActiveOptimizationSettings()
+   {
+      return tools.getIKController().getActiveOptimizationSettings();
+   }
+
    public void updateCapturabilityBasedStatus(CapturabilityBasedStatus newStatus)
    {
       tools.updateCapturabilityBasedStatus(newStatus);

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java

@@ -82,6 +82,11 @@ public KinematicsStreamingToolboxModule(DRCRobotModel robotModel,
                                                             registry);
       controller.setRobotStateUpdater(robotStateUpdater);
       controller.setCollisionModel(robotModel.getHumanoidRobotKinematicsCollisionModel());
+      List<String> inactiveJoints = parameters.getInactiveJoints();
+      for (int i = 0; i < inactiveJoints.size(); i++)
+      {
+         controller.getActiveOptimizationSettings().deactivateJoint(controller.getDesiredFullRobotModel().getOneDoFJointByName(inactiveJoints.get(i)));
+      }
       Map<String, Double> initialConfiguration = fromStandPrep(robotModel);
       if (initialConfiguration != null)
          controller.setInitialRobotConfigurationNamedMap(initialConfiguration);

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java

@@ -9,6 +9,8 @@
 import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
 import us.ihmc.commons.UnitConversions;
 
+import java.util.ArrayList;
+import java.util.List;
 import java.util.Map;
 
 public class KinematicsStreamingToolboxParameters
@@ -269,6 +271,8 @@ public enum ClockType
    private double solverPrivilegedDefaultWeight;
    private double solverPrivilegedDefaultGain;
 
+   private final List<String> inactiveJoints = new ArrayList<>();
+
    public static KinematicsStreamingToolboxParameters defaultParameters()
    {
       KinematicsStreamingToolboxParameters parameters = new KinematicsStreamingToolboxParameters();
@@ -1059,4 +1063,9 @@ public void setSolverPrivilegedDefaultGain(double solverPrivilegedDefaultGain)
    {
       this.solverPrivilegedDefaultGain = solverPrivilegedDefaultGain;
    }
+
+   public List<String> getInactiveJoints()
+   {
+      return inactiveJoints;
+   }
 }