waypoints.add(originOfRigidBody); waypoints.add(originOfRigidBody); trajectorySelectionMatrix.clearSelection(); explorationSelectionMatrix.clearSelection(); explorationConfigurationSpaces.clear();
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMCWrongExpressedInAndOnFrame() { 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(pelvis.getBodyFixedFrame(), pelvis.getBodyFixedFrame(), desiredTorque, desiredForce); // select only torque SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); selectionMatrix.setToLinearSelectionOnly(); boolean caughtException = false; try { submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); } catch (ReferenceFrameMismatchException e) { caughtException = true; } assertTrue("Wrong frame", caughtException); }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMCWrongExpressedOnFrame() { 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(pelvis.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce); // select only torque SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); selectionMatrix.setToAngularSelectionOnly(); boolean caughtException = false; try { submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); } catch (ReferenceFrameMismatchException e) { caughtException = true; } assertTrue("Wrong frame", caughtException); }
@ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testVMCSelectTorque() { 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); // select only torque SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); selectionMatrix.setToAngularSelectionOnly(); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMCWrongExpressedInFrame() { 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(), pelvis.getBodyFixedFrame(), desiredTorque, desiredForce); // select only torque SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); selectionMatrix.setToAngularSelectionOnly(); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
@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.6) @Test(timeout = 30000) public void testVMCSelectForceZ() { 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.selectLinearZ(true); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMCSelectForceX() { 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.selectLinearX(true); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMCSelectForceY() { 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.selectLinearY(true); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
@ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testVMCSelectTorqueX() { 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.selectAngularX(true); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMCSelectForce() { double gravity = -9.81; 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); // select only force SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); selectionMatrix.setToLinearSelectionOnly(); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }
@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); }
public SelectionMatrix6D getSelectionMatrix() { SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); if (trajectorySelectionMatrix.getLinearPart().isXSelected() || explorationSelectionMatrix.getLinearPart().isXSelected()) selectionMatrix.getLinearPart().selectXAxis(true); if (trajectorySelectionMatrix.getLinearPart().isYSelected() || explorationSelectionMatrix.getLinearPart().isYSelected()) selectionMatrix.getLinearPart().selectYAxis(true); if (trajectorySelectionMatrix.getLinearPart().isZSelected() || explorationSelectionMatrix.getLinearPart().isZSelected()) selectionMatrix.getLinearPart().selectZAxis(true); if (trajectorySelectionMatrix.getAngularPart().isXSelected() || explorationSelectionMatrix.getAngularPart().isXSelected()) selectionMatrix.getAngularPart().selectXAxis(true); if (trajectorySelectionMatrix.getAngularPart().isYSelected() || explorationSelectionMatrix.getAngularPart().isYSelected()) selectionMatrix.getAngularPart().selectYAxis(true); if (trajectorySelectionMatrix.getAngularPart().isZSelected() || explorationSelectionMatrix.getAngularPart().isZSelected()) selectionMatrix.getAngularPart().selectZAxis(true); return selectionMatrix; }