@Override public void set(ArmTrajectoryCommand other) { clear(other.getRobotSide()); jointspaceTrajectory.set(other.getJointspaceTrajectory()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testIsCommandValid() { ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(new Random()); assertTrue(armTrajectoryCommand.isCommandValid()); armTrajectoryCommand.clear(); assertFalse(armTrajectoryCommand.isCommandValid()); }
@Override public boolean isCommandValid() { return robotSide != null && executionMode != null && getNumberOfJoints() > 0; } }
@Override public void set(ArmTrajectoryCommand other) { set(other.robotSide, other.getTrajectoryPointLists()); commandId = other.commandId; executionMode = other.executionMode; previousCommandId = other.previousCommandId; }
@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); }
public void testSetArmTrajectoryCommand() ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(new Random()); ArmTrajectoryCommand otherArmTrajectoryCommand = new ArmTrajectoryCommand(); armTrajectoryCommand.set(otherArmTrajectoryCommand); 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);
@Override public void clear() { clear(null); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetRobotSide() { ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(); assertNull(armTrajectoryCommand.getRobotSide()); armTrajectoryCommand.setRobotSide(RobotSide.LEFT); assertEquals(RobotSide.LEFT, armTrajectoryCommand.getRobotSide()); }
@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); } } }
public boolean handleArmTrajectoryCommand(ArmTrajectoryCommand command, boolean initializeToCurrent) { if (!ControllerCommandValidationTools.checkArmTrajectoryCommand(controlledJoints, command)) return false; isReadyToHandleQueuedCommands.set(true); clearCommandQueues(command.getCommandId()); RecyclingArrayList<OneDoFJointTrajectoryCommand> jointTrajectoryCommands = command.getTrajectoryPointLists(); int numberOfJoints = command.getNumberOfJoints(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointTrajectoryCommand jointTrajectoryCommand = jointTrajectoryCommands.get(jointIndex); initializeTrajectoryGenerator(initializeToCurrent, 0.0, jointIndex, jointTrajectoryCommand); } return true; }
long previousCommandId = command.getPreviousCommandId(); for (int jointIndex = 0; jointIndex < command.getNumberOfJoints(); jointIndex++) numberOfQueuedCommands.get(joint).increment(); OneDoFJointTrajectoryCommand jointTrajectoryCommand = command.getJointTrajectoryPointList(jointIndex); localCommand.setCommandId(command.getCommandId()); lastCommandId.set(command.getCommandId());
public void handleArmTrajectoryCommand(ArmTrajectoryCommand command) { boolean success; switch (command.getExecutionMode()) { case OVERRIDE: boolean initializeToCurrent = stateMachine.getCurrentStateEnum() != HandControlMode.JOINTSPACE; success = jointspaceControlState.handleArmTrajectoryCommand(command, initializeToCurrent); if (success) requestedState.set(jointspaceControlState.getStateEnum()); else PrintTools.warn(this, "Can't execute ArmTrajectoryCommand! " + command.getRobotSide()); return; case QUEUE: success = jointspaceControlState.queueArmTrajectoryCommand(command); if (!success) { holdPositionInJointspace(); PrintTools.warn(this, "Can't execute ArmTrajectoryCommand! " + command.getRobotSide()); } return; default: PrintTools.warn(this, "Unknown " + ExecutionMode.class.getSimpleName() + " value: " + command.getExecutionMode() + ". Command ignored."); return; } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetMessageClass() { ArmTrajectoryCommand otherArmTrajectoryCommand = new ArmTrajectoryCommand(); assertEquals(ArmTrajectoryMessage.class, otherArmTrajectoryCommand.getMessageClass()); }
public void handleArmTrajectoryCommands(List<ArmTrajectoryCommand> commands) { for (int i = 0; i < commands.size(); i++) { ArmTrajectoryCommand command = commands.get(i); RobotSide robotSide = command.getRobotSide(); handControlModules.get(robotSide).handleArmTrajectoryCommand(command); } }
@Override public void set(ArmTrajectoryMessage message) { set(message.getRobotSide(), message.getTrajectoryPointLists()); commandId = message.getUniqueId(); executionMode = message.getExecutionMode(); previousCommandId = message.getPreviousMessageId(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testArmTrajectoryCommand() { new ArmTrajectoryCommand(); }
public static boolean checkArmTrajectoryCommand(OneDoFJoint[] joints, ArmTrajectoryCommand command) { return checkOneDoFJointTrajectoryCommandList(joints, command.getTrajectoryPointLists()); }
@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()); }
public ArmTrajectoryCommand() { clear(); }