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