Removed argument from method getDefaultControlModeForRigidBody in the WalkingControllerParameters.

Author: gwiedebach

Atlas/src/us/ihmc/atlas/parameters/AtlasWalkingControllerParameters.java

@@ -488,16 +488,11 @@ private PID3DGains createHandPositionControlGains()
 
    /** {@inheritDoc} */
    @Override
-   public RigidBodyControlMode getDefaultControlModeForRigidBody(String bodyName)
+   public Map<String, RigidBodyControlMode> getDefaultControlModesForRigidBodies()
    {
-      if (bodyName.equals(jointMap.getChestName()))
-      {
-         return RigidBodyControlMode.TASKSPACE;
-      }
-      else
-      {
-         return RigidBodyControlMode.JOINTSPACE;
-      }
+      Map<String, RigidBodyControlMode> defaultControlModes = new HashMap<>();
+      defaultControlModes.put(jointMap.getChestName(), RigidBodyControlMode.TASKSPACE);
+      return defaultControlModes;
    }
 
    /** {@inheritDoc} */

CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/configurations/WalkingControllerParameters.java

@@ -237,16 +237,20 @@ public List<ImmutableTriple<String, PID3DGains, List<String>>> getTaskspacePosit
    }
 
    /**
-    * Returns the default control mode for a rigid body. The modes are defined in {@link RigidBodyControlMode}
-    * and by default the mode should be {@link RigidBodyControlMode#JOINTSPACE}. In some cases (e.g. the chest)
-    * it makes more sense to use the default mode {@link RigidBodyControlMode#TASKSPACE}.
+    * Returns a map with default control modes for each rigid body.
+    * <p>
+    * The key of the map is the rigid body name as defined in the joint map. Possible
+    * control modes are defined in {@link RigidBodyControlMode}. By default (if a body
+    * is not contained in the map) {@link RigidBodyControlMode#JOINTSPACE} will be used
+    * for the body. In some cases (e.g. the chest) it makes more sense to use the default
+    * mode {@link RigidBodyControlMode#TASKSPACE}.
+    * </p>
     *
-    * @param bodyName is the name of the {@link RigidBody}
     * @return the default control mode of the body
     */
-   public RigidBodyControlMode getDefaultControlModeForRigidBody(String bodyName)
+   public Map<String, RigidBodyControlMode> getDefaultControlModesForRigidBodies()
    {
-      return RigidBodyControlMode.JOINTSPACE;
+      return new HashMap<>();
    }
 
    /**

CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java

@@ -153,6 +153,12 @@ public void setGains(Map<String, YoPIDGains> jointspaceGains, YoPID3DGains tasks
 
    public void setDefaultControlMode(RigidBodyControlMode defaultControlMode)
    {
+      if (defaultControlMode == null)
+      {
+         this.defaultControlMode.set(RigidBodyControlMode.JOINTSPACE);
+         return;
+      }
+
       if (defaultControlMode == RigidBodyControlMode.TASKSPACE && homePose == null)
       {
          throw new RuntimeException("Need to define home pose if default control mode for body " + bodyName + " is set to TASKSPACE.");

CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/HighLevelControlManagerFactory.java

@@ -164,7 +164,7 @@ public RigidBodyControlManager getOrCreateRigidBodyManager(RigidBody bodyToContr
 
       ContactablePlaneBody contactableBody = controllerToolbox.getContactableBody(bodyToControl);
       YoGraphicsListRegistry graphicsListRegistry = controllerToolbox.getYoGraphicsListRegistry();
-      RigidBodyControlMode defaultControlModeForRigidBody = walkingControllerParameters.getDefaultControlModeForRigidBody(bodyName);
+      RigidBodyControlMode defaultControlModeForRigidBody = walkingControllerParameters.getDefaultControlModesForRigidBodies().get(bodyName);
 
       RigidBodyControlManager manager = new RigidBodyControlManager(bodyToControl, baseBody, elevator, homeConfiguration, homePose, positionControlledJoints,
             integrationSettings, trajectoryFrames, controlFrame, baseFrame, contactableBody, yoTime, graphicsListRegistry, registry);

IHMCAvatarInterfaces/test/us/ihmc/avatar/controllerAPI/EndToEndChestTrajectoryMessageTest.java

@@ -34,8 +34,6 @@
 import us.ihmc.humanoidRobotics.communication.packets.walking.ChestTrajectoryMessage;
 import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
-import us.ihmc.yoVariables.variable.YoDouble;
-import us.ihmc.yoVariables.variable.YoInteger;
 import us.ihmc.robotics.geometry.FrameOrientation;
 import us.ihmc.robotics.math.trajectories.waypoints.MultipleWaypointsOrientationTrajectoryGenerator;
 import us.ihmc.robotics.math.trajectories.waypoints.SimpleSO3TrajectoryPoint;
@@ -50,6 +48,8 @@
 import us.ihmc.simulationconstructionset.util.simulationTesting.SimulationTestingParameters;
 import us.ihmc.tools.MemoryTools;
 import us.ihmc.tools.thread.ThreadTools;
+import us.ihmc.yoVariables.variable.YoDouble;
+import us.ihmc.yoVariables.variable.YoInteger;
 
 public abstract class EndToEndChestTrajectoryMessageTest implements MultiRobotTestInterface
 {
@@ -244,82 +244,82 @@ public void testSelectionMatrixWithAllAxisOffUsingSingleTrajectoryPoint() throws
    public void testSettingWeightMatrixUsingSingleTrajectoryPoint() throws Exception
    {
       BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
-      
+
       Random random = new Random(564574L);
-      
+
       DRCObstacleCourseStartingLocation selectedLocation = DRCObstacleCourseStartingLocation.DEFAULT_BUT_ALMOST_PI;
-      
+
       drcSimulationTestHelper = new DRCSimulationTestHelper(getClass().getSimpleName(), selectedLocation, simulationTestingParameters, getRobotModel());
       SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet();
-      
+
       ThreadTools.sleep(1000);
       boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.5);
       assertTrue(success);
-      
+
       FullHumanoidRobotModel fullRobotModel = drcSimulationTestHelper.getControllerFullRobotModel();
       HumanoidReferenceFrames humanoidReferenceFrames = new HumanoidReferenceFrames(fullRobotModel);
       ReferenceFrame pelvisZUpFrame = humanoidReferenceFrames.getPelvisZUpFrame();
       humanoidReferenceFrames.updateFrames();
-      
+
       double trajectoryTime = 1.0;
       RigidBody pelvis = fullRobotModel.getPelvis();
       RigidBody chest = fullRobotModel.getChest();
-      
+
       OneDoFJoint[] spineClone = ScrewTools.cloneOneDoFJointPath(pelvis, chest);
-      
+
       for(int i = 0; i < 50; i++)
       {
          ScrewTestTools.setRandomPositionsWithinJointLimits(spineClone, random);
          RigidBody chestClone = spineClone[spineClone.length - 1].getSuccessor();
          FrameOrientation desiredRandomChestOrientation = new FrameOrientation(chestClone.getBodyFixedFrame());
          desiredRandomChestOrientation.changeFrame(ReferenceFrame.getWorldFrame());
-         
+
          Quaternion desiredOrientation = new Quaternion();
          desiredRandomChestOrientation.getQuaternion(desiredOrientation);
-         
+
          ChestTrajectoryMessage chestTrajectoryMessage = new ChestTrajectoryMessage(trajectoryTime, desiredOrientation, pelvisZUpFrame, pelvisZUpFrame);
          chestTrajectoryMessage.setExecutionMode(ExecutionMode.OVERRIDE, -1);
-         
+
          WeightMatrix3D weightMatrix = new WeightMatrix3D();
-         
+
          double xWeight = random.nextDouble();
          double yWeight = random.nextDouble();
          double zWeight = random.nextDouble();
-         
+
          weightMatrix.setWeights(xWeight, yWeight, zWeight);
          chestTrajectoryMessage.setWeightMatrix(weightMatrix);
          drcSimulationTestHelper.send(chestTrajectoryMessage);
-         
+
          assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getControllerDT() * 4.0));
          assertWeightsMatch(xWeight, yWeight, zWeight, chest, scs);
       }
-      
+
       ScrewTestTools.setRandomPositionsWithinJointLimits(spineClone, random);
       RigidBody chestClone = spineClone[spineClone.length - 1].getSuccessor();
       FrameOrientation desiredRandomChestOrientation = new FrameOrientation(chestClone.getBodyFixedFrame());
       desiredRandomChestOrientation.changeFrame(ReferenceFrame.getWorldFrame());
-      
+
       Quaternion desiredOrientation = new Quaternion();
       desiredRandomChestOrientation.getQuaternion(desiredOrientation);
-      
+
       ChestTrajectoryMessage chestTrajectoryMessage = new ChestTrajectoryMessage(trajectoryTime, desiredOrientation, pelvisZUpFrame, pelvisZUpFrame);
       chestTrajectoryMessage.setExecutionMode(ExecutionMode.OVERRIDE, -1);
-      
+
       WeightMatrix3D weightMatrix = new WeightMatrix3D();
-      
+
       double xWeight = Double.NaN;
       double yWeight = Double.NaN;
       double zWeight = Double.NaN;
-      
+
       weightMatrix.setWeights(xWeight, yWeight, zWeight);
       chestTrajectoryMessage.setWeightMatrix(weightMatrix);
       drcSimulationTestHelper.send(chestTrajectoryMessage);
-      
+
       assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getControllerDT() * 4.0));
       assertAngularWeightsMatchDefault(chest, scs);
-      
+
    }
-   
+
    private void assertAngularWeightsMatchDefault(RigidBody rigidBody, SimulationConstructionSet scs)
    {
       String prefix = rigidBody.getName() + "Taskspace";
@@ -332,7 +332,7 @@ private void assertAngularWeightsMatchDefault(RigidBody rigidBody, SimulationCon
       assertEquals(defaultAngularWeightX.getDoubleValue(), angularWeightX.getDoubleValue(), 1e-8);
       assertEquals(defaultAngularWeightY.getDoubleValue(), angularWeightY.getDoubleValue(), 1e-8);
       assertEquals(defaultAngularWeightZ.getDoubleValue(), angularWeightZ.getDoubleValue(), 1e-8);
-      
+
    }
 
    private void assertWeightsMatch(double xWeight, double yWeight, double zWeight, RigidBody rigidBody, SimulationConstructionSet scs)
@@ -345,7 +345,7 @@ private void assertWeightsMatch(double xWeight, double yWeight, double zWeight,
 //      YoDouble linearWeightX = (YoDouble) scs.getVariable(prefix + "LinearWeightX_RO");
 //      YoDouble linearWeightY = (YoDouble) scs.getVariable(prefix + "LinearWeightY_RO");
 //      YoDouble linearWeightZ = (YoDouble) scs.getVariable(prefix + "LinearWeightZ_RO");
-      
+
       assertEquals(xWeight, angularWeightX.getDoubleValue(), 1e-8);
       assertEquals(yWeight, angularWeightY.getDoubleValue(), 1e-8);
       assertEquals(zWeight, angularWeightZ.getDoubleValue(), 1e-8);
@@ -968,7 +968,11 @@ public void testQueueWithWrongPreviousId() throws Exception
       success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.05 + getRobotModel().getControllerDT());
       assertTrue(success);
 
-      RigidBodyControlMode defaultControlMode = getRobotModel().getWalkingControllerParameters().getDefaultControlModeForRigidBody(chest.getName());
+      RigidBodyControlMode defaultControlMode = getRobotModel().getWalkingControllerParameters().getDefaultControlModesForRigidBodies().get(chest.getName());
+      if (defaultControlMode == null)
+      {
+         defaultControlMode = RigidBodyControlMode.JOINTSPACE;
+      }
       assertEquals(defaultControlMode, EndToEndArmTrajectoryMessageTest.findControllerState(chest.getName(), scs));
       assertNumberOfWaypoints(1, scs, chest);
    }
@@ -1031,7 +1035,11 @@ public void testQueueWithUsingDifferentTrajectoryFrameWithoutOverride() throws E
       LookLeftMessageWithChangeTrajFrame.setExecutionMode(ExecutionMode.QUEUE, -1);
       drcSimulationTestHelper.send(LookLeftMessageWithChangeTrajFrame);
       assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getControllerDT() * 2.0));
-      RigidBodyControlMode defaultControlMode = getRobotModel().getWalkingControllerParameters().getDefaultControlModeForRigidBody(chest.getName());
+      RigidBodyControlMode defaultControlMode = getRobotModel().getWalkingControllerParameters().getDefaultControlModesForRigidBodies().get(chest.getName());
+      if (defaultControlMode == null)
+      {
+         defaultControlMode = RigidBodyControlMode.JOINTSPACE;
+      }
       assertEquals(defaultControlMode, EndToEndArmTrajectoryMessageTest.findControllerState(chest.getName(), scs));
       assertNumberOfWaypoints(1, scs, chest);
 
@@ -1293,7 +1301,11 @@ public void testStopAllTrajectory() throws Exception
 
       Quaternion desiredOrientationAfterStop = findControllerDesiredOrientation(scs, chest);
 
-      RigidBodyControlMode defaultControlMode = getRobotModel().getWalkingControllerParameters().getDefaultControlModeForRigidBody(chest.getName());
+      RigidBodyControlMode defaultControlMode = getRobotModel().getWalkingControllerParameters().getDefaultControlModesForRigidBodies().get(chest.getName());
+      if (defaultControlMode == null)
+      {
+         defaultControlMode = RigidBodyControlMode.JOINTSPACE;
+      }
       assertEquals(defaultControlMode, EndToEndArmTrajectoryMessageTest.findControllerState(chest.getName(), scs));
       assertNumberOfWaypoints(1, scs, chest);
 

Valkyrie/src/us/ihmc/valkyrie/parameters/ValkyrieWalkingControllerParameters.java

@@ -431,16 +431,11 @@ private PID3DGains createHandPositionControlGains()
 
    /** {@inheritDoc} */
    @Override
-   public RigidBodyControlMode getDefaultControlModeForRigidBody(String bodyName)
+   public Map<String, RigidBodyControlMode> getDefaultControlModesForRigidBodies()
    {
-      if (bodyName.equals(jointMap.getChestName()))
-      {
-         return RigidBodyControlMode.TASKSPACE;
-      }
-      else
-      {
-         return RigidBodyControlMode.JOINTSPACE;
-      }
+      Map<String, RigidBodyControlMode> defaultControlModes = new HashMap<>();
+      defaultControlModes.put(jointMap.getChestName(), RigidBodyControlMode.TASKSPACE);
+      return defaultControlModes;
    }
 
    /** {@inheritDoc} */