selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); selectionMatrix6D.setSelectionFrame(selectionFrame);
selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); selectionMatrix6D.setSelectionFrame(selectionFrame);
boolean linearYSelected = random.nextBoolean(); boolean linearZSelected = random.nextBoolean(); selectionMatrix6D.setLinearAxisSelection(linearXSelected, linearYSelected, linearZSelected);
SelectionMatrix3DMessage linearSelection = trajectoryMessage.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected());
@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(); }
@Override public void setFromMessage(SE3TrajectoryMessage message) { HumanoidMessageTools.checkIfDataFrameIdsMatch(message.getFrameInformation(), trajectoryPointList.getReferenceFrame()); List<SE3TrajectoryPointMessage> trajectoryPointMessages = message.getTaskspaceTrajectoryPoints(); int numberOfPoints = trajectoryPointMessages.size(); for (int i = 0; i < numberOfPoints; i++) { SE3TrajectoryPointMessage se3TrajectoryPointMessage = trajectoryPointMessages.get(i); trajectoryPointList.addTrajectoryPoint(se3TrajectoryPointMessage.getTime(), se3TrajectoryPointMessage.getPosition(), se3TrajectoryPointMessage.getOrientation(), se3TrajectoryPointMessage.getLinearVelocity(), se3TrajectoryPointMessage.getAngularVelocity()); } setQueueableCommandVariables(message.getQueueingProperties()); selectionMatrix.resetSelection(); selectionMatrix.setAngularAxisSelection(message.getAngularSelectionMatrix().getXSelected(), message.getAngularSelectionMatrix().getYSelected(), message.getAngularSelectionMatrix().getZSelected()); selectionMatrix.setLinearAxisSelection(message.getLinearSelectionMatrix().getXSelected(), message.getLinearSelectionMatrix().getYSelected(), message.getLinearSelectionMatrix().getZSelected()); weightMatrix.clear(); weightMatrix.setAngularWeights(message.getAngularWeightMatrix().getXWeight(), message.getAngularWeightMatrix().getYWeight(), message.getAngularWeightMatrix().getZWeight()); weightMatrix.setLinearWeights(message.getLinearWeightMatrix().getXWeight(), message.getLinearWeightMatrix().getYWeight(), message.getLinearWeightMatrix().getZWeight()); useCustomControlFrame = message.getUseCustomControlFrame(); message.getControlFramePose().get(controlFramePoseInBodyFrame); }
boolean linearYSelected = random.nextBoolean(); boolean linearZSelected = random.nextBoolean(); selectionMatrix6D.setLinearAxisSelection(linearXSelected, linearYSelected, linearZSelected); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart()));
SelectionMatrix3DMessage linearSelection = message.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected());
boolean zLinearSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(xAngularSelected, yAngularSelected, zAngularSelected); selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); assertEquals(xAngularSelected, selectionMatrix6D.isAngularXSelected()); assertEquals(yAngularSelected, selectionMatrix6D.isAngularYSelected());
SelectionMatrix3DMessage linearSelection = message.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected());
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();
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();