Merge branch 'develop' into feature/door-perception

Author: calvertdw

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java

@@ -68,7 +68,8 @@ public CommunicationsSyncedRobotModel(DRCRobotModel robotModel,
       {
          for (RobotSide side : handModels.sides())
          {
-            handWrenchCalculators.put(side, new HandWrenchCalculator(side, fullRobotModel, null, StateEstimatorParameters.ROBOT_CONFIGURATION_DATA_PUBLISH_DT));
+            if (robotModel.getRobotVersion().hasSakeGripperJoints(side))
+               handWrenchCalculators.put(side, new HandWrenchCalculator(side, fullRobotModel, null, StateEstimatorParameters.ROBOT_CONFIGURATION_DATA_PUBLISH_DT));
          }
       }
    }

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/OneDoFJointFeedbackControlCommand.java

@@ -391,7 +391,7 @@ else if (object instanceof OneDoFJointFeedbackControlCommand)
    @Override
    public String toString()
    {
-      return getClass().getSimpleName() + ": control mode: " + controlMode + ", q: " + referencePosition + ", qd: " + referenceVelocity + ", qdd: "
+      return getClass().getSimpleName() + ": joint: " + joint.getName() + ", control mode: " + controlMode + ", q: " + referencePosition + ", qd: " + referenceVelocity + ", qdd: "
             + referenceAcceleration + ", tau: " + referenceEffort;
    }
 }

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/SpatialFeedbackControlCommand.java

@@ -1326,8 +1326,8 @@ else if (object instanceof SpatialFeedbackControlCommand)
    public String toString()
    {
       String ret = getClass().getSimpleName() + ": ";
-      ret += "base = " + spatialAccelerationCommand.getBase() + ", ";
-      ret += "endEffector = " + spatialAccelerationCommand.getEndEffector() + ", ";
+      ret += "base = " + spatialAccelerationCommand.getBase().getName() + ", ";
+      ret += "endEffector = " + spatialAccelerationCommand.getEndEffector().getName() + ", ";
       ret += "position = " + referencePosition + ", orientation = " + referenceOrientation.toStringAsYawPitchRoll();
       return ret;
    }

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/wrenchDistribution/CoPObjectiveCalculator.java

@@ -1,12 +1,12 @@
+
 package us.ihmc.commonWalkingControlModules.wrenchDistribution;
 
+import org.ejml.data.DMatrix;
 import org.ejml.data.DMatrixRMaj;
 import org.ejml.dense.row.CommonOps_DDRM;
 import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly;
 import us.ihmc.matrixlib.NativeMatrix;
 
-import java.lang.annotation.Native;
-
 public class CoPObjectiveCalculator
 {
    private final DMatrixRMaj fzRow = new DMatrixRMaj(0, 0);
@@ -15,35 +15,8 @@ public class CoPObjectiveCalculator
    public void computeTask(DMatrixRMaj wrenchJacobianInPlaneFrame,
                            FramePoint2DReadOnly desiredCoPInPlaneFrame,
                            int rhoSize,
-                           NativeMatrix jacobianToPack,
-                           NativeMatrix objectiveToPack)
-   {
-      fzRow.reshape(1, rhoSize);
-      singleCopRow.reshape(1, rhoSize);
-
-      int fzIndex = 5;
-      CommonOps_DDRM.extractRow(wrenchJacobianInPlaneFrame, fzIndex, fzRow);
-
-      // [x_cop * J_fz + J_ty] * rho == 0
-      int tauYIndex = 1;
-      CommonOps_DDRM.extractRow(wrenchJacobianInPlaneFrame, tauYIndex, singleCopRow);
-      CommonOps_DDRM.add(desiredCoPInPlaneFrame.getX(), fzRow, 1.0, singleCopRow, singleCopRow);
-      jacobianToPack.insert(singleCopRow, 0, 0);
-
-      // [y_cop * J_fz - J_tx] * rho == 0
-      int tauXIndex = 0;
-      CommonOps_DDRM.extractRow(wrenchJacobianInPlaneFrame, tauXIndex, singleCopRow);
-      CommonOps_DDRM.add(desiredCoPInPlaneFrame.getY(), fzRow, -1.0, singleCopRow, singleCopRow);
-      jacobianToPack.insert(singleCopRow, 1, 0);
-
-      objectiveToPack.zero();
-   }
-
-   public void computeTask(DMatrixRMaj wrenchJacobianInPlaneFrame,
-                           FramePoint2DReadOnly desiredCoPInPlaneFrame,
-                           int rhoSize,
-                           DMatrixRMaj jacobianToPack,
-                           DMatrixRMaj objectiveToPack)
+                           DMatrix jacobianToPack,
+                           DMatrix objectiveToPack)
    {
       fzRow.reshape(1, rhoSize);
       singleCopRow.reshape(1, rhoSize);
@@ -55,13 +28,19 @@ public void computeTask(DMatrixRMaj wrenchJacobianInPlaneFrame,
       int tauYIndex = 1;
       CommonOps_DDRM.extractRow(wrenchJacobianInPlaneFrame, tauYIndex, singleCopRow);
       CommonOps_DDRM.add(desiredCoPInPlaneFrame.getX(), fzRow, 1.0, singleCopRow, singleCopRow);
-      CommonOps_DDRM.insert(singleCopRow, jacobianToPack, 0, 0);
+      if (jacobianToPack instanceof NativeMatrix jacobian)
+         jacobian.insert(singleCopRow, 0, 0);
+      else
+         CommonOps_DDRM.insert(singleCopRow, jacobianToPack, 0, 0);
 
       // [y_cop * J_fz - J_tx] * rho == 0
       int tauXIndex = 0;
       CommonOps_DDRM.extractRow(wrenchJacobianInPlaneFrame, tauXIndex, singleCopRow);
       CommonOps_DDRM.add(desiredCoPInPlaneFrame.getY(), fzRow, -1.0, singleCopRow, singleCopRow);
-      CommonOps_DDRM.insert(singleCopRow, jacobianToPack, 1, 0);
+      if (jacobianToPack instanceof NativeMatrix jacobian)
+         jacobian.insert(singleCopRow, 1, 0);
+      else
+         CommonOps_DDRM.insert(singleCopRow, jacobianToPack, 1, 0);
 
       objectiveToPack.zero();
    }

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/wrenchDistribution/WrenchMatrixCalculator.java

@@ -284,6 +284,7 @@ public boolean getNextCenterOfPressureInput(NativeQPInputTypeA inputToPack)
          rhoStartIndex += helper.getRhoSize();
       }
 
+      // This reshape call also zeros the task objectives, jacobians, and weights
       inputToPack.reshape(2);
       inputToPack.setConstraintType(command.getConstraintType());
 
@@ -293,14 +294,12 @@ public boolean getNextCenterOfPressureInput(NativeQPInputTypeA inputToPack)
                                          inputToPack.getTaskJacobian(),
                                          inputToPack.getTaskObjective());
 
-      inputToPack.getTaskObjective().zero();
-      inputToPack.getTaskWeightMatrix().zero();
       if (command.getConstraintType() == ConstraintType.OBJECTIVE)
       {
          weight.setIncludingFrame(command.getWeight());
          weight.changeFrameAndProjectToXYPlane(planeFrame);
-         inputToPack.getTaskWeightMatrix().set(0, 0, command.getWeight().getX());
-         inputToPack.getTaskWeightMatrix().set(1, 1, command.getWeight().getY());
+         inputToPack.getTaskWeightMatrix().set(0, 0, weight.getX());
+         inputToPack.getTaskWeightMatrix().set(1, 1, weight.getY());
       }
       else if (command.getConstraintType() != ConstraintType.EQUALITY)
       {