@ContinuousIntegrationTest(estimatedDuration = 4.7) @Test(timeout = 30000) public void testVMCWithPlanarArm() throws Exception { PlanarRobotArm robotArm = VirtualModelMomentumControllerTestHelper.createPlanarArm(); SCSRobotFromInverseDynamicsRobotModel scsRobotArm = robotArm.getSCSRobotArm(); List<RigidBodyBasics> endEffectors = new ArrayList<>(); RigidBodyBasics hand = robotArm.getHand(); endEffectors.add(hand); double forceX = random.nextDouble() * 10.0; double forceZ = random.nextDouble() * 10.0; double torqueY = random.nextDouble() * 10.0; Vector3D desiredForce = new Vector3D(forceX, 0.0, forceZ); Vector3D desiredTorque = new Vector3D(0.0, torqueY, 0.0); List<Vector3D> desiredForces = new ArrayList<>(); List<Vector3D> desiredTorques = new ArrayList<>(); desiredForces.add(desiredForce); desiredTorques.add(desiredTorque); List<ExternalForcePoint> externalForcePoints = new ArrayList<>(); externalForcePoints.add(robotArm.getExternalForcePoint()); SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); VirtualModelMomentumControllerTestHelper.createVirtualModelMomentumControlTest(scsRobotArm, robotArm, robotArm.getCenterOfMassFrame(), endEffectors, desiredForces, desiredTorques, externalForcePoints, selectionMatrix, simulationTestingParameters); }
@ContinuousIntegrationTest(estimatedDuration = 6.6) @Test(timeout = 33000) public void testVMCWithArm() throws Exception { RobotArm robotArm = VirtualModelMomentumControllerTestHelper.createRobotArm(); SCSRobotFromInverseDynamicsRobotModel scsRobotArm = robotArm.getSCSRobotArm(); List<RigidBodyBasics> endEffectors = new ArrayList<>(); RigidBodyBasics hand = robotArm.getHand(); endEffectors.add(hand); Vector3D desiredForce = EuclidCoreRandomTools.nextVector3D(random, -10.0, 10.0); Vector3D desiredTorque = EuclidCoreRandomTools.nextVector3D(random, -10.0, 10.0); List<Vector3D> desiredForces = new ArrayList<>(); List<Vector3D> desiredTorques = new ArrayList<>(); desiredForces.add(desiredForce); desiredTorques.add(desiredTorque); List<ExternalForcePoint> externalForcePoints = new ArrayList<>(); externalForcePoints.add(robotArm.getExternalForcePoint()); VirtualModelMomentumControllerTestHelper.createVirtualModelMomentumControlTest(scsRobotArm, robotArm, robotArm.getCenterOfMassFrame(), endEffectors, desiredForces, desiredTorques, externalForcePoints, new SelectionMatrix6D(), simulationTestingParameters); }
externalForcePoints.add(sideDependentExternalForcePoints.get(RobotSide.RIGHT)); SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
@ContinuousIntegrationTest(estimatedDuration = 10.0) @Test(timeout = 50000) public void testHydra() throws Exception { ForkedRobotArm robotArm = VirtualModelMomentumControllerTestHelper.createForkedRobotArm(); SCSRobotFromInverseDynamicsRobotModel scsRobotArm = robotArm.getSCSRobotArm(); List<RigidBodyBasics> endEffectors = new ArrayList<>(); RigidBodyBasics leftHand = robotArm.getHand(RobotSide.LEFT); RigidBodyBasics rightHand = robotArm.getHand(RobotSide.RIGHT); endEffectors.add(leftHand); endEffectors.add(rightHand); double forceZ = random.nextDouble() * 10.0; Vector3D desiredForce1 = new Vector3D(0.0, 0.0, forceZ); Vector3D desiredForce2 = new Vector3D(0.0, 0.0, forceZ); Vector3D desiredTorque1 = new Vector3D(0.0, 0.0, 0.0); Vector3D desiredTorque2 = new Vector3D(0.0, 0.0, 0.0); List<Vector3D> desiredForces = new ArrayList<>(); List<Vector3D> desiredTorques = new ArrayList<>(); desiredForces.add(desiredForce1); desiredForces.add(desiredForce2); desiredTorques.add(desiredTorque1); desiredTorques.add(desiredTorque2); SideDependentList<ExternalForcePoint> sideDependentExternalForcePoints = robotArm.getExternalForcePoints(); List<ExternalForcePoint> externalForcePoints = new ArrayList<>(); externalForcePoints.add(sideDependentExternalForcePoints.get(RobotSide.LEFT)); externalForcePoints.add(sideDependentExternalForcePoints.get(RobotSide.RIGHT)); SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); VirtualModelMomentumControllerTestHelper.createVirtualModelMomentumControlTest(scsRobotArm, robotArm, robotArm.getCenterOfMassFrame(), endEffectors, desiredForces, desiredTorques, externalForcePoints, selectionMatrix, simulationTestingParameters); }
@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.6) @Test(timeout = 30000) public void testVMCSelectAll() { 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(); 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.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.5) @Test(timeout = 30000) public void testVMCSelectForceXTorqueY() { 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.selectLinearX(true); selectionMatrix.selectAngularY(true); 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.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 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); }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMCSelectForceYZTorqueX() { 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.selectLinearY(true); selectionMatrix.selectLinearZ(true); selectionMatrix.selectAngularX(true); 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; }