Feature/onr demo

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java

@@ -64,6 +64,7 @@ public class RigidBodyControlManager implements SCS2YoGraphicHolder
    private final RigidBodyLoadBearingControlState loadBearingControlState;
    private final RigidBodyExternalWrenchManager externalWrenchManager;
 
+   private final RigidBodyJointControlHelper jointControlHelper;
    private final double[] initialJointPositions;
    private final FramePose3D homePose;
 
@@ -107,7 +108,7 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl,
 
       initialJointPositions = new double[jointsToControl.length];
 
-      RigidBodyJointControlHelper jointControlHelper = new RigidBodyJointControlHelper(bodyName, jointsToControl, yoTime, controlDT, enableFunctionGenerators, parentRegistry);
+      jointControlHelper = new RigidBodyJointControlHelper(bodyName, jointsToControl, yoTime, controlDT, enableFunctionGenerators, parentRegistry);
 
       jointspaceControlState = new RigidBodyJointspaceControlState(bodyName, jointsToControl, homeConfiguration, yoTime, jointControlHelper, registry);
 
@@ -605,16 +606,14 @@ public void resetJointIntegrators()
 
    private void computeDesiredJointPositions(double[] desiredJointPositionsToPack)
    {
-      if (stateMachine.getCurrentStateKey() == jointspaceControlState.getControlMode())
-      {
-         for (int i = 0; i < jointsToControl.length; i++)
-            desiredJointPositionsToPack[i] = jointspaceControlState.getJointDesiredPosition(i);
-      }
-      else if (loadBearingControlState != null && stateMachine.getCurrentStateKey() == loadBearingControlState.getControlMode()
-               && loadBearingControlState.isJointspaceControlActive())
+      boolean isInJointspaceMode = stateMachine.getCurrentStateKey() == jointspaceControlState.getControlMode();
+      boolean isInTaskspaceHybridMode = stateMachine.getCurrentStateKey() == taskspaceControlState.getControlMode() && taskspaceControlState.isHybridModeActive();
+      boolean isInLoadbearingHybridMode = loadBearingControlState != null && stateMachine.getCurrentStateKey() == loadBearingControlState.getControlMode() && loadBearingControlState.isJointspaceControlActive();
+
+      if (isInJointspaceMode || isInTaskspaceHybridMode || isInLoadbearingHybridMode)
       {
          for (int i = 0; i < jointsToControl.length; i++)
-            desiredJointPositionsToPack[i] = loadBearingControlState.getJointDesiredPosition(i);
+            desiredJointPositionsToPack[i] = jointControlHelper.getJointDesiredPosition(i);
       }
       else
       {

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyJointControlHelper.java

@@ -3,6 +3,7 @@
 import us.ihmc.commonWalkingControlModules.controlModules.ControllerCommandValidationTools;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.JointspaceFeedbackControlCommand;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.OneDoFJointFeedbackControlCommand;
+import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.JointAccelerationIntegrationCommand;
 import us.ihmc.commons.Conversions;
 import us.ihmc.commons.lists.RecyclingArrayDeque;
 import us.ihmc.communication.packets.ExecutionMode;
@@ -17,6 +18,10 @@
 import us.ihmc.robotics.math.functionGenerator.YoFunctionGeneratorNew;
 import us.ihmc.robotics.math.trajectories.generators.MultipleWaypointsTrajectoryGenerator;
 import us.ihmc.robotics.math.trajectories.trajectorypoints.OneDoFTrajectoryPoint;
+import us.ihmc.sensorProcessing.outputData.JointDesiredOutput;
+import us.ihmc.sensorProcessing.outputData.JointDesiredOutputList;
+import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;
+import us.ihmc.yoVariables.parameters.BooleanParameter;
 import us.ihmc.yoVariables.providers.DoubleProvider;
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoBoolean;
@@ -77,9 +82,12 @@ public class RigidBodyJointControlHelper
 
    private final OneDoFTrajectoryPoint lastPointAdded = new OneDoFTrajectoryPoint();
    private final JointspaceFeedbackControlCommand feedbackControlCommand = new JointspaceFeedbackControlCommand();
+   private final JointDesiredOutputList jointDesiredOutputList;
+   private final JointAccelerationIntegrationCommand accelerationIntegrationCommand = new JointAccelerationIntegrationCommand();
 
    private final OneDoFJointBasics[] joints;
    private final int numberOfJoints;
+   private final BooleanParameter[] bypassAccelerationIntegration;
 
    private final DoubleProvider time;
 
@@ -88,6 +96,7 @@ public RigidBodyJointControlHelper(String bodyName, OneDoFJointBasics[] jointsTo
       warningPrefix = shortName + " for " + bodyName + ": ";
       registry = new YoRegistry(bodyName + shortName);
       trajectoryDone = new YoBoolean(shortName + "Done", registry);
+      this.bypassAccelerationIntegration = new BooleanParameter[jointsToControl.length];
 
       this.time = time;
       this.joints = jointsToControl;
@@ -98,6 +107,7 @@ public RigidBodyJointControlHelper(String bodyName, OneDoFJointBasics[] jointsTo
       hasGains = new YoBoolean(prefix + "HasGains", registry);
       usingWeightFromMessage = new YoBoolean(prefix + "UsingWeightFromMessage", registry);
       functionGeneratorErrorCalculator = enableFunctionGenerators ? new FunctionGeneratorErrorCalculator(bodyName, controlDT, registry) : null;
+      jointDesiredOutputList = new JointDesiredOutputList(jointsToControl);
 
       for (int jointIdx = 0; jointIdx < jointsToControl.length; jointIdx++)
       {
@@ -126,6 +136,8 @@ public RigidBodyJointControlHelper(String bodyName, OneDoFJointBasics[] jointsTo
             int jointIdxFinal = jointIdx;
             functionGeneratorErrorCalculator.addTrajectorySignal(functionGenerators.get(jointIdx), () -> getJointDesiredPosition(jointIdxFinal), jointsToControl[jointIdx]);
          }
+
+         bypassAccelerationIntegration[jointIdx] = new BooleanParameter(jointsToControl[jointIdx].getName() + "BypassAccelerationIntegration", registry);
       }
 
       streamTimestampOffset = new YoDouble(prefix + "StreamTimestampOffset", registry);
@@ -484,6 +496,38 @@ public boolean handleTrajectoryCommand(JointspaceTrajectoryCommand command, doub
       return true;
    }
 
+   public JointDesiredOutputListReadOnly getJointDesiredData()
+   {
+      boolean bypassAccelerationIntegration = false;
+      jointDesiredOutputList.clear();
+
+      for (int jointIdx = 0; jointIdx < jointDesiredOutputList.getNumberOfJointsWithDesiredOutput(); jointIdx++)
+      {
+         if (this.bypassAccelerationIntegration[jointIdx].getValue())
+         {
+            bypassAccelerationIntegration = true;
+            JointDesiredOutput lowLevelJointData = jointDesiredOutputList.getJointDesiredOutput(jointIdx);
+            lowLevelJointData.setDesiredPosition(getJointDesiredPosition(jointIdx));
+            lowLevelJointData.setDesiredVelocity(getJointDesiredVelocity(jointIdx));
+         }
+      }
+
+      return bypassAccelerationIntegration ? jointDesiredOutputList : null;
+   }
+
+   public JointAccelerationIntegrationCommand getAccelerationIntegrationCommand()
+   {
+      accelerationIntegrationCommand.clear();
+
+      for (int jointIdx = 0; jointIdx < jointDesiredOutputList.getNumberOfJointsWithDesiredOutput(); jointIdx++)
+      {
+         accelerationIntegrationCommand.addJointToComputeDesiredPositionFor(joints[jointIdx])
+                                       .setDisableAccelerationIntegration(bypassAccelerationIntegration[jointIdx].getValue());
+      }
+
+      return accelerationIntegrationCommand;
+   }
+
    public void queuePointsAtTimeWithZeroVelocity(double time, double[] jointPositions)
    {
       for (int jointIdx = 0; jointIdx < numberOfJoints; jointIdx++)

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyJointspaceControlState.java

@@ -60,10 +60,6 @@ public class RigidBodyJointspaceControlState extends RigidBodyControlState
 
    private final int numberOfJoints;
    private final double[] jointsHomeConfiguration;
-   private final JointDesiredOutputList jointDesiredOutputList;
-
-   private final BooleanParameter[] bypassAccelerationIntegration;
-   private final JointAccelerationIntegrationCommand accelerationIntegrationCommand = new JointAccelerationIntegrationCommand();
 
    public RigidBodyJointspaceControlState(String bodyName,
                                           OneDoFJointBasics[] jointsToControl,
@@ -75,14 +71,7 @@ public RigidBodyJointspaceControlState(String bodyName,
       super(RigidBodyControlMode.JOINTSPACE, bodyName, yoTime, parentRegistry);
       this.jointControlHelper = jointControlHelper;
       this.jointsToControl = jointsToControl;
-      this.bypassAccelerationIntegration = new BooleanParameter[jointsToControl.length];
-
-      for (int i = 0; i < jointsToControl.length; i++)
-      {
-         bypassAccelerationIntegration[i] = new BooleanParameter(jointsToControl[i].getName() + "BypassAccelerationIntegration", parentRegistry);
-      }
 
-      jointDesiredOutputList = new JointDesiredOutputList(jointsToControl);
 
       numberOfJoints = jointsToControl.length;
       jointsHomeConfiguration = new double[numberOfJoints];
@@ -112,20 +101,7 @@ public void setDefaultWeight(DoubleProvider weight)
    @Override
    public JointDesiredOutputListReadOnly getJointDesiredData()
    {
-      boolean bypassAccelerationIntegration = false;
-
-      for (int jointIdx = 0; jointIdx < jointDesiredOutputList.getNumberOfJointsWithDesiredOutput(); jointIdx++)
-      {
-         if (this.bypassAccelerationIntegration[jointIdx].getValue())
-         {
-            bypassAccelerationIntegration = true;
-            JointDesiredOutput lowLevelJointData = jointDesiredOutputList.getJointDesiredOutput(jointIdx);
-            lowLevelJointData.setDesiredPosition(getJointDesiredPosition(jointIdx));
-            lowLevelJointData.setDesiredVelocity(getJointDesiredVelocity(jointIdx));
-         }
-      }
-
-      return bypassAccelerationIntegration ? jointDesiredOutputList : null;
+      return jointControlHelper.getJointDesiredData();
    }
 
    public void setGains(Map<String, PIDGainsReadOnly> jointspaceHighLevelGains)
@@ -255,15 +231,7 @@ public void onExit(double timeInState)
    @Override
    public InverseDynamicsCommand<?> getInverseDynamicsCommand()
    {
-      accelerationIntegrationCommand.clear();
-
-      for (int jointIdx = 0; jointIdx < jointDesiredOutputList.getNumberOfJointsWithDesiredOutput(); jointIdx++)
-      {
-         accelerationIntegrationCommand.addJointToComputeDesiredPositionFor(jointsToControl[jointIdx])
-                                       .setDisableAccelerationIntegration(bypassAccelerationIntegration[jointIdx].getValue());
-      }
-
-      return accelerationIntegrationCommand;
+      return jointControlHelper.getAccelerationIntegrationCommand();
    }
 
    @Override

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java

@@ -4,6 +4,7 @@
 import us.ihmc.commonWalkingControlModules.controlModules.TaskspaceTrajectoryStatusMessageHelper;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.FeedbackControlCommand;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.FeedbackControlCommandList;
+import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.InverseDynamicsCommand;
 import us.ihmc.communication.packets.ExecutionMode;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly;
@@ -15,6 +16,7 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
+import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;
 import us.ihmc.yoVariables.parameters.BooleanParameter;
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoBoolean;
@@ -212,6 +214,32 @@ public FeedbackControlCommand<?> getFeedbackControlCommand()
       return orientationHelper.getFeedbackControlCommand();
    }
 
+   @Override
+   public JointDesiredOutputListReadOnly getJointDesiredData()
+   {
+      if (hybridModeActive.getValue())
+      {
+         return jointControlHelper.getJointDesiredData();
+      }
+      else
+      {
+         return null;
+      }
+   }
+
+   @Override
+   public InverseDynamicsCommand<?> getInverseDynamicsCommand()
+   {
+      if (hybridModeActive.getValue())
+      {
+         return jointControlHelper.getAccelerationIntegrationCommand();
+      }
+      else
+      {
+         return null;
+      }
+   }
+
    public FrameQuaternionReadOnly getDesiredOrientation()
    {
       return orientationHelper.getFeedbackControlCommand().getReferenceOrientation();

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java

@@ -7,6 +7,7 @@
 import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.OrientationFeedbackControlCommand;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.PointFeedbackControlCommand;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.SpatialFeedbackControlCommand;
+import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.InverseDynamicsCommand;
 import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.SpatialAccelerationCommand;
 import us.ihmc.communication.packets.ExecutionMode;
 import us.ihmc.euclid.referenceFrame.FrameQuaternion;
@@ -23,6 +24,7 @@
 import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition;
+import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;
 import us.ihmc.yoVariables.parameters.BooleanParameter;
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoBoolean;
@@ -363,6 +365,32 @@ public FeedbackControlCommand<?> createFeedbackControlTemplate()
       return feedbackControlCommandList;
    }
 
+   @Override
+   public JointDesiredOutputListReadOnly getJointDesiredData()
+   {
+      if (hybridModeActive.getValue())
+      {
+         return jointControlHelper.getJointDesiredData();
+      }
+      else
+      {
+         return null;
+      }
+   }
+
+   @Override
+   public InverseDynamicsCommand<?> getInverseDynamicsCommand()
+   {
+      if (hybridModeActive.getValue())
+      {
+         return jointControlHelper.getAccelerationIntegrationCommand();
+      }
+      else
+      {
+         return null;
+      }
+   }
+
    @Override
    public double getLastTrajectoryPointTime()
    {

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java

@@ -73,6 +73,7 @@
 import us.ihmc.robotics.stateMachine.factories.StateMachineFactory;
 import us.ihmc.robotics.time.ExecutionTimer;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
+import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;
 import us.ihmc.yoVariables.parameters.DoubleParameter;
 import us.ihmc.yoVariables.providers.DoubleProvider;
 import us.ihmc.yoVariables.registry.YoRegistry;
@@ -977,9 +978,10 @@ private void submitControllerCoreCommands(WalkingState currentState)
                controllerCoreCommand.addFeedbackControlCommand(bodyManager.getFeedbackControlCommand());
                controllerCoreCommand.addInverseDynamicsCommand(bodyManager.getInverseDynamicsCommand());
 
-               if (bodyManager.getJointDesiredData() != null)
+               JointDesiredOutputListReadOnly jointDesiredData = bodyManager.getJointDesiredData();
+               if (jointDesiredData != null)
                {
-                  controllerCoreCommand.completeLowLevelJointData(bodyManager.getJointDesiredData());
+                  controllerCoreCommand.completeLowLevelJointData(jointDesiredData);
                }
             }
          }

ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTStatusFootstepList.java

@@ -0,0 +1,59 @@
+package us.ihmc.communication.crdt;
+
+import behavior_msgs.msg.dds.FootstepPlanActionFootstepDefinitionMessage;
+import us.ihmc.commons.lists.RecyclingArrayList;
+import us.ihmc.communication.ros2.ROS2ActorDesignation;
+import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly;
+import us.ihmc.idl.IDLSequence;
+import us.ihmc.robotics.robotSide.RobotSide;
+
+/**
+ * Represents a list of footsteps that should only be modified by one actor type
+ * and read-only for the others. The internal writeable instance is kept protected
+ * from unchecked modifications.
+ */
+public class CRDTStatusFootstepList extends CRDTStatusMutableField<RecyclingArrayList<FootstepPlanActionFootstepDefinitionMessage>>
+{
+   public CRDTStatusFootstepList(ROS2ActorDesignation sideThatCanModify, CRDTInfo crdtInfo)
+   {
+      super(sideThatCanModify, crdtInfo, () -> new RecyclingArrayList<>(FootstepPlanActionFootstepDefinitionMessage::new));
+   }
+
+   public Pose3DReadOnly getPoseReadOnly(int index)
+   {
+      return getValueInternal().get(index).getSolePose();
+   }
+
+   public RobotSide getSide(int index)
+   {
+      return RobotSide.fromByte(getValueInternal().get(index).getRobotSide());
+   }
+
+   public int getSize()
+   {
+      return getValueInternal().size();
+   }
+
+   public void toMessage(IDLSequence.Object<FootstepPlanActionFootstepDefinitionMessage> message)
+   {
+      message.clear();
+
+      for (FootstepPlanActionFootstepDefinitionMessage footstep : getValueInternal())
+      {
+         message.add().set(footstep);
+      }
+   }
+
+   public void fromMessage(IDLSequence.Object<FootstepPlanActionFootstepDefinitionMessage> message)
+   {
+      if (isModificationDisallowed()) // Ignore updates if we are the only side that can modify
+      {
+         getValueInternal().clear();
+
+         for (FootstepPlanActionFootstepDefinitionMessage footstep : message)
+         {
+            getValueInternal().add().set(footstep);
+         }
+      }
+   }
+}