public void setPelvisAngularControl(boolean roll, boolean pitch, boolean yaw) { pelvisSelectionMatrix.setAngularAxisSelection(roll, pitch, yaw); pelvisSelectionMatrix.clearLinearSelection(); }
public void setChestAngularControl(boolean roll, boolean pitch, boolean yaw) { chestSelectionMatrix.setAngularAxisSelection(roll, pitch, yaw); chestSelectionMatrix.clearLinearSelection(); }
boolean zLinearSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(xAngularSelected, yAngularSelected, zAngularSelected); selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); selectionMatrix6D.setSelectionFrame(selectionFrame);
boolean zLinearSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(xAngularSelected, yAngularSelected, zAngularSelected); selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); selectionMatrix6D.setSelectionFrame(selectionFrame);
boolean angularYSelected = random.nextBoolean(); boolean angularZSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(angularXSelected, angularYSelected, angularZSelected);
SelectionMatrix3DMessage angularSelection = trajectoryMessage.getAngularSelectionMatrix(); 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 angularYSelected = random.nextBoolean(); boolean angularZSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(angularXSelected, angularYSelected, angularZSelected);
SelectionMatrix3DMessage angularSelection = message.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = message.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected());
boolean yLinearSelected = random.nextBoolean(); boolean zLinearSelected = random.nextBoolean(); selectionMatrix6D.setAngularAxisSelection(xAngularSelected, yAngularSelected, zAngularSelected); selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); assertEquals(xAngularSelected, selectionMatrix6D.isAngularXSelected());
SelectionMatrix3DMessage angularSelection = message.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = message.getLinearSelectionMatrix(); selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected());
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())
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())