From 7e036c657cd19c883963c5605048af3e83c008b6 Mon Sep 17 00:00:00 2001 From: James Foster Date: Tue, 28 Jan 2025 15:23:21 -0600 Subject: [PATCH 1/2] Switch to ihmc-yovariables YoMatrix; deprecate SCS2 YoMatrix --- .../java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java | 4 ++++ .../contactPointBased/ContactPointBasedRobotPhysics.java | 4 ++-- .../physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java | 4 ++-- .../impulseBased/YoSingleContactImpulseCalculator.java | 2 +- .../scs2/simulation/screwTools/SimMultiBodySystemTools.java | 2 +- 5 files changed, 10 insertions(+), 6 deletions(-) diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java index 6545d44f9..c7d376f07 100644 --- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java +++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java @@ -10,6 +10,10 @@ import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoInteger; +/** + * @deprecated please use {@link us.ihmc.yoVariables.math.YoMatrix} + */ +@Deprecated public class YoMatrix implements DMatrix, ReshapeMatrix { // TODO: eventually consolidate YoMatrix implementations diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/contactPointBased/ContactPointBasedRobotPhysics.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/contactPointBased/ContactPointBasedRobotPhysics.java index 479d5460e..af7d460ed 100644 --- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/contactPointBased/ContactPointBasedRobotPhysics.java +++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/contactPointBased/ContactPointBasedRobotPhysics.java @@ -12,7 +12,7 @@ import us.ihmc.scs2.simulation.RobotJointWrenchCalculator; import us.ihmc.scs2.simulation.collision.Collidable; import us.ihmc.scs2.simulation.collision.FrameShapePosePredictor; -import us.ihmc.scs2.simulation.physicsEngine.YoMatrix; +import us.ihmc.yoVariables.math.YoMatrix; import us.ihmc.scs2.simulation.robot.RobotInterface; import us.ihmc.scs2.simulation.robot.RobotPhysicsOutput; import us.ihmc.scs2.simulation.robot.controller.RobotOneDoFJointDampingCalculator; @@ -192,6 +192,6 @@ public RobotPhysicsOutput getPhysicsOutput() private void sumJointTauContributions() { MultiBodySystemTools.extractJointsState(owner.getJointsToConsider(), JointStateType.EFFORT, jointsTau); - jointsTau.add(jointsTauLowLevelController); + jointsTau.addEquals(jointsTauLowLevelController); } } diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java index 440d69109..05fc9c8d1 100644 --- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java +++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/ImpulseBasedRobotPhysics.java @@ -16,7 +16,7 @@ import us.ihmc.scs2.simulation.RobotJointWrenchCalculator; import us.ihmc.scs2.simulation.collision.Collidable; import us.ihmc.scs2.simulation.collision.FrameShapePosePredictor; -import us.ihmc.scs2.simulation.physicsEngine.YoMatrix; +import us.ihmc.yoVariables.math.YoMatrix; import us.ihmc.scs2.simulation.robot.RobotInterface; import us.ihmc.scs2.simulation.robot.RobotPhysicsOutput; import us.ihmc.scs2.simulation.robot.controller.RobotOneDoFJointDampingCalculator; @@ -310,6 +310,6 @@ public RigidBodyTwistProvider getRigidBodyTwistChangeProvider() private void sumJointTauContributions() { MultiBodySystemTools.extractJointsState(owner.getJointsToConsider(), JointStateType.EFFORT, jointsTau); - jointsTau.add(jointsTauLowLevelController); + jointsTau.addEquals(jointsTauLowLevelController); } } diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/YoSingleContactImpulseCalculator.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/YoSingleContactImpulseCalculator.java index b5f3b1876..e75ec8a81 100644 --- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/YoSingleContactImpulseCalculator.java +++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/impulseBased/YoSingleContactImpulseCalculator.java @@ -18,7 +18,7 @@ import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameTwist; import us.ihmc.scs2.simulation.collision.CollisionResult; -import us.ihmc.scs2.simulation.physicsEngine.YoMatrix; +import us.ihmc.yoVariables.math.YoMatrix; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/screwTools/SimMultiBodySystemTools.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/screwTools/SimMultiBodySystemTools.java index 8fee4cd6f..5b8972446 100644 --- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/screwTools/SimMultiBodySystemTools.java +++ b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/screwTools/SimMultiBodySystemTools.java @@ -5,7 +5,7 @@ import us.ihmc.mecano.multiBodySystem.interfaces.*; import us.ihmc.mecano.multiBodySystem.iterators.SubtreeStreams; import us.ihmc.mecano.tools.MultiBodySystemTools; -import us.ihmc.scs2.simulation.physicsEngine.YoMatrix; +import us.ihmc.yoVariables.math.YoMatrix; import us.ihmc.scs2.simulation.robot.multiBodySystem.interfaces.*; import us.ihmc.yoVariables.registry.YoRegistry; From 44c2b85f732afe931f64c38f288b3c0646e06d44 Mon Sep 17 00:00:00 2001 From: James Foster Date: Thu, 30 Jan 2025 12:13:58 -0600 Subject: [PATCH 2/2] Delete SCS2 YoMatrix --- .../simulation/physicsEngine/YoMatrix.java | 275 ------------------ 1 file changed, 275 deletions(-) delete mode 100644 scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java diff --git a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java b/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java deleted file mode 100644 index c7d376f07..000000000 --- a/scs2-simulation/src/main/java/us/ihmc/scs2/simulation/physicsEngine/YoMatrix.java +++ /dev/null @@ -1,275 +0,0 @@ -package us.ihmc.scs2.simulation.physicsEngine; - -import org.ejml.data.DMatrix; -import org.ejml.data.Matrix; -import org.ejml.data.MatrixType; -import org.ejml.data.ReshapeMatrix; -import org.ejml.ops.MatrixIO; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoInteger; - -/** - * @deprecated please use {@link us.ihmc.yoVariables.math.YoMatrix} - */ -@Deprecated -public class YoMatrix implements DMatrix, ReshapeMatrix -{ - // TODO: eventually consolidate YoMatrix implementations - - private static final long serialVersionUID = 2156411740647948028L; - - private final int maxNumberOfRows, maxNumberOfColumns; - - private final YoInteger numberOfRows, numberOfColumns; - private final YoDouble[][] variables; - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, null, null, registry); - } - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); - } - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, columnNames, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) - { - this(name, description, maxNumberOfRows, maxNumberOfColumns, null, null, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) - { - this(name, description, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) - { - this.maxNumberOfRows = maxNumberOfRows; - this.maxNumberOfColumns = maxNumberOfColumns; - - this.numberOfRows = new YoInteger(name + "NumRows", registry); - this.numberOfColumns = new YoInteger(name + "NumCols", registry); - - this.numberOfRows.set(maxNumberOfRows); - this.numberOfColumns.set(maxNumberOfColumns); - - variables = new YoDouble[maxNumberOfRows][maxNumberOfColumns]; - - for (int row = 0; row < maxNumberOfRows; row++) - { - for (int column = 0; column < maxNumberOfColumns; column++) - { - switch (checkNames(rowNames, columnNames)) - { - case NONE: - { - variables[row][column] = new YoDouble(name + "_" + row + "_" + column, description, registry); // names are simply the row and column indices - break; - } - case ROWS: - { - if (maxNumberOfColumns > 1) - throw new IllegalArgumentException("The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); - - variables[row][column] = new YoDouble(name + "_" + rowNames[row], description, registry); // names are the row names, no column identifier - break; - } - case ROWS_AND_COLUMNS: - { - variables[row][column] = new YoDouble(name + "_" + rowNames[row] + "_" + columnNames[column], description, registry); // names are the row and column names - break; - } - } - } - } - } - - private enum NamesProvided - { - NONE, ROWS, ROWS_AND_COLUMNS - } - - private NamesProvided checkNames(String[] rowNames, String[] columnNames) - { - if (rowNames == null && columnNames == null) - return NamesProvided.NONE; - else if (rowNames != null && columnNames == null) - return NamesProvided.ROWS; - else - return NamesProvided.ROWS_AND_COLUMNS; - } - - @Override - public void reshape(int numRows, int numCols) - { - if (numRows > maxNumberOfRows) - throw new IllegalArgumentException("Too many rows. Expected less or equal to " + maxNumberOfRows + ", was " + numRows); - else if (numCols > maxNumberOfColumns) - throw new IllegalArgumentException("Too many columns. Expected less or equal to " + maxNumberOfColumns + ", was " + numCols); - else if (numRows < 0 || numCols < 0) - throw new IllegalArgumentException("Cannot reshape with a negative number of rows or columns."); - - numberOfRows.set(numRows); - numberOfColumns.set(numCols); - - for (int row = 0; row < numRows; row++) - { - for (int col = numCols; col < maxNumberOfColumns; col++) - { - unsafe_set(row, col, Double.NaN); - } - } - - for (int row = numRows; row < maxNumberOfRows; row++) - { - for (int col = 0; col < maxNumberOfColumns; col++) - { - unsafe_set(row, col, Double.NaN); - } - } - } - - @Override - public void set(Matrix original) - { - if (original instanceof DMatrix) - { - DMatrix otherMatrix = (DMatrix) original; - reshape(otherMatrix.getNumRows(), otherMatrix.getNumRows()); - for (int row = 0; row < getNumRows(); row++) - { - for (int col = 0; col < getNumCols(); col++) - { - set(row, col, otherMatrix.unsafe_get(row, col)); - } - } - } - } - - @Override - public void set(int row, int col, double val) - { - if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) - throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); - unsafe_set(row, col, val); - } - - @Override - public void unsafe_set(int row, int col, double val) - { - variables[row][col].set(val); - } - - @Override - public double get(int row, int col) - { - if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) - throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); - return unsafe_get(row, col); - } - - @Override - public double unsafe_get(int row, int col) - { - return variables[row][col].getValue(); - } - - @Override - public void zero() - { - for (int row = 0; row < getNumRows(); row++) - { - for (int col = 0; col < getNumCols(); col++) - { - variables[row][col].set(0.0); - } - } - } - - public void setToNaN(int numRows, int numCols) - { - reshape(numRows, numCols); - for (int row = 0; row < numRows; row++) - { - for (int col = 0; col < numCols; col++) - { - unsafe_set(row, col, Double.NaN); - } - } - } - - public void add(DMatrix otherMatrix) - { - if (otherMatrix.getNumRows() != getNumRows() || otherMatrix.getNumCols() != getNumCols()) - throw new IllegalArgumentException("Incompatible matrix sizes."); - for (int row = 0; row < getNumRows(); row++) - { - for (int col = 0; col < getNumCols(); col++) - { - unsafe_set(row, col, unsafe_get(row, col) + otherMatrix.unsafe_get(row, col)); - } - } - } - - @Override - public int getNumRows() - { - return numberOfRows.getValue(); - } - - @Override - public int getNumCols() - { - return numberOfColumns.getValue(); - } - - @Override - public int getNumElements() - { - return getNumRows() * getNumCols(); - } - - @Override - public MatrixType getType() - { - return MatrixType.UNSPECIFIED; - } - - @Override - public void print() - { - MatrixIO.printFancy(System.out, this, MatrixIO.DEFAULT_LENGTH); - } - - @Override - public void print(String format) - { - MatrixIO.print(System.out, this, format); - } - - @Override - public T createLike() - { - throw new UnsupportedOperationException(); - } - - @Override - public T create(int numRows, int numCols) - { - throw new UnsupportedOperationException(); - } - - @Override - public T copy() - { - throw new UnsupportedOperationException(); - } -}