public void setAngularSelection(boolean selectX, boolean selectY, boolean selectZ) { angularSelection.setAxisSelection(selectX, selectY, selectZ); }
/** * Creates and initializes a new selection matrix. * * @param selectionFrame the new frame to which the axes selection is referring to. * @param xSelected whether the x-axis is an axis of interest. * @param ySelected whether the y-axis is an axis of interest. * @param zSelected whether the z-axis is an axis of interest. * @see #setSelectionFrame(ReferenceFrame) * @see #setAxisSelection(boolean, boolean, boolean) */ public SelectionMatrix3D(ReferenceFrame selectionFrame, boolean xSelected, boolean ySelected, boolean zSelected) { setSelectionFrame(selectionFrame); setAxisSelection(xSelected, ySelected, zSelected); }
boolean zSelected = random.nextBoolean(); selectionMatrix3D.setAxisSelection(xSelected, ySelected, zSelected); selectionMatrix3D.setSelectionFrame(selectionFrame);
boolean zSelected = random.nextBoolean(); selectionMatrix3D.setAxisSelection(xSelected, ySelected, zSelected); selectionMatrix3D.setSelectionFrame(selectionFrame);
boolean zSelected = random.nextBoolean(); selectionMatrix3D.setAxisSelection(xSelected, ySelected, zSelected); selectionMatrix3D.setSelectionFrame(selectionFrame);
selectionMatrix6D.setSelectionFrame(selectionFrame); angularPart3D.setAxisSelection(xAngularSelected, yAngularSelected, zAngularSelected); angularPart3D.setSelectionFrame(selectionFrame); linearPart3D.setAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); linearPart3D.setSelectionFrame(selectionFrame);
@Override public void setFromMessage(EuclideanTrajectoryMessage message) { HumanoidMessageTools.checkIfDataFrameIdsMatch(message.getFrameInformation(), trajectoryPointList.getReferenceFrame()); List<EuclideanTrajectoryPointMessage> trajectoryPointMessages = message.getTaskspaceTrajectoryPoints(); int numberOfPoints = trajectoryPointMessages.size(); for (int i = 0; i < numberOfPoints; i++) { EuclideanTrajectoryPointMessage euclideanTrajectoryPointMessage = trajectoryPointMessages.get(i); trajectoryPointList.addTrajectoryPoint(euclideanTrajectoryPointMessage.getTime(), euclideanTrajectoryPointMessage.getPosition(), euclideanTrajectoryPointMessage.getLinearVelocity()); } setQueueableCommandVariables(message.getQueueingProperties()); selectionMatrix.resetSelection(); selectionMatrix.setAxisSelection(message.getSelectionMatrix().getXSelected(), message.getSelectionMatrix().getYSelected(), message.getSelectionMatrix().getZSelected()); weightMatrix.clear(); weightMatrix.setWeights(message.getWeightMatrix().getXWeight(), message.getWeightMatrix().getYWeight(), message.getWeightMatrix().getZWeight()); useCustomControlFrame = message.getUseCustomControlFrame(); message.getControlFramePose().get(controlFramePoseInBodyFrame); }
@Override public void setFromMessage(SO3TrajectoryMessage message) { HumanoidMessageTools.checkIfDataFrameIdsMatch(message.getFrameInformation(), trajectoryPointList.getReferenceFrame()); List<SO3TrajectoryPointMessage> trajectoryPointMessages = message.getTaskspaceTrajectoryPoints(); int numberOfPoints = trajectoryPointMessages.size(); for (int i = 0; i < numberOfPoints; i++) { SO3TrajectoryPointMessage so3TrajectoryPointMessage = trajectoryPointMessages.get(i); trajectoryPointList.addTrajectoryPoint(so3TrajectoryPointMessage.getTime(), so3TrajectoryPointMessage.getOrientation(), so3TrajectoryPointMessage.getAngularVelocity()); } setQueueableCommandVariables(message.getQueueingProperties()); selectionMatrix.resetSelection(); selectionMatrix.setAxisSelection(message.getSelectionMatrix().getXSelected(), message.getSelectionMatrix().getYSelected(), message.getSelectionMatrix().getZSelected()); weightMatrix.clear(); weightMatrix.setWeights(message.getWeightMatrix().getXWeight(), message.getWeightMatrix().getYWeight(), message.getWeightMatrix().getZWeight()); useCustomControlFrame = message.getUseCustomControlFrame(); message.getControlFramePose().get(controlFramePoseInBodyFrame); }
/** * Convenience method to create a {@link KinematicsToolboxCenterOfMassMessage} that can be used * to hold the current center of mass of a robot given its root body. * <p> * By default the weight of the task is set to {@value #DEFAULT_CENTER_OF_MASS_WEIGHT} which it a * rather strong weight for the center of mass. As result, the solution should be very close to * the current center of mass position. * </p> * * @param rootBody the root body of the robot for which the center of mass is to be held in * place. * @param holdX whether the x-coordinate should be maintained. * @param holdY whether the y-coordinate should be maintained. * @param holdZ whether the z-coordinate should be maintained. * @return the message ready to send to the {@code KinematicsToolbosModule}. */ public static KinematicsToolboxCenterOfMassMessage holdCenterOfMassCurrentPosition(RigidBodyBasics rootBody, boolean holdX, boolean holdY, boolean holdZ) { KinematicsToolboxCenterOfMassMessage message = new KinematicsToolboxCenterOfMassMessage(); CenterOfMassCalculator calculator = new CenterOfMassCalculator(rootBody, worldFrame); calculator.reset(); message.getDesiredPositionInWorld().set(calculator.getCenterOfMass()); message.getWeights().set(MessageTools.createWeightMatrix3DMessage(DEFAULT_CENTER_OF_MASS_WEIGHT)); SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); selectionMatrix3D.setAxisSelection(holdX, holdY, holdZ); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix3D)); message.setDestination(PacketDestination.KINEMATICS_TOOLBOX_MODULE.ordinal()); return message; }
boolean ySelected = random.nextBoolean(); boolean zSelected = random.nextBoolean(); selectionMatrix3D.setAxisSelection(xSelected, ySelected, zSelected); assertEquals(xSelected, selectionMatrix3D.isXSelected()); assertEquals(ySelected, selectionMatrix3D.isYSelected());
public void set(KinematicsPlanningToolboxCenterOfMassMessage message, Map<Long, RigidBodyBasics> rigidBodyNamedBasedHashMap, ReferenceFrameHashCodeResolver referenceFrameResolver) { for (int i = 0; i < message.getWayPointTimes().size(); i++) { waypointTimes.add(message.getWayPointTimes().get(i)); waypoints.add().set(message.getDesiredWayPointPositionsInWorld().get(i)); } selectionMatrix.clearSelectionFrame(); SelectionMatrix3DMessage linearSelection = message.getSelectionMatrix(); selectionMatrix.setAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); weightMatrix.clear(); WeightMatrix3DMessage linearWeight = message.getWeights(); weightMatrix.setWeights(linearWeight.getXWeight(), linearWeight.getYWeight(), linearWeight.getZWeight()); if (referenceFrameResolver != null) { ReferenceFrame linearSelectionFrame = referenceFrameResolver.getReferenceFrameFromHashCode(linearSelection.getSelectionFrameId()); selectionMatrix.setSelectionFrame(linearSelectionFrame); ReferenceFrame linearWeightFrame = referenceFrameResolver.getReferenceFrameFromHashCode(linearWeight.getWeightFrameId()); weightMatrix.setWeightFrame(linearWeightFrame); } }