@Override public boolean isCommandValid() { return executionModeValid() && getNumberOfJoints() > 0; }
@Override public void set(JointspaceTrajectoryCommand other) { setQueueableCommandVariables(other); set(other.getTrajectoryPointLists()); }
public NeckTrajectoryCommand(Random random) { jointspaceTrajectory = new JointspaceTrajectoryCommand(random); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000, expected = IndexOutOfBoundsException.class) public void testClear() { ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(); armTrajectoryCommand.clear(); assertNull(armTrajectoryCommand.getRobotSide()); assertEquals(Packet.VALID_MESSAGE_DEFAULT_ID, armTrajectoryCommand.getJointspaceTrajectory().getCommandId()); assertEquals(0.0, armTrajectoryCommand.getExecutionDelayTime(), 1e-9); assertEquals(ExecutionMode.OVERRIDE, armTrajectoryCommand.getJointspaceTrajectory().getExecutionMode()); assertEquals(0.0, armTrajectoryCommand.getExecutionTime(), 1e-9); assertEquals(0, armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints()); assertEquals(Packet.INVALID_MESSAGE_ID, armTrajectoryCommand.getJointspaceTrajectory().getPreviousCommandId()); assertEquals(0, armTrajectoryCommand.getJointspaceTrajectory().getTrajectoryPointLists().size()); armTrajectoryCommand.getJointspaceTrajectory().getJointTrajectoryPoint(0, 0); }
armTrajectoryCommand.epsilonEquals(otherArmTrajectoryCommand, 1e-8); assertEquals(armTrajectoryCommand.getJointspaceTrajectory().getCommandId(), otherArmTrajectoryCommand.getJointspaceTrajectory().getCommandId()); assertEquals(armTrajectoryCommand.getExecutionDelayTime(), otherArmTrajectoryCommand.getExecutionDelayTime(), 1e-9); assertEquals(armTrajectoryCommand.getJointspaceTrajectory().getExecutionMode(), otherArmTrajectoryCommand.getJointspaceTrajectory().getExecutionMode()); assertEquals(armTrajectoryCommand.getExecutionTime(), otherArmTrajectoryCommand.getExecutionTime(), 1e-9); assertEquals(armTrajectoryCommand.getMessageClass(), otherArmTrajectoryCommand.getMessageClass()); assertEquals(armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints(), otherArmTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints()); assertEquals(armTrajectoryCommand.getJointspaceTrajectory().getPreviousCommandId(), otherArmTrajectoryCommand.getJointspaceTrajectory().getPreviousCommandId()); assertEquals(armTrajectoryCommand.getRobotSide(), otherArmTrajectoryCommand.getRobotSide()); for (int i = 0; i < armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints(); i++) OneDoFJointTrajectoryCommand jointTrajectoryPointList = armTrajectoryCommand.getJointspaceTrajectory().getJointTrajectoryPointList(i); OneDoFJointTrajectoryCommand otherJointTrajectoryPointList = otherArmTrajectoryCommand.getJointspaceTrajectory().getJointTrajectoryPointList(i);
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testSetArmTrajectoryMessage() { Random random = new Random(); ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(); ArmTrajectoryMessage message = RandomHumanoidMessages.nextArmTrajectoryMessage(random); armTrajectoryCommand.setFromMessage(message); assertEquals(message.getJointspaceTrajectory().getQueueingProperties().getExecutionDelayTime(), armTrajectoryCommand.getExecutionDelayTime(), 1e-9); assertEquals(ExecutionMode.fromByte(message.getJointspaceTrajectory().getQueueingProperties().getExecutionMode()), armTrajectoryCommand.getJointspaceTrajectory().getExecutionMode()); assertEquals(message.getJointspaceTrajectory().getJointTrajectoryMessages().size(), armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints()); for (int i = 0; i < message.getJointspaceTrajectory().getJointTrajectoryMessages().size(); i++) { int numberOfJointTrajectoryPoints = message.getJointspaceTrajectory().getJointTrajectoryMessages().get(i).getTrajectoryPoints().size(); OneDoFJointTrajectoryCommand jointTrajectoryPointList = armTrajectoryCommand.getJointspaceTrajectory().getJointTrajectoryPointList(i); assertEquals(numberOfJointTrajectoryPoints, jointTrajectoryPointList.getNumberOfTrajectoryPoints()); for (int j = 0; j < numberOfJointTrajectoryPoints; j++) { SimpleTrajectoryPoint1D trajectoryPoint = jointTrajectoryPointList.getTrajectoryPoint(j); TrajectoryPoint1DMessage jointTrajectoryPoint = message.getJointspaceTrajectory().getJointTrajectoryMessages().get(i).getTrajectoryPoints().get(j); assertEquals(jointTrajectoryPoint.getPosition(), trajectoryPoint.getPosition(), 1e-9); assertEquals(jointTrajectoryPoint.getVelocity(), trajectoryPoint.getVelocity(), 1e-9); assertEquals(jointTrajectoryPoint.getTime(), trajectoryPoint.getTime(), 1e-9); } } }
@Override public void clear() { robotSide = null; jointspaceTrajectory.clear(); }
@Override public boolean epsilonEquals(JointspaceTrajectoryCommand other, double epsilon) { if (this.jointTrajectoryInputs.size() != other.getTrajectoryPointLists().size()) { return false; } for (int i = 0; i < this.jointTrajectoryInputs.size(); i++) { if (!this.jointTrajectoryInputs.get(i).epsilonEquals(other.getTrajectoryPointLists().get(i), epsilon)) { return false; } } return super.epsilonEquals(other, epsilon); }
@Override public boolean epsilonEquals(SpineTrajectoryCommand other, double epsilon) { return jointspaceTrajectory.epsilonEquals(jointspaceTrajectory, epsilon); }
@Override public double getExecutionDelayTime() { return jointspaceTrajectory.getExecutionDelayTime(); }
@Override public void clear() { clearQueuableCommandVariables(); jointTrajectoryInputs.clear(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testClearRobotSide() { ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(); assertNull(armTrajectoryCommand.getRobotSide()); armTrajectoryCommand.clear(RobotSide.LEFT); assertEquals(RobotSide.LEFT, armTrajectoryCommand.getRobotSide()); assertEquals(Packet.VALID_MESSAGE_DEFAULT_ID, armTrajectoryCommand.getJointspaceTrajectory().getCommandId()); assertEquals(0.0, armTrajectoryCommand.getExecutionDelayTime(), 1e-9); assertEquals(ExecutionMode.OVERRIDE, armTrajectoryCommand.getJointspaceTrajectory().getExecutionMode()); assertEquals(0.0, armTrajectoryCommand.getExecutionTime(), 1e-9); assertEquals(0, armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints()); assertEquals(Packet.INVALID_MESSAGE_ID, armTrajectoryCommand.getJointspaceTrajectory().getPreviousCommandId()); assertEquals(0, armTrajectoryCommand.getJointspaceTrajectory().getTrajectoryPointLists().size()); }
@Override public void clear() { jointspaceTrajectory.clear(); }
@Override public boolean epsilonEquals(ArmTrajectoryCommand other, double epsilon) { if (robotSide != other.robotSide) return false; if (!jointspaceTrajectory.epsilonEquals(other.jointspaceTrajectory, epsilon)) return false; return true; }
@Override public double getExecutionDelayTime() { return jointspaceTrajectory.getExecutionDelayTime(); }
public void clear(RobotSide robotSide) { this.robotSide = robotSide; jointspaceTrajectory.clear(); }
public ArmTrajectoryCommand() { robotSide = null; jointspaceTrajectory = new JointspaceTrajectoryCommand(); }
@Override public boolean epsilonEquals(NeckTrajectoryCommand other, double epsilon) { return jointspaceTrajectory.epsilonEquals(other.jointspaceTrajectory, epsilon); }
@Override public double getExecutionDelayTime() { return jointspaceTrajectory.getExecutionDelayTime(); }
public JointspaceTrajectoryCommand() { clear(); }