/** * Updates the selection of the axes of interest for the angular part of this selection matrix. * <p> * Note that it is preferable to also set angular selection frame to which this selection is * referring to. * </p> * <p> * The linear part of this selection matrix remains unchanged. * </p> * * @param xSelected whether the angular x-axis is an axis of interest. * @param ySelected whether the angular y-axis is an axis of interest. * @param zSelected whether the angular z-axis is an axis of interest. */ public void setAngularAxisSelection(boolean xSelected, boolean ySelected, boolean zSelected) { selectAngularX(xSelected); selectAngularY(ySelected); selectAngularZ(zSelected); }
break; case 2: selectAngularZ(select); break; case 3:
public static void setSelectionMatrix(SelectionMatrix6D selectionMatrix, ConfigurationSpaceName configurationSpaceName, boolean select) { switch (configurationSpaceName) { case X: selectionMatrix.selectLinearX(select); break; case Y: selectionMatrix.selectLinearY(select); break; case Z: selectionMatrix.selectLinearZ(select); break; case ROLL: selectionMatrix.selectAngularX(select); break; case PITCH: selectionMatrix.selectAngularY(select); break; case YAW: selectionMatrix.selectAngularZ(select); break; default: throw new RuntimeException("Unexpected enum value: " + configurationSpaceName); } }
@ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testVMCSelectTorqueZ() { RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity); RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT); RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor(); RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor(); // send in the correct frame with identity selection matrix FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame()); FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame()); Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce); SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); selectionMatrix.selectAngularZ(true); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
@ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testVMCSelectForceXTorqueXZ() { RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity); RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT); RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor(); RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor(); // send in the correct frame with identity selection matrix FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame()); FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame()); Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce); SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.selectLinearX(true); selectionMatrix.selectAngularX(true); selectionMatrix.selectAngularZ(true); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
selectionMatrix6D.selectAngularZ(zAngularSelected); selectionMatrix6D.selectLinearX(xLinearSelected); selectionMatrix6D.selectLinearY(yLinearSelected);