@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); } } }