Fix Ability Hand slider issue

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

@@ -0,0 +1,78 @@
+package us.ihmc.communication.crdt;
+
+import us.ihmc.idl.IDLSequence;
+
+/**
+ * Represents an enum array that can be modified by both the
+ * robot and the operator.
+ * <p>
+ * Warning: With this type, the data should not be continuously modified
+ * tick after tick, as that will mean the value is essentially never
+ * synced properly to the other side.
+ */
+public class CRDTBidirectionalEnumArray<T extends Enum<T>> extends CRDTBidirectionalMutableField<T[]>
+{
+   public CRDTBidirectionalEnumArray(LatestTimestampModifiable latestTimestampModifiable, T[] initialArray)
+   {
+      super(latestTimestampModifiable, initialArray);
+   }
+
+   public T getValueReadOnly(int index)
+   {
+      return getValueInternal()[index];
+   }
+
+   public void setValue(int index, T value)
+   {
+      if (getValueReadOnly(index) != value)
+         getValueAndModify()[index] = value;
+   }
+
+   public int getLength()
+   {
+      return getValueInternal().length;
+   }
+
+   public void toMessage(IDLSequence.Byte messageArray)
+   {
+      T[] values = getValueInternal();
+      messageArray.resetQuick();
+      for (int i = 0; i < Math.min(getLength(), messageArray.size()); i++)
+      {
+         messageArray.add(values[i] == null ? -1 : (byte) values[i].ordinal());
+      }
+   }
+
+   public void toMessage(byte[] messageArray)
+   {
+      T[] values = getValueInternal();
+      for (int i = 0; i < Math.min(getLength(), messageArray.length); i++)
+      {
+         messageArray[i] = values[i] == null ? -1 : (byte) values[i].ordinal();
+      }
+   }
+
+   public void fromMessage(IDLSequence.Byte messageArray, T[] enumValues)
+   {
+      if (isModificationIncoming())
+      {
+         T[] values = getValueInternal();
+         for (int i = 0; i < Math.min(getLength(), messageArray.size()); i++)
+         {
+            values[i] = messageArray.get(i) == -1 ? null : enumValues[messageArray.get(i)];
+         }
+      }
+   }
+
+   public void fromMessage(byte[] messageArray, T[] enumValues)
+   {
+      if (isModificationIncoming())
+      {
+         T[] values = getValueInternal();
+         for (int i = 0; i < Math.min(getLength(), messageArray.length); i++)
+         {
+            values[i] = messageArray[i] == -1 ? null : enumValues[messageArray[i]];
+         }
+      }
+   }
+}

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

@@ -0,0 +1,96 @@
+package us.ihmc.communication.crdt;
+
+import us.ihmc.idl.IDLSequence;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Represents an enum List that can be modified by both the
+ * robot and the operator.
+ * <p>
+ * Warning: With this type, the data should not be continuously modified
+ * tick after tick, as that will mean the value is essentially never
+ * synced properly to the other side.
+ */
+public class CRDTBidirectionalEnumList<T extends Enum<T>> extends CRDTBidirectionalMutableField<List<T>>
+{
+   public CRDTBidirectionalEnumList(LatestTimestampModifiable latestTimestampModifiable)
+   {
+      super(latestTimestampModifiable, new ArrayList<>());
+   }
+
+   public T getValueReadOnly(int index)
+   {
+      return getValueInternal().get(index);
+   }
+
+   public void setValue(int index, T value)
+   {
+      if (getValueReadOnly(index) != value)
+         getValueAndModify().set(index, value);
+   }
+
+   public void add(T value)
+   {
+      getValueAndModify().add(value);
+   }
+
+   public void remove(int index)
+   {
+      getValueAndModify().remove(index);
+   }
+
+   public void clear()
+   {
+      getValueAndModify().clear();
+   }
+
+   public int getSize()
+   {
+      return getValueInternal().size();
+   }
+
+   public void toMessage(IDLSequence.Byte messageArray)
+   {
+      List<T> values = getValueInternal();
+      messageArray.resetQuick();
+      for (int i = 0; i < getSize(); i++)
+      {
+         messageArray.add(values.get(i) == null ? -1 : (byte) values.get(i).ordinal());
+      }
+   }
+
+   public void toMessage(byte[] messageArray)
+   {
+      List<T> values = getValueInternal();
+      for (int i = 0; i < Math.min(getSize(), messageArray.length); i++)
+      {
+         messageArray[i] = values.get(i) == null ? -1 : (byte) values.get(i).ordinal();
+      }
+   }
+
+   public void fromMessage(IDLSequence.Byte messageArray, T[] enumValues)
+   {
+      if (isModificationIncoming())
+      {
+         getValueInternal().clear();
+         for (int i = 0; i < messageArray.size(); i++)
+         {
+            getValueInternal().add(messageArray.get(i) == -1 ? null : enumValues[messageArray.get(i)]);
+         }
+      }
+   }
+
+   public void fromMessage(byte[] messageArray, T[] enumValues)
+   {
+      if (isModificationIncoming())
+      {
+         getValueInternal().clear();
+         for (int i = 0; i < messageArray.length; i++)
+         {
+            getValueInternal().add(messageArray[i] == -1 ? null : enumValues[messageArray[i]]);
+         }
+      }
+   }
+}

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

@@ -0,0 +1,100 @@
+package us.ihmc.communication.crdt;
+
+import gnu.trove.list.array.TIntArrayList;
+import us.ihmc.idl.IDLSequence.Byte;
+import us.ihmc.idl.IDLSequence.Integer;
+
+/**
+ * Represents an integer List that can be modified by both the
+ * robot and the operator.
+ * <p>
+ * Warning: With this type, the data should not be continuously modified
+ * tick after tick, as that will mean the value is essentially never
+ * synced properly to the other side.
+ */
+public class CRDTBidirectionalIntegerList extends CRDTBidirectionalMutableField<TIntArrayList>
+{
+   public CRDTBidirectionalIntegerList(LatestTimestampModifiable latestTimestampModifiable)
+   {
+      super(latestTimestampModifiable, new TIntArrayList());
+   }
+
+   public int getValueReadOnly(int index)
+   {
+      return getValueInternal().get(index);
+   }
+
+   public void setValue(int index, int value)
+   {
+      if (getValueReadOnly(index) != value)
+         getValueAndModify().set(index, value);
+   }
+
+   public void add(int value)
+   {
+      getValueAndModify().add(value);
+   }
+
+   public void remove(int index)
+   {
+      getValueAndModify().removeAt(index);
+   }
+
+   public void clear()
+   {
+      getValueAndModify().clear();
+   }
+
+   public int getSize()
+   {
+      return getValueInternal().size();
+   }
+
+   public void toMessage(int[] messageArray)
+   {
+      getValueInternal().toArray(messageArray, 0, Math.min(getSize(), messageArray.length));
+   }
+
+   public void toMessage(Integer message)
+   {
+      message.clear();
+      for (int i = 0; i < getSize(); ++i)
+         message.add(getValueReadOnly(i));
+   }
+
+   public void toMessage(Byte message)
+   {
+      message.resetQuick();
+      for (int i = 0; i < getSize(); ++i)
+         message.add((byte) getValueReadOnly(i));
+   }
+
+   public void fromMessage(int[] messageArray)
+   {
+      if (isModificationIncoming())
+      {
+         getValueInternal().clear();
+         getValueInternal().add(messageArray);
+      }
+   }
+
+   public void fromMessage(Integer message)
+   {
+      if (isModificationIncoming())
+      {
+         getValueInternal().clear();
+         for (int i = 0; i < message.size(); ++i)
+            getValueInternal().add(message.get(i));
+      }
+   }
+
+   public void fromMessage(Byte message)
+   {
+      if (isModificationIncoming())
+      {
+         getValueInternal().clear();
+         for (int i = 0; i < message.size(); ++i)
+            getValueInternal().add(message.get(i));
+      }
+   }
+}

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

@@ -0,0 +1,94 @@
+package us.ihmc.communication.crdt;
+
+import us.ihmc.idl.IDLSequence.StringBuilderHolder;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Represents a String List that can be modified by both the
+ * robot and the operator.
+ * <p>
+ * Warning: With this type, the data should not be continuously modified
+ * tick after tick, as that will mean the value is essentially never
+ * synced properly to the other side.
+ */
+public class CRDTBidirectionalStringList extends CRDTBidirectionalMutableField<List<String>>
+{
+   public CRDTBidirectionalStringList(LatestTimestampModifiable latestTimestampModifiable)
+   {
+      super(latestTimestampModifiable, new ArrayList<>());
+   }
+
+   public String getValueReadOnly(int index)
+   {
+      return getValueInternal().get(index);
+   }
+
+   public void setValue(int index, String value)
+   {
+      if (!getValueReadOnly(index).equals(value))
+         getValueAndModify().set(index, value);
+   }
+
+   public void add(String value)
+   {
+      getValueAndModify().add(value);
+   }
+
+   public void remove(int index)
+   {
+      getValueAndModify().remove(index);
+   }
+
+   public void clear()
+   {
+      getValueAndModify().clear();
+   }
+
+   public int getSize()
+   {
+      return getValueInternal().size();
+   }
+
+   public void toMessage(StringBuilder[] messageArray)
+   {
+      for (int i = 0; i < getSize() && i < messageArray.length; ++i)
+      {
+         messageArray[i] = new StringBuilder(getValueReadOnly(i));
+      }
+   }
+
+   public void toMessage(StringBuilderHolder message)
+   {
+      message.clear();
+      for (int i = 0; i < getSize(); ++i)
+      {
+         message.add(getValueReadOnly(i));
+      }
+   }
+
+   public void fromMessage(StringBuilder[] messageArray)
+   {
+      if (isModificationIncoming())
+      {
+         getValueInternal().clear();
+         for (int i = 0; i < messageArray.length; ++i)
+         {
+            getValueInternal().add(messageArray[i].toString());
+         }
+      }
+   }
+
+   public void fromMessage(StringBuilderHolder message)
+   {
+      if (isModificationIncoming())
+      {
+         getValueInternal().clear();
+         for (int i = 0; i < message.size(); ++i)
+         {
+            getValueInternal().add(message.getString(i));
+         }
+      }
+   }
+}

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

@@ -24,6 +24,18 @@ public RigidBodyTransformReadOnly getValueReadOnly()
       return getValueInternal();
    }
 
+   /** Prefer this method in the case you need to call it every tick, as it no-ops in the case the value is the same. */
+   public void setValue(RigidBodyTransformReadOnly value, double epsilon)
+   {
+      // rotation and translation must be checked separately to handle pose-transform comparison case
+      // translation must use epsilonEquals because it can be vector vs. point
+      if (!(getValueInternal().getRotation().geometricallyEquals(value.getRotation(), epsilon)
+         && getValueInternal().getTranslation().epsilonEquals(value.getTranslation(), epsilon)))
+      {
+         accessValue().set(value);
+      }
+   }
+
    public void toMessage(Pose3D poseMessage)
    {
       poseMessage.set(getValueReadOnly());