@Override public void clear() { clearQueuableCommandVariables(); trajectoryPointList.clear(); selectionMatrix.resetSelection(); weightMatrix.clear(); }
public void setHandLinearControlAndYawPitchOnly(RobotSide robotSide) { handSelectionMatrices.get(robotSide).resetSelection(); handSelectionMatrices.get(robotSide).selectAngularX(false); }
public void clear(ReferenceFrame referenceFrame) { trajectoryPointList.clear(referenceFrame); clearQueuableCommandVariables(); selectionMatrix.resetSelection(); weightMatrix.clear(); }
@Override public void clear() { endEffectorHashCode = 0; endEffector = null; waypointTimes.clear(); waypoints.clear(); controlFramePose.setToNaN(ReferenceFrame.getWorldFrame()); selectionMatrix.resetSelection(); weight = Double.NaN; }
@Override public void clear() { endEffectorHashCode = 0; endEffector = null; desiredPose.setToNaN(ReferenceFrame.getWorldFrame()); controlFramePose.setToNaN(ReferenceFrame.getWorldFrame()); selectionMatrix.resetSelection(); weightMatrix.clear(); }
@Override public void clear() { endEffectorHashCode = 0; endEffector = null; waypoints.clear(); waypointTimes.clear(); selectionMatrix.resetSelection(); weightMatrix.clear(); controlFramePose.setToNaN(ReferenceFrame.getWorldFrame()); allowablePositionDisplacement.clear(); allowableOrientationDisplacement.clear(); }
selectionMatrix.resetSelection(); WaypointBasedTrajectoryMessage trajectory = WholeBodyTrajectoryToolboxMessageTools.createTrajectoryMessage(hand, 0.0, trajectoryTime, timeResolution, handFunction, selectionMatrix);
selectionMatrix.resetSelection(); WaypointBasedTrajectoryMessage trajectory = createTrajectoryMessage(hand, 0.0, trajectoryTime, timeResolution, handFunction, selectionMatrix); Pose3D controlFramePose = new Pose3D();
selectionMatrix.resetSelection(); selectionMatrix.clearAngularSelection(); WaypointBasedTrajectoryMessage trajectory = createTrajectoryMessage(hand, 0.0, trajectoryTime, timeResolution, handFunction, selectionMatrix);
selectionMatrix.resetSelection(); SelectionMatrix3DMessage angularSelection = trajectoryMessage.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = trajectoryMessage.getLinearSelectionMatrix();
selectionMatrix.resetSelection(); WaypointBasedTrajectoryMessage trajectory = createTrajectoryMessage(hand, 0.0, trajectoryTime, timeResolution, handFunction, selectionMatrix); Pose3D controlFramePose = new Pose3D();
@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); }
assertEquals(zLinearSelected, selectionMatrix6D.isLinearZSelected()); selectionMatrix6D.resetSelection(); assertTrue(selectionMatrix6D.isAngularXSelected()); assertTrue(selectionMatrix6D.isAngularYSelected()); selectionMatrix6D.resetSelection(); assertNull(selectionMatrix6D.getAngularSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame());
selectionMatrix.resetSelection(); SelectionMatrix3DMessage angularSelection = trajectory.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = trajectory.getLinearSelectionMatrix();
selectionMatrix.resetSelection(); SelectionMatrix3DMessage angularSelection = trajectory.getAngularSelectionMatrix(); SelectionMatrix3DMessage linearSelection = trajectory.getLinearSelectionMatrix();