From 17012df073b299f1cce10321fd25ca1eb7a9362f Mon Sep 17 00:00:00 2001 From: Stephen McCrory Date: Fri, 28 Feb 2025 10:47:00 -0600 Subject: [PATCH] Adding KST param to deactivate joints, similar to controller --- .../IKStreamingRTPluginFactory.java | 7 +++++++ .../KinematicsStreamingToolboxController.java | 6 ++++++ .../KinematicsStreamingToolboxModule.java | 5 +++++ .../KinematicsStreamingToolboxParameters.java | 9 +++++++++ 4 files changed, 27 insertions(+) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java index d9aa7627852..c99e42423ca 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java +++ b/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 inactiveJoints = parameters.getInactiveJoints(); + for (int i = 0; i < inactiveJoints.size(); i++) + { + kinematicsStreamingToolboxController.getActiveOptimizationSettings().deactivateJoint(desiredFullRobotModel.getOneDoFJointByName(inactiveJoints.get(i))); + } + kinematicsStreamingToolboxController.setCollisionModel(collisionModel); MessageUnpacker wholeBodyStreamingMessageUnpacker = MessageUnpackingTools.createWholeBodyStreamingMessageUnpacker(); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java index 0f28542f294..74f138be4be 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java +++ b/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); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java index 626707f08e1..8298d4748b8 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java +++ b/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 inactiveJoints = parameters.getInactiveJoints(); + for (int i = 0; i < inactiveJoints.size(); i++) + { + controller.getActiveOptimizationSettings().deactivateJoint(controller.getDesiredFullRobotModel().getOneDoFJointByName(inactiveJoints.get(i))); + } Map initialConfiguration = fromStandPrep(robotModel); if (initialConfiguration != null) controller.setInitialRobotConfigurationNamedMap(initialConfiguration); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java index 28cbe1d59c1..842c396609e 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java +++ b/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 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 getInactiveJoints() + { + return inactiveJoints; + } }