Feature/yo graphic robot (#175)

Adds a yoGraphic robot that allows to visualize the state of a robot (given a URDF model) and the yoVariables that store the state.
ihmcrobotics · Apr 8, 2024 · e83424d · e83424d
1 parent 61516ec
commit e83424d
Show file tree

Hide file tree

Showing 80 changed files with 3,346 additions and 707 deletions.
diff --git a/scs2-definition/src/main/java/us/ihmc/scs2/definition/DefinitionIOTools.java b/scs2-definition/src/main/java/us/ihmc/scs2/definition/DefinitionIOTools.java
diff --git a/...finition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoOrientation3DDefinition.java b/...finition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoOrientation3DDefinition.java
@@ -9,12 +9,14 @@
  * variable name/fullname, or is a constant value, which case it is set to the string representation
  * of the value.
  * </p>
- * 
+ *
  * @author Sylvain Bertrand
  */
 @XmlSeeAlso({YoQuaternionDefinition.class, YoYawPitchRollDefinition.class})
 public abstract class YoOrientation3DDefinition extends YoCompositeDefinition
 {
+   public abstract YoOrientation3DDefinition copy();
+
    /**
     * Parses the given {@code value} into a {@link YoOrientation3DDefinition}. The given {@code String}
     * representation is expected to have been generated using {@link #toString()}. If the format

diff --git a/...-definition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoQuaternionDefinition.java b/...-definition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoQuaternionDefinition.java
@@ -269,6 +269,12 @@ public String[] getComponentValues()
       return new String[] {x, y, z, s};
    }
 
+   @Override
+   public YoQuaternionDefinition copy()
+   {
+      return new YoQuaternionDefinition(x, y, z, s, referenceFrame);
+   }
+
    /**
     * Parses the given {@code value} into a {@link YoQuaternionDefinition}. The given {@code String}
     * representation is expected to have been generated using {@link #toString()}. If the format

diff --git a/scs2-definition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoTuple2DDefinition.java b/scs2-definition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoTuple2DDefinition.java
@@ -1,11 +1,11 @@
 package us.ihmc.scs2.definition.yoComposite;
 
-import javax.xml.bind.annotation.XmlElement;
-import javax.xml.bind.annotation.XmlRootElement;
-
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory;
 
+import javax.xml.bind.annotation.XmlElement;
+import javax.xml.bind.annotation.XmlRootElement;
+
 /**
  * A {@link YoTuple2DDefinition} represents a template for creating a tuple 2D which components can
  * be backed by {@code YoVariable}s.
@@ -166,6 +166,16 @@ public String[] getComponentValues()
       return new String[] {x, y};
    }
 
+   /**
+    * Creates a copy of this tuple 2D definition.
+    *
+    * @return the copy.
+    */
+   public YoTuple2DDefinition copy()
+   {
+      return new YoTuple2DDefinition(x, y, referenceFrame);
+   }
+
    /**
     * Parses the given {@code value} into a {@link YoTuple2DDefinition}. The given {@code String}
     * representation is expected to have been generated using {@link #toString()}. If the format

diff --git a/scs2-definition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoTuple3DDefinition.java b/scs2-definition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoTuple3DDefinition.java
@@ -1,11 +1,11 @@
 package us.ihmc.scs2.definition.yoComposite;
 
-import javax.xml.bind.annotation.XmlElement;
-import javax.xml.bind.annotation.XmlRootElement;
-
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory;
 
+import javax.xml.bind.annotation.XmlElement;
+import javax.xml.bind.annotation.XmlRootElement;
+
 /**
  * A {@link YoTuple3DDefinition} represents a template for creating a tuple 3D which components can
  * be backed by {@code YoVariable}s.
@@ -21,7 +21,7 @@
  * See {@link YoGraphicDefinitionFactory} for factory methods to facilitate the creation of a
  * {@code YoTuple3DDefinition}.
  * </p>
- * 
+ *
  * @author Sylvain Bertrand
  */
 @XmlRootElement(name = "YoTuple3D")
@@ -198,6 +198,11 @@ public String[] getComponentValues()
       return new String[] {x, y, z};
    }
 
+   public YoTuple3DDefinition copy()
+   {
+      return new YoTuple3DDefinition(x, y, z, referenceFrame);
+   }
+
    /**
     * Parses the given {@code value} into a {@link YoTuple3DDefinition}. The given {@code String}
     * representation is expected to have been generated using {@link #toString()}. If the format

diff --git a/...efinition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoYawPitchRollDefinition.java b/...efinition/src/main/java/us/ihmc/scs2/definition/yoComposite/YoYawPitchRollDefinition.java
@@ -1,11 +1,11 @@
 package us.ihmc.scs2.definition.yoComposite;
 
-import javax.xml.bind.annotation.XmlElement;
-import javax.xml.bind.annotation.XmlRootElement;
-
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory;
 
+import javax.xml.bind.annotation.XmlElement;
+import javax.xml.bind.annotation.XmlRootElement;
+
 /**
  * A {@link YoYawPitchRollDefinition} represents a template for creating a yaw-pitch-roll which
  * components can be backed by {@code YoVariable}s.
@@ -198,6 +198,12 @@ public String[] getComponentValues()
       return new String[] {yaw, pitch, roll};
    }
 
+   @Override
+   public YoYawPitchRollDefinition copy()
+   {
+      return new YoYawPitchRollDefinition(yaw, pitch, roll, referenceFrame);
+   }
+
    /**
     * Parses the given {@code value} into a {@link YoYawPitchRollDefinition}. The given {@code String}
     * representation is expected to have been generated using {@link #toString()}. If the format

diff --git a/scs2-definition/src/main/java/us/ihmc/scs2/definition/yoGraphic/YoGraphic2DDefinition.java b/scs2-definition/src/main/java/us/ihmc/scs2/definition/yoGraphic/YoGraphic2DDefinition.java
@@ -1,12 +1,11 @@
 package us.ihmc.scs2.definition.yoGraphic;
 
-import java.util.Objects;
-
-import javax.xml.bind.annotation.XmlElement;
-
 import us.ihmc.scs2.definition.visual.ColorDefinition;
 import us.ihmc.scs2.definition.visual.PaintDefinition;
 
+import javax.xml.bind.annotation.XmlElement;
+import java.util.Objects;
+
 /**
  * Base class representing a template used to create a single 2D yoGraphic.
  * <p>
@@ -22,7 +21,7 @@
  * See {@link YoGraphicDefinitionFactory} for factory methods simplifying the creation of yoGraphic
  * definitions.
  * </p>
- * 
+ *
  * @author Sylvain Bertrand
  */
 public abstract class YoGraphic2DDefinition extends YoGraphicDefinition
@@ -36,6 +35,26 @@ public abstract class YoGraphic2DDefinition extends YoGraphicDefinition
 
    public YoGraphic2DDefinition()
    {
+      super();
+   }
+
+   /**
+    * Creates a new 2D yoGraphic definition.
+    *
+    * @param other the other definition to copy. Not modified.
+    */
+   public YoGraphic2DDefinition(YoGraphic2DDefinition other)
+   {
+      super(other);
+      fillColor = other.fillColor == null ? null : other.fillColor.copy();
+      strokeColor = other.strokeColor == null ? null : other.strokeColor.copy();
+      strokeWidth = other.strokeWidth;
+   }
+
+   @Override
+   protected void registerFields()
+   {
+      super.registerFields();
       registerPaintField("fillColor", this::getFillColor, this::setFillColor);
       registerPaintField("strokeColor", this::getStrokeColor, this::setStrokeColor);
       registerStringField("strokeWidth", this::getStrokeWidth, this::setStrokeWidth);
@@ -47,7 +66,7 @@ public YoGraphic2DDefinition()
     * See {@link ColorDefinition} for setting the color to a constant value, or other implementations
     * of {@link PaintDefinition} notably for colors backed by {@code YoVariable}s.
     * </p>
-    * 
+    *
     * @param fillColor the color to fill the shape with.
     */
    @XmlElement(name = "fillColorNew")
@@ -62,7 +81,7 @@ public final void setFillColor(PaintDefinition fillColor)
     * See {@link ColorDefinition} for setting the color to a constant value, or other implementations
     * of {@link PaintDefinition} notably for colors backed by {@code YoVariable}s.
     * </p>
-    * 
+    *
     * @param strokeColor the stroke color.
     */
    @XmlElement(name = "strokeColorNew")
@@ -73,7 +92,7 @@ public final void setStrokeColor(PaintDefinition strokeColor)
 
    /**
     * Sets a constant value the width of the stroke.
-    * 
+    *
     * @param strokeWidth the stroke width.
     */
    public final void setStrokeWidth(double strokeWidth)
@@ -84,7 +103,7 @@ public final void setStrokeWidth(double strokeWidth)
    /**
     * Sets the width of the stroke, can be backed by a {@code YoVariable} by providing the
     * name/fullname, or a constant.
-    * 
+    *
     * @param strokeWidth the stroke width.
     */
    @XmlElement

diff --git a/scs2-definition/src/main/java/us/ihmc/scs2/definition/yoGraphic/YoGraphic3DDefinition.java b/scs2-definition/src/main/java/us/ihmc/scs2/definition/yoGraphic/YoGraphic3DDefinition.java
@@ -1,12 +1,11 @@
 package us.ihmc.scs2.definition.yoGraphic;
 
-import java.util.Objects;
-
-import javax.xml.bind.annotation.XmlElement;
-
 import us.ihmc.scs2.definition.visual.ColorDefinition;
 import us.ihmc.scs2.definition.visual.PaintDefinition;
 
+import javax.xml.bind.annotation.XmlElement;
+import java.util.Objects;
+
 /**
  * Base class representing a template used to create a single 3D yoGraphic.
  * <p>
@@ -22,17 +21,32 @@
  * See {@link YoGraphicDefinitionFactory} for factory methods simplifying the creation of yoGraphic
  * definitions.
  * </p>
- * 
+ *
  * @author Sylvain Bertrand
  */
 public abstract class YoGraphic3DDefinition extends YoGraphicDefinition
 {
    /** The diffuse color of the shape. */
    protected PaintDefinition color;
+   protected String drawMode = "FILL";
 
    public YoGraphic3DDefinition()
    {
+   }
+
+   public YoGraphic3DDefinition(YoGraphic3DDefinition other)
+   {
+      super(other);
+      color = other.color == null ? null : other.color.copy();
+      drawMode = other.drawMode;
+   }
+
+   @Override
+   protected void registerFields()
+   {
+      super.registerFields();
       registerPaintField("color", this::getColor, this::setColor);
+      registerStringField("drawMode", this::getDrawMode, this::setDrawMode);
    }
 
    /**
@@ -41,7 +55,7 @@ public YoGraphic3DDefinition()
     * See {@link ColorDefinition} for setting the color to a constant value, or other implementations
     * of {@link PaintDefinition} notably for colors backed by {@code YoVariable}s.
     * </p>
-    * 
+    *
     * @param color
     */
    @XmlElement(name = "colorNew")
@@ -50,11 +64,27 @@ public final void setColor(PaintDefinition color)
       this.color = color;
    }
 
+   /**
+    * Sets the draw mode of the shape.
+    *
+    * @param drawMode the draw mode of the shape. The following values are supported: "FILL", "LINE".
+    */
+   @XmlElement
+   public final void setDrawMode(String drawMode)
+   {
+      this.drawMode = drawMode;
+   }
+
    public final PaintDefinition getColor()
    {
       return color;
    }
 
+   public String getDrawMode()
+   {
+      return drawMode;
+   }
+
    @Override
    public boolean equals(Object object)
    {
@@ -70,6 +100,8 @@ else if (object instanceof YoGraphic3DDefinition other)
       {
          if (!Objects.equals(color, other.color))
             return false;
+         if (!Objects.equals(drawMode, other.drawMode))
+            return false;
          return true;
       }
       else