private int queueExcedingTrajectoryPointsIfNeeded(int jointIndex, OneDoFJointTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); OneDoFJoint joint = controlledJoints[jointIndex]; MultipleWaypointsTrajectoryGenerator jointTrajectoryGenerator = jointTrajectoryGenerators.get(joint); int maximumNumberOfWaypoints = jointTrajectoryGenerator.getMaximumNumberOfWaypoints(); maximumNumberOfWaypoints -= jointTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; OneDoFJointTrajectoryCommand commandForExcedent = commandQueues.get(joint).addFirst(); numberOfQueuedCommands.get(joint).increment(); commandForExcedent.clear(); commandForExcedent.setCommandId(command.getCommandId()); for (int trajectoryPointIndex = maximumNumberOfWaypoints; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { commandForExcedent.addTrajectoryPoint(command.getTrajectoryPoint(trajectoryPointIndex)); } double timeOffsetToSubtract = command.getTrajectoryPoint(maximumNumberOfWaypoints - 1).getTime(); commandForExcedent.subtractTimeOffset(timeOffsetToSubtract); return maximumNumberOfWaypoints; }
@Override public boolean isCommandValid() { boolean numberOfTrajectoryPointsIsPositive = getNumberOfTrajectoryPoints() > 0; boolean weightIsValid = !Double.isInfinite(getWeight()); if (Double.isFinite(weight)) { weightIsValid &= weight >= 0; } return numberOfTrajectoryPointsIsPositive && weightIsValid; } }
public JointspaceTrajectoryCommand(Random random) { clear(); int degreesOfFreedom = random.nextInt(10) + 1; for(int i = 0; i < degreesOfFreedom; i++) { OneDoFJointTrajectoryCommand oneDoFJointTrajectoryCommand = new OneDoFJointTrajectoryCommand(random); jointTrajectoryInputs.add().set(oneDoFJointTrajectoryCommand); } }
OneDoFJointTrajectoryCommand otherJointTrajectoryPointList = otherArmTrajectoryCommand.getJointspaceTrajectory().getJointTrajectoryPointList(i); assertEquals(jointTrajectoryPointList.getExecutionDelayTime(), otherJointTrajectoryPointList.getExecutionDelayTime(), 1e-8); assertEquals(jointTrajectoryPointList.getExecutionTime(), otherJointTrajectoryPointList.getExecutionTime(), 1e-8); assertEquals(jointTrajectoryPointList.getMessageClass(), otherJointTrajectoryPointList.getMessageClass()); assertEquals(jointTrajectoryPointList.getNumberOfTrajectoryPoints(), otherJointTrajectoryPointList.getNumberOfTrajectoryPoints()); assertEquals(jointTrajectoryPointList.getTrajectoryTime(), otherJointTrajectoryPointList.getTrajectoryTime(), 1e-8); assertEquals(jointTrajectoryPointList.getWeight(), otherJointTrajectoryPointList.getWeight(), 1e-8); assertTrue(jointTrajectoryPointList.getLastTrajectoryPoint().epsilonEquals(otherJointTrajectoryPointList.getLastTrajectoryPoint(), 1e-8)); for (int j = 0; j < jointTrajectoryPointList.getNumberOfTrajectoryPoints(); j++) SimpleTrajectoryPoint1D trajectoryPoint = jointTrajectoryPointList.getTrajectoryPoint(j); SimpleTrajectoryPoint1D otherTrajectoryPoint = otherJointTrajectoryPointList.getTrajectoryPoint(j);
MultipleWaypointsTrajectoryGenerator trajectoryGenerator = jointTrajectoryGenerators.get(joint); jointTrajectoryCommand.addTimeOffset(firstWaypointTime); if (jointTrajectoryCommand.getNumberOfTrajectoryPoints() == 0) if (jointTrajectoryCommand.getTrajectoryPoint(0).getTime() > firstWaypointTime + 1.0e-5) trajectoryGenerator.appendWaypoint(jointTrajectoryCommand.getTrajectoryPoint(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); } } }
public SimpleTrajectoryPoint1D getJointTrajectoryPoint(int jointIndex, int trajectoryPointIndex) { return jointTrajectoryInputs.get(jointIndex).getTrajectoryPoint(trajectoryPointIndex); }
if (jointTrajectoryCommand.getTrajectoryPoint(0).getTime() < 1.0e-5) localCommand.set(jointTrajectoryCommand); localCommand.setCommandId(command.getCommandId());
@Override public void setFromMessage(OneDoFJointTrajectoryMessage message) { this.clear(); List<TrajectoryPoint1DMessage> trajectoryPointMessages = message.getTrajectoryPoints(); int numberOfPoints = trajectoryPointMessages.size(); for (int i = 0; i < numberOfPoints; i++) { TrajectoryPoint1DMessage trajectoryPoint1DMessage = trajectoryPointMessages.get(i); this.addTrajectoryPoint(trajectoryPoint1DMessage.getTime(), trajectoryPoint1DMessage.getPosition(), trajectoryPoint1DMessage.getVelocity()); } setWeight(message.getWeight()); }
@Override public boolean isCommandValid() { return getNumberOfTrajectoryPoints() > 0; } }
public void appendTrajectoryPoint(int jointIndex, SimpleTrajectoryPoint1D trajectoryPoint) { jointTrajectoryInputs.getAndGrowIfNeeded(jointIndex).addTrajectoryPoint(trajectoryPoint); }
private void set(int jointIndex, OneDoFJointTrajectoryCommand otherTrajectoryPointList) { OneDoFJointTrajectoryCommand thisJointTrajectoryPointList = jointTrajectoryInputs.getAndGrowIfNeeded(jointIndex); thisJointTrajectoryPointList.set(otherTrajectoryPointList); }
/** {@inheritDoc}} */ @Override public void addTimeOffset(double timeOffsetToAdd) { for (int i = 0; i < jointTrajectoryInputs.size(); i++) jointTrajectoryInputs.get(i).addTimeOffset(timeOffsetToAdd); }
public SimpleTrajectoryPoint1D getJointTrajectoryPoint(int jointIndex, int trajectoryPointIndex) { return jointTrajectoryInputs.get(jointIndex).getTrajectoryPoint(trajectoryPointIndex); }
public int getNumberOfJointTrajectoryPoints(int jointIndex) { return jointTrajectoryInputs.get(jointIndex).getNumberOfTrajectoryPoints(); }
public SimpleTrajectoryPoint1D getJointTrajectoryPoint(int jointIndex, int trajectoryPointIndex) { return jointTrajectoryInputs.get(jointIndex).getTrajectoryPoint(trajectoryPointIndex); }
public int getNumberOfJointTrajectoryPoints(int jointIndex) { return jointTrajectoryInputs.get(jointIndex).getNumberOfTrajectoryPoints(); }
public SimpleTrajectoryPoint1D getJointTrajectoryPoint(int jointIndex, int trajectoryPointIndex) { return jointTrajectoryInputs.get(jointIndex).getTrajectoryPoint(trajectoryPointIndex); }