Fixing spatialFeedbackController.

Set the selectionMatrix.getAngularPar().setAxisSelection(false,false, true) makes error.

Author: Beomyeong

ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java

@@ -411,7 +411,8 @@ public void submitFeedbackControlCommand(SpatialFeedbackControlCommand command)
       dampingRatioMatrix.set(4,4, gains.getPositionGains().getDampingRatios()[1]);
       dampingRatioMatrix.set(5,5, gains.getPositionGains().getDampingRatios()[2]);
       command.getSpatialAccelerationCommand().getSelectionMatrix(selectionMatrix);
-      selectionMatrix.getAngularPart().setAxisSelection(false, false, true);
+      // This one should be set from the orientation gains, put zero for x, y kind of this way
+//      selectionMatrix.getAngularPart().setAxisSelection(false, false, true);
       angularGainsFrame = command.getAngularGainsFrame();
       linearGainsFrame = command.getLinearGainsFrame();
 

ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceSpatialFeedbackControllerTest.java

@@ -93,9 +93,9 @@ public void testBaseFrame()
       spatialFeedbackControlCommand.set(baseBody, endEffector);
       spatialFeedbackControlCommand.setGains(gains);
       spatialFeedbackControlCommand.setInverseDynamics(desiredPose, zero, zero);
-      spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand);
       spatialFeedbackController.setEnabled(true);
       spatialFeedbackController.setImpedanceEnabled(true);
+      spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand);
 
       MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator();
       NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(MultiBodySystemTools.computeDegreesOfFreedom(joints));
@@ -174,9 +174,10 @@ public void testConvergence() throws Exception
       spatialFeedbackControlCommand.setGains(gains);
       spatialFeedbackControlCommand.setControlFrameFixedInEndEffector(bodyFixedPointToControl);
       spatialFeedbackControlCommand.setInverseDynamics(desiredOrientation, desiredPosition, new FrameVector3D(worldFrame), new FrameVector3D(worldFrame), new FrameVector3D(worldFrame), new FrameVector3D(worldFrame));
-      spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand);
       spatialFeedbackController.setEnabled(true);
       spatialFeedbackController.setImpedanceEnabled(true);
+      spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand);
+
 
       int numberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsToOptimizeFor);
       NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(numberOfDoFs);
@@ -275,16 +276,17 @@ public void testConvergenceWithJerryQP() throws Exception
       SpatialFeedbackControlCommand spatialFeedbackControlCommand = new SpatialFeedbackControlCommand();
       spatialFeedbackControlCommand.set(elevator, endEffector);
       DefaultPIDSE3Gains gains = new DefaultPIDSE3Gains();
-      gains.getPositionGains().setProportialAndDerivativeGains(1.5, Double.NaN);
+      gains.getPositionGains().setProportialAndDerivativeGains(10, Double.NaN);
       gains.getOrientationGains().setProportialAndDerivativeGains(1.5, Double.NaN);
       gains.getPositionGains().setDampingRatios(1);
       gains.getOrientationGains().setDampingRatios(1);
       spatialFeedbackControlCommand.setGains(gains);
       spatialFeedbackControlCommand.setControlFrameFixedInEndEffector(bodyFixedPointToControl);
       spatialFeedbackControlCommand.setInverseDynamics(desiredOrientation, desiredPosition, new FrameVector3D(worldFrame), new FrameVector3D(worldFrame), new FrameVector3D(worldFrame), new FrameVector3D(worldFrame));
-      spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand);
       spatialFeedbackController.setEnabled(true);
       spatialFeedbackController.setImpedanceEnabled(true);
+      spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand);
+
 
       int numberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsToOptimizeFor);
       NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(numberOfDoFs);