@Override public void set(QuadrupedBodyTrajectoryCommand other) { isExpressedInAbsoluteTime = other.isExpressedInAbsoluteTime; se3Trajectory.set(other.se3Trajectory); }
public QuadrupedBodyTrajectoryCommand() { se3Trajectory = new SE3TrajectoryControllerCommand(); }
/** * Same as {@link #set(T)} but does not change the trajectory points. * @param other */ public void setPropertiesOnly(T other) { commandId = other.getCommandId(); executionMode = other.getExecutionMode(); previousCommandId = other.getPreviousCommandId(); selectionMatrix.set(other.getSelectionMatrix()); }
@Override public void set(T other) { setIncludingFrame(other); setPropertiesOnly(other); }
public void set(ReferenceFrame dataFrame, ReferenceFrame trajectoryFrame, SE3TrajectoryMessage message) { this.trajectoryFrame = trajectoryFrame; clear(dataFrame); setFromMessage(message); }
@Override public void set(SE3TrajectoryControllerCommand other) { trajectoryPointList.setIncludingFrame(other.getTrajectoryPointList()); setPropertiesOnly(other); trajectoryFrame = other.getTrajectoryFrame(); useCustomControlFrame = other.useCustomControlFrame(); other.getControlFramePose(controlFramePoseInBodyFrame); }
public static void convertToSE3(SO3TrajectoryControllerCommand command, SE3TrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().clearLinearSelection(); commandToPack.getSelectionMatrix().setAngularPart(command.getSelectionMatrix()); commandToPack.getWeightMatrix().clearLinearWeights(); commandToPack.getWeightMatrix().setAngularPart(command.getWeightMatrix()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setToOrientationTrajectoryIncludingFrame(command.getTrajectoryPointList()); }
message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weightMatrix.getLinearPart())); SE3TrajectoryControllerCommand command = new SE3TrajectoryControllerCommand(); command.set(resolver, message); manager.handleTaskspaceTrajectoryCommand(command); manager.compute();
public static void convertToEuclidean(SE3TrajectoryControllerCommand command, EuclideanTrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().set(command.getSelectionMatrix().getLinearPart()); commandToPack.getWeightMatrix().set(command.getWeightMatrix().getLinearPart()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setIncludingFrame(command.getTrajectoryPointList()); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSO3() { Random random = new Random(4219L); for (int i = 0; i < 100; i++) { SO3TrajectoryControllerCommand expected = new SO3TrajectoryControllerCommand(random); SE3TrajectoryControllerCommand intermediate = new SE3TrajectoryControllerCommand(); SO3TrajectoryControllerCommand actual = new SO3TrajectoryControllerCommand(); CommandConversionTools.convertToSE3(expected, intermediate); CommandConversionTools.convertToSO3(intermediate, actual); Assert.assertEquals(0, intermediate.getSelectionMatrix().getLinearPart().getNumberOfSelectedAxes()); Assert.assertTrue(expected.epsilonEquals(actual, Double.MIN_VALUE)); } }
/** * Same as {@link #set(SE3TrajectoryControllerCommand)} but does not change the trajectory * points. * * @param other */ public void setPropertiesOnly(SE3TrajectoryControllerCommand other) { setQueueableCommandVariables(other); selectionMatrix.set(other.getSelectionMatrix()); weightMatrix.set(other.getWeightMatrix()); trajectoryFrame = other.getTrajectoryFrame(); }
@Override public void clear() { isExpressedInAbsoluteTime = true; se3Trajectory.clear(); }
@Override public boolean isCommandValid() { return robotSide != null && baseForControl != null && super.isCommandValid(); } }
/** * Set this command to the contents of the z height of the pelvis trajectory command Copies the z * points, velocities, and the linear z weight and frame * * @param command the other command */ public void set(PelvisTrajectoryCommand command) { clear(command.getSE3Trajectory().getDataFrame()); euclideanTrajectory.setQueueableCommandVariables(command.getSE3Trajectory()); WeightMatrix3D weightMatrix = command.getSE3Trajectory().getWeightMatrix().getLinearPart(); double zAxisWeight = weightMatrix.getZAxisWeight(); WeightMatrix3D currentWeightMatrix = euclideanTrajectory.getWeightMatrix(); currentWeightMatrix.setZAxisWeight(zAxisWeight); currentWeightMatrix.setWeightFrame(weightMatrix.getWeightFrame()); for (int i = 0; i < command.getSE3Trajectory().getNumberOfTrajectoryPoints(); i++) { FrameSE3TrajectoryPoint trajectoryPoint = command.getSE3Trajectory().getTrajectoryPoint(i); double time = trajectoryPoint.getTime(); double position = trajectoryPoint.getPositionZ(); double velocity = trajectoryPoint.getLinearVelocityZ(); tempPoint.setToZero(); tempPoint.setZ(position); tempVector.setToZero(); tempVector.setZ(velocity); euclideanTrajectory.addTrajectoryPoint(time, tempPoint, tempVector); } enableUserPelvisControl = true; enableUserPelvisControlDuringWalking = command.isEnableUserPelvisControlDuringWalking(); }
/** * Same as {@link #set(FootTrajectoryCommand)} but does not change the trajectory points. * * @param other */ public void setPropertiesOnly(FootTrajectoryCommand other) { se3Trajectory.setPropertiesOnly(other.se3Trajectory); robotSide = other.robotSide; }
footPointList.addTrajectoryPoint(2.0, new Point3D(1.1, -0.2, 0.35), new Quaternion(0.0, 0.0, 0.0, 1.0), new Vector3D(), new Vector3D()); footTrajectoryCommand.getSE3Trajectory().setTrajectoryPointList(footPointList); footTrajectoryCommand.setRobotSide(RobotSide.RIGHT); footTrajectoryCommand.getSE3Trajectory().setTrajectoryFrame(ReferenceFrame.getWorldFrame()); queuedControllerCommands.add(footTrajectoryCommand);
@Override public boolean isCommandValid() { return executionMode != null && getNumberOfTrajectoryPoints() > 0; } }
public static void convertToSE3(EuclideanTrajectoryControllerCommand command, SE3TrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().clearAngularSelection(); commandToPack.getSelectionMatrix().setLinearPart(command.getSelectionMatrix()); commandToPack.getWeightMatrix().clearAngularWeights(); commandToPack.getWeightMatrix().setLinearPart(command.getWeightMatrix()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setToPositionTrajectoryIncludingFrame(command.getTrajectoryPointList()); }
message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weightMatrix.getLinearPart())); SE3TrajectoryControllerCommand command = new SE3TrajectoryControllerCommand(); command.set(worldFrame, worldFrame, message); manager.handleTaskspaceTrajectoryCommand(command); manager.compute();
public static void convertToSO3(SE3TrajectoryControllerCommand command, SO3TrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().set(command.getSelectionMatrix().getAngularPart()); commandToPack.getWeightMatrix().set(command.getWeightMatrix().getAngularPart()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setIncludingFrame(command.getTrajectoryPointList()); }