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; }
SelectionMatrix6D selectionMatrix6D = new SelectionMatrix6D(); assertNull(selectionMatrix6D.getAngularSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); assertTrue(selectionMatrix6D.isAngularXSelected()); assertTrue(selectionMatrix6D.isAngularYSelected()); assertTrue(selectionMatrix6D.isAngularZSelected()); assertTrue(selectionMatrix6D.isAngularPartActive()); assertTrue(selectionMatrix6D.isLinearXSelected()); assertTrue(selectionMatrix6D.isLinearYSelected()); assertTrue(selectionMatrix6D.isLinearZSelected()); assertTrue(selectionMatrix6D.isLinearPartActive()); boolean yLinearSelected = random.nextBoolean(); boolean zLinearSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(xAngularSelected, yAngularSelected, zAngularSelected); selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); assertEquals(xAngularSelected, selectionMatrix6D.isAngularXSelected()); assertEquals(yAngularSelected, selectionMatrix6D.isAngularYSelected()); assertEquals(zAngularSelected, selectionMatrix6D.isAngularZSelected()); assertEquals(xAngularSelected || yAngularSelected || zAngularSelected, selectionMatrix6D.isAngularPartActive()); assertEquals(xLinearSelected, selectionMatrix6D.isLinearXSelected()); assertEquals(yLinearSelected, selectionMatrix6D.isLinearYSelected()); assertEquals(zLinearSelected, selectionMatrix6D.isLinearZSelected()); assertEquals(xLinearSelected || yLinearSelected || zLinearSelected, selectionMatrix6D.isLinearPartActive()); selectionMatrix6D.selectAngularX(xAngularSelected); selectionMatrix6D.selectAngularY(yAngularSelected); selectionMatrix6D.selectAngularZ(zAngularSelected); selectionMatrix6D.selectLinearX(xLinearSelected);
/** * Returns true if any of the linear axis are enabled * * @return */ public boolean isLinearPartActive() { return isLinearXSelected() || isLinearYSelected() || isLinearZSelected(); }
/** * Returns true if any of the angular axis are enabled * * @return */ public boolean isAngularPartActive() { return isAngularXSelected() || isAngularYSelected() || isAngularZSelected(); }
public void setChestAngularControl(boolean roll, boolean pitch, boolean yaw) { chestSelectionMatrix.setAngularAxisSelection(roll, pitch, yaw); chestSelectionMatrix.clearLinearSelection(); }
SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.resetSelection(); SelectionMatrix3DMessage angularSelection = trajectory.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = trajectory.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); if (!selectionMatrix.isLinearXSelected() || !selectionMatrix.isLinearYSelected() || !selectionMatrix.isLinearZSelected()) positionErrorQ.zero(); if (!selectionMatrix.isAngularXSelected() || !selectionMatrix.isAngularYSelected() || !selectionMatrix.isAngularZSelected()) rotationErrorQ.zero();
SelectionMatrix6D selectionMatrix6D = new SelectionMatrix6D(); boolean angularXSelected = random.nextBoolean(); boolean angularYSelected = random.nextBoolean(); boolean angularZSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(angularXSelected, angularYSelected, angularZSelected); selectionMatrix6D.setLinearAxisSelection(linearXSelected, linearYSelected, linearZSelected); selectionMatrix6D.setSelectionFrames(angularSelectionFrame, linearSelectionFrame); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart())); SelectionMatrix6D selectionMatrixFromCommand = new SelectionMatrix6D(); spatialAccelerationCommand.getSelectionMatrix(selectionMatrixFromCommand); SelectionMatrix3D selectionMatrixLinearPart = selectionMatrix6D.getLinearPart(); assertEquals(linearXSelected, selectionMatrixLinearPart.isXSelected()); assertEquals(linearYSelected, selectionMatrixLinearPart.isYSelected()); assertEquals(linearZSelected, selectionMatrixLinearPart.isZSelected()); SelectionMatrix3D selectionMatrixAngularPart = selectionMatrix6D.getAngularPart(); assertEquals(angularXSelected, selectionMatrixAngularPart.isXSelected()); assertEquals(angularYSelected, selectionMatrixAngularPart.isYSelected()); assertEquals(angularZSelected, selectionMatrixAngularPart.isZSelected()); assertEquals(angularSelectionFrame, selectionMatrix6D.getAngularSelectionFrame()); assertEquals(linearSelectionFrame, selectionMatrix6D.getLinearSelectionFrame()); assertTrue(selectionMatrix6D.equals(selectionMatrixFromCommand));
PelvisTrajectoryMessage pelvisTrajectoryMessage = new PelvisTrajectoryMessage(); pelvisTrajectoryMessage.setEnableUserPelvisControlDuringWalking(false); SelectionMatrix6D selectionMatrix6D = new SelectionMatrix6D(); selectionMatrix6D.clearAngularSelection(); selectionMatrix6D.clearLinearSelection(); selectionMatrix6D.selectLinearZ(true); pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix() .set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); pelvisTrajectoryMessage.getSe3Trajectory().getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart()));
return isAngularXSelected(); case 1: return isAngularYSelected(); case 2: return isAngularZSelected(); case 3: return isLinearXSelected(); case 4: return isLinearYSelected(); case 5: return isLinearZSelected(); default: throw new IndexOutOfBoundsException(Integer.toString(axisIndex));
tf = Math.max(t0, trajectoryMessage.getWaypointTimes().get(trajectoryMessage.getWaypoints().size() - 1)); SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.resetSelection(); SelectionMatrix3DMessage angularSelection = trajectoryMessage.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = trajectoryMessage.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); if (!selectionMatrix.isLinearXSelected() && !selectionMatrix.isLinearYSelected() && !selectionMatrix.isLinearZSelected()) continue; // The position part is not dictated by trajectory, let's not visualize.
public static WaypointBasedTrajectoryMessage createWaypointBasedTrajectoryMessage(RigidBodyBasics endEffector, double[] waypointTimes, Pose3D[] waypoints, SelectionMatrix6D selectionMatrix) { WaypointBasedTrajectoryMessage message = new WaypointBasedTrajectoryMessage(); message.setEndEffectorHashCode(endEffector.hashCode()); if (waypointTimes.length != waypoints.length) throw new RuntimeException("Inconsistent array lengths."); message.getWaypointTimes().reset(); message.getWaypointTimes().add(waypointTimes); MessageTools.copyData(waypoints, message.getWaypoints()); if (selectionMatrix != null) { message.getAngularSelectionMatrix().setSelectionFrameId(MessageTools.toFrameId(selectionMatrix.getAngularSelectionFrame())); message.getAngularSelectionMatrix().setXSelected(selectionMatrix.isAngularXSelected()); message.getAngularSelectionMatrix().setYSelected(selectionMatrix.isAngularYSelected()); message.getAngularSelectionMatrix().setZSelected(selectionMatrix.isAngularZSelected()); message.getLinearSelectionMatrix().setSelectionFrameId(MessageTools.toFrameId(selectionMatrix.getLinearSelectionFrame())); message.getLinearSelectionMatrix().setXSelected(selectionMatrix.isLinearXSelected()); message.getLinearSelectionMatrix().setYSelected(selectionMatrix.isLinearYSelected()); message.getLinearSelectionMatrix().setZSelected(selectionMatrix.isLinearZSelected()); } return message; }
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); } }
SelectionMatrix6D selectionMatrix6D = new SelectionMatrix6D(); boolean zLinearSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(xAngularSelected, yAngularSelected, zAngularSelected); selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); selectionMatrix6D.setSelectionFrame(selectionFrame); selectionMatrix6D.getFullSelectionMatrixInFrame(destinationFrame, expectedSelectionMatrix); MatrixTools.removeZeroRows(expectedSelectionMatrix, 1.0e-7); selectionMatrix6D.getCompactSelectionMatrixInFrame(destinationFrame, actualSelectionMatrix);
@Override public void set(WaypointBasedTrajectoryMessage message, Map<Integer, RigidBodyBasics> rigidBodyNamedBasedHashMap, ReferenceFrameHashCodeResolver referenceFrameResolver) { clear(); endEffectorHashCode = message.getEndEffectorHashCode(); if (rigidBodyNamedBasedHashMap == null) endEffector = null; else endEffector = rigidBodyNamedBasedHashMap.get(endEffectorHashCode); for (int i = 0; i < message.getWaypoints().size(); i++) { waypointTimes.add(message.getWaypointTimes().get(i)); waypoints.add().set(message.getWaypoints().get(i)); } ReferenceFrame referenceFrame = endEffector == null ? null : endEffector.getBodyFixedFrame(); controlFramePose.setIncludingFrame(referenceFrame, message.getControlFramePositionInEndEffector(), message.getControlFrameOrientationInEndEffector()); selectionMatrix.resetSelection(); SelectionMatrix3DMessage angularSelection = message.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = message.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); weight = message.getWeight(); }
weightMatrix.setLinearWeights(linearWeight.getXWeight(), linearWeight.getYWeight(), linearWeight.getZWeight()); selectionMatrix.clearSelectionFrame(); SelectionMatrix3DMessage angularSelection = message.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = message.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); selectionMatrix.setSelectionFrames(angularSelectionFrame, linearSelectionFrame); ReferenceFrame angularWeightFrame = referenceFrameResolver.getReferenceFrameFromHashCode(angularWeight.getWeightFrameId()); ReferenceFrame linearWeightFrame = referenceFrameResolver.getReferenceFrameFromHashCode(linearWeight.getWeightFrameId());
public void setDesiredPelvisHeight(FramePoint3D pointContainingDesiredHeight) { yoDesiredPelvisPosition.setMatchingFrame(pointContainingDesiredHeight); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
@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); }
rigidBodyMessage.getControlFramePositionInEndEffector().set(command.getControlFramePose().getPosition()); rigidBodyMessage.getControlFrameOrientationInEndEffector().set(command.getControlFramePose().getOrientation()); rigidBodyMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(command.getSelectionMatrix().getLinearPart())); rigidBodyMessage.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(command.getSelectionMatrix().getAngularPart())); rigidBodyMessage.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(command.getWeightMatrix().getLinearPart())); rigidBodyMessage.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(command.getWeightMatrix().getAngularPart()));
@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 selectionMatrix = new SelectionMatrix6D(); selectionMatrix.resetSelection(); WaypointBasedTrajectoryMessage trajectory = WholeBodyTrajectoryToolboxMessageTools.createTrajectoryMessage(hand, 0.0, trajectoryTime, timeResolution, handFunction, selectionMatrix);