/** * Sets the selection frame of both the angular and linear parts to {@code null}. * <p> * When the selection frame is {@code null}, the conversion into a 6-by-6 selection matrix will * be done regardless of the destination frame. * </p> */ public void clearSelectionFrame() { setSelectionFrame(null); }
public void setDesiredPelvisHeight(FramePoint3D pointContainingDesiredHeight) { yoDesiredPelvisPosition.setMatchingFrame(pointContainingDesiredHeight); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
public void setDesiredPelvisHeight(double desiredHeightInWorld) { yoDesiredPelvisPosition.setZ(desiredHeightInWorld); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
selectionMatrix6D.setSelectionFrame(selectionFrame);
public void holdCurrentPelvisHeight() { yoDesiredPelvisPosition.setFromReferenceFrame(fullRobotModel.getPelvis().getParentJoint().getFrameAfterJoint()); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
selectionMatrix6D.setSelectionFrame(selectionFrame);
assertNull(selectionMatrix6D.getLinearSelectionFrame()); ReferenceFrame randomFrame = EuclidFrameRandomTools.nextReferenceFrame("blop" + i, random, ReferenceFrame.getWorldFrame()); selectionMatrix6D.setSelectionFrame(randomFrame); assertTrue(randomFrame == selectionMatrix6D.getAngularSelectionFrame()); assertTrue(randomFrame == selectionMatrix6D.getLinearSelectionFrame()); selectionMatrix6D.setSelectionFrame(randomFrame); selectionMatrix6D.setToLinearSelectionOnly(); assertNull(selectionMatrix6D.getAngularSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); selectionMatrix6D.setSelectionFrame(randomFrame); selectionMatrix6D.setToAngularSelectionOnly(); assertNull(selectionMatrix6D.getAngularSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); selectionMatrix6D.setSelectionFrame(randomFrame); selectionMatrix6D.resetSelection(); assertNull(selectionMatrix6D.getAngularSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); selectionMatrix6D.setSelectionFrame(randomFrame); selectionMatrix6D.clearAngularSelectionFrame(); assertNull(selectionMatrix6D.getAngularSelectionFrame()); assertTrue(randomFrame == selectionMatrix6D.getLinearSelectionFrame()); selectionMatrix6D.setSelectionFrame(randomFrame); selectionMatrix6D.clearLinearSelectionFrame(); assertTrue(randomFrame == selectionMatrix6D.getAngularSelectionFrame());
@ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testVMCSelectTorqueY() { 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.setSelectionFrame(robotLeg.getReferenceFrames().getCenterOfMassFrame()); selectionMatrix.clearSelection(); selectionMatrix.selectAngularY(true); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }