public CubicPolynomialTrajectoryGenerator(String namePrefix, DoubleProvider initialPositionProvider, DoubleProvider finalPositionProvider, DoubleProvider trajectoryTimeProvider, YoVariableRegistry parentRegistry) { this(namePrefix, initialPositionProvider, new ConstantDoubleProvider(0.0), finalPositionProvider, new ConstantDoubleProvider(0.0), trajectoryTimeProvider, parentRegistry); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void test() { Random random = new Random(); double expectedValue = RandomNumbers.nextDouble(random, Double.MIN_VALUE, Double.MAX_VALUE); ConstantDoubleProvider constantDoubleProvider = new ConstantDoubleProvider(expectedValue); double actualValue = constantDoubleProvider.getValue(); assertEquals(expectedValue, actualValue, EPS); }
public CubicPolynomialTrajectoryGenerator(String namePrefix, DoubleProvider initialPositionProvider, DoubleProvider finalPositionProvider, DoubleProvider trajectoryTimeProvider, YoVariableRegistry parentRegistry) { this(namePrefix, initialPositionProvider, new ConstantDoubleProvider(0.0), finalPositionProvider, new ConstantDoubleProvider(0.0), trajectoryTimeProvider, parentRegistry); }
@Before public void setUp() { initialPositionProvider = new ConstantDoubleProvider(0.0); trajectoryTimeProvider = new ConstantDoubleProvider(timeRequired); initialVelocityProvider = new ConstantDoubleProvider(INITIALVELOCITY); }
private CirclePositionTrajectoryGenerator createTrajectory(ReferenceFrame referenceFrame, FramePoint3D offset, double rotationAngle, double trajectoryTime) { PositionProvider initialPositionProvider = new ConstantPositionProvider(offset); DoubleProvider desiredRotationAngleProvider = new ConstantDoubleProvider(rotationAngle); YoVariableRegistry parentRegistry = new YoVariableRegistry("CirclePositionTrajectoryGeneratorTest"); return new CirclePositionTrajectoryGenerator("", referenceFrame, new ConstantDoubleProvider(trajectoryTime), initialPositionProvider, parentRegistry, desiredRotationAngleProvider); } }
private DoubleTrajectoryGenerator createConstantAccelerationTrajectoryCrossingZeroAtCrossTime(Random rand, double crossTime, double trajectoryTime, boolean positiveVelocity) { double initialPositionSign = positiveVelocity ? -1.0 : 1.0; DoubleProvider initialPositionProvider = new ConstantDoubleProvider(initialPositionSign * RandomNumbers.nextDouble(rand, 0.1, 1000.0)); DoubleProvider velocityProvider = new ConstantDoubleProvider(-initialPositionProvider.getValue() / crossTime); DoubleProvider trajectoryTimeProvider = new ConstantDoubleProvider(trajectoryTime); YoVariableRegistry registry = new YoVariableRegistry("osenroi"); ConstantVelocityTrajectoryGenerator trajectory = new ConstantVelocityTrajectoryGenerator("hihi", initialPositionProvider, velocityProvider, trajectoryTimeProvider, registry); trajectory.initialize(); return trajectory; } }
@Before public void setUp() { joint.setQ(0.0); joint.setQd(0.0); joint.setQdd(0.0); trajectoryTimeProvider = new ConstantDoubleProvider(timeRequired); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testConstructor() { finalPositionProvider = new ConstantDoubleProvider(CONSTANT * timeRequired * timeRequired + INITIALVELOCITY * timeRequired); generator = null; try { generator = new ConstantAccelerationTrajectoryGenerator(namePrefix, initialPositionProvider, finalPositionProvider, initialVelocityProvider, trajectoryTimeProvider, null); fail(); } catch (NullPointerException npe) { } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testSimpleTrajectory() { DoubleProvider initialPosition = new ConstantDoubleProvider(0.0); DoubleProvider velocity = new ConstantDoubleProvider(1.0); DoubleProvider trajectoryTimeProvider = new ConstantDoubleProvider(5.0); ConstantVelocityTrajectoryGenerator simpleLinearTrajectory = new ConstantVelocityTrajectoryGenerator("", initialPosition, velocity, trajectoryTimeProvider, new YoVariableRegistry("")); simpleLinearTrajectory.initialize(); simpleLinearTrajectory.compute(2.5); assertTrue(simpleLinearTrajectory.getValue() == 2.5); assertTrue(simpleLinearTrajectory.getVelocity() == 1.0); assertTrue(simpleLinearTrajectory.getAcceleration() == 0.0); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testRandomTrajectories() { final double numberOfIterations = 100; final double minimumTrajectoryTime = 1e-3; Random random = new Random(499300L); for(int i = 0; i < numberOfIterations; i++) { DoubleProvider initialPosition = new ConstantDoubleProvider(2 * random.nextDouble() - 1); DoubleProvider velocity = new ConstantDoubleProvider(2 * random.nextDouble() - 1); DoubleProvider trajectoryTime = new ConstantDoubleProvider(random.nextDouble() + minimumTrajectoryTime); constantVelocityTrajectoryGenerator = new ConstantVelocityTrajectoryGenerator("", initialPosition, velocity, trajectoryTime, new YoVariableRegistry("")); constantVelocityTrajectoryGenerator.initialize(); double randomTimeDuringTrajectory = trajectoryTime.getValue() * random.nextDouble(); double expectedPosition = initialPosition.getValue() + velocity.getValue() * randomTimeDuringTrajectory; checkTrajectoryPositionVelocityAndAcceleration(randomTimeDuringTrajectory, expectedPosition, velocity.getValue()); assertFalse(constantVelocityTrajectoryGenerator.isDone()); double randomTimeBeforeTrajectory = - random.nextDouble(); constantVelocityTrajectoryGenerator.compute(randomTimeBeforeTrajectory); assertFalse(constantVelocityTrajectoryGenerator.isDone()); double randomTimeAfterTrajectory = trajectoryTime.getValue() + random.nextDouble() + epsilon; constantVelocityTrajectoryGenerator.compute(randomTimeAfterTrajectory); assertTrue(constantVelocityTrajectoryGenerator.isDone()); } }
@Before public void setUp() { parentRegistry = new YoVariableRegistry("parentRegistryTEST"); referenceFrame = ReferenceFrame.constructARootFrame("rootNameTEST"); position = new FramePoint3D(referenceFrame, xValue, yValue, zValue); initialPositionProvider = new ConstantPositionProvider(position); finalPositionProvider = new ConstantPositionProvider(position); trajectoryTimeProvider = new ConstantDoubleProvider(10.0); }
trajectoryTimeProvider = new ConstantDoubleProvider(diagnosticParameters.getInitialJointSplineDuration()); submitDiagnostic(new WaitDiagnosticTask(trajectoryTimeProvider.getValue()));
@Before public void setUp() { worldFrame = ReferenceFrame.getWorldFrame(); registry = new YoVariableRegistry("reg"); FramePose3D initialPose = EuclidFrameRandomTools.nextFramePose3D(random, worldFrame, 1.0, 1.0, 1.0); FrameQuaternion initialOrientation = new FrameQuaternion(initialPose.getOrientation()); initialOrientationProvider = new ConstantOrientationProvider(initialOrientation); trajectoryTimeProvider = new ConstantDoubleProvider(1.0); trajectoryGenerator = new CirclePoseTrajectoryGenerator("test", worldFrame, trajectoryTimeProvider, registry, null); trajectoryGenerator.setDesiredRotationAngle(Math.PI); trajectoryGenerator.setInitialPose(initialPose); trajectoryGenerator.initialize(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testIsDone() { finalPositionProvider = new ConstantDoubleProvider(CONSTANT * timeRequired * timeRequired + INITIALVELOCITY * timeRequired); generator = new ConstantAccelerationTrajectoryGenerator(namePrefix, initialPositionProvider, finalPositionProvider, initialVelocityProvider, trajectoryTimeProvider, parentRegistry); generator.initialize(); generator.compute(trajectoryTimeProvider.getValue()); assertFalse(generator.isDone()); generator.compute(trajectoryTimeProvider.getValue() + EPSILON); assertTrue(generator.isDone()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) //s = -c * t ^ 2 + v0 * t public void testDecreasing() { finalPositionProvider = new ConstantDoubleProvider(-CONSTANT * timeRequired * timeRequired + INITIALVELOCITY * timeRequired); generator = new ConstantAccelerationTrajectoryGenerator(namePrefix, initialPositionProvider, finalPositionProvider, initialVelocityProvider, trajectoryTimeProvider, parentRegistry); generator.initialize(); for (double t = 0; t < timeRequired; t += DT) { generator.compute(t); assertEquals(-CONSTANT * t * t + INITIALVELOCITY * t, generator.getValue(), EPSILON); assertEquals(-2.0 * CONSTANT * t + INITIALVELOCITY, generator.getVelocity(), EPSILON); assertEquals(-2.0 * CONSTANT, generator.getAcceleration(), EPSILON); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) //s = c * t ^ 2 + v0 * t public void testIncreasing() { finalPositionProvider = new ConstantDoubleProvider(CONSTANT * timeRequired * timeRequired + INITIALVELOCITY * timeRequired); generator = new ConstantAccelerationTrajectoryGenerator(namePrefix, initialPositionProvider, finalPositionProvider, initialVelocityProvider, trajectoryTimeProvider, parentRegistry); generator.initialize(); for (double t = 0; t < timeRequired; t += DT) { generator.compute(t); assertEquals(CONSTANT * t * t + INITIALVELOCITY * t, generator.getValue(), EPSILON); assertEquals(2.0 * CONSTANT * t + INITIALVELOCITY, generator.getVelocity(), EPSILON); assertEquals(2.0 * CONSTANT, generator.getAcceleration(), EPSILON); } }
trajectoryTimeProvider.set(trajectoryTime); DoubleProvider initialPositionProvider = new ConstantDoubleProvider(0.0); DoubleProvider finalPositionProvider = new ConstantDoubleProvider(1.0);
trajectoryTimeProvider = new ConstantDoubleProvider(diagnosticParameters.getInitialJointSplineDuration()); submitDiagnostic(new WaitDiagnosticTask(trajectoryTimeProvider.getValue()));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testTooBigTime() { YoVariableRegistry registry = new YoVariableRegistry("youpiloup"); SimpleOrientationTrajectoryGenerator trajToTest = new SimpleOrientationTrajectoryGenerator("blop", worldFrame, registry); DoubleProvider trajectoryTimeProvider = new ConstantDoubleProvider(10.0); FrameQuaternion initialOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, worldFrame); FrameQuaternion finalOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, worldFrame); trajToTest.setInitialOrientation(initialOrientation); trajToTest.setFinalOrientation(finalOrientation); trajToTest.setTrajectoryTime(trajectoryTimeProvider.getValue()); trajToTest.initialize(); trajToTest.compute(15.0); FrameQuaternion orientation1 = new FrameQuaternion(finalOrientation); FrameVector3D angularVelocity1 = new FrameVector3D(worldFrame); FrameVector3D angularAcceleration1 = new FrameVector3D(worldFrame); FrameQuaternion orientation2 = new FrameQuaternion(); FrameVector3D angularVelocity2 = new FrameVector3D(); FrameVector3D angularAcceleration2 = new FrameVector3D(); trajToTest.getAngularData(orientation2, angularVelocity2, angularAcceleration2); assertTrue(orientation1.epsilonEquals(orientation2, EPSILON)); assertTrue(angularVelocity1.epsilonEquals(angularVelocity2, EPSILON)); assertTrue(angularAcceleration1.epsilonEquals(angularAcceleration2, EPSILON)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testNegativeTime() { YoVariableRegistry registry = new YoVariableRegistry("youpiloup"); SimpleOrientationTrajectoryGenerator trajToTest = new SimpleOrientationTrajectoryGenerator("blop", worldFrame, registry); DoubleProvider trajectoryTimeProvider = new ConstantDoubleProvider(10.0); FrameQuaternion initialOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, worldFrame); FrameQuaternion finalOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, worldFrame); trajToTest.setInitialOrientation(initialOrientation); trajToTest.setFinalOrientation(finalOrientation); trajToTest.setTrajectoryTime(trajectoryTimeProvider.getValue()); trajToTest.initialize(); trajToTest.compute(-5.0); FrameQuaternion orientation1 = new FrameQuaternion(initialOrientation); FrameVector3D angularVelocity1 = new FrameVector3D(worldFrame); FrameVector3D angularAcceleration1 = new FrameVector3D(worldFrame); FrameQuaternion orientation2 = new FrameQuaternion(); FrameVector3D angularVelocity2 = new FrameVector3D(); FrameVector3D angularAcceleration2 = new FrameVector3D(); trajToTest.getAngularData(orientation2, angularVelocity2, angularAcceleration2); assertTrue(orientation1.epsilonEquals(orientation2, EPSILON)); assertTrue(angularVelocity1.epsilonEquals(angularVelocity2, EPSILON)); assertTrue(angularAcceleration1.epsilonEquals(angularAcceleration2, EPSILON)); }