public ConstantPoseTrajectoryGenerator(String namePrefix, boolean allowMultipleFrames, ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry) { this.allowMultipleFrames = allowMultipleFrames; YoVariableRegistry registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames(namePrefix + "ConstantPosition", registry, referenceFrame); position = yoFramePointInMultipleFrames; YoFrameQuaternionInMultipleFrames yoFrameQuaternionInMultiplesFrames = new YoFrameQuaternionInMultipleFrames(namePrefix + "ConstantOrientation", registry, referenceFrame); orientation = yoFrameQuaternionInMultiplesFrames; multipleFramesHolders = new ArrayList<YoMultipleFramesHolder>(); multipleFramesHolders.add(yoFramePointInMultipleFrames); multipleFramesHolders.add(yoFrameQuaternionInMultiplesFrames); }
public ConstantPoseTrajectoryGenerator(String namePrefix, boolean allowMultipleFrames, ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry) { this.allowMultipleFrames = allowMultipleFrames; YoVariableRegistry registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames(namePrefix + "ConstantPosition", registry, referenceFrame); position = yoFramePointInMultipleFrames; YoFrameQuaternionInMultipleFrames yoFrameQuaternionInMultiplesFrames = new YoFrameQuaternionInMultipleFrames(namePrefix + "ConstantOrientation", registry, referenceFrame); orientation = yoFrameQuaternionInMultiplesFrames; multipleFramesHolders = new ArrayList<YoMultipleFramesHolder>(); multipleFramesHolders.add(yoFramePointInMultipleFrames); multipleFramesHolders.add(yoFrameQuaternionInMultiplesFrames); }
try new YoFramePointInMultipleFrames("blop1", registry, allFrames); new YoFramePointInMultipleFrames("blop2", registry); fail("Should have thrown an exception"); new YoFramePointInMultipleFrames("blop3", registry, new ReferenceFrame[]{}); fail("Should have thrown an exception");
YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, worldFrame);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testSetToZero() { YoVariableRegistry registry = new YoVariableRegistry("youhou"); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, allFrames); yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame); FramePoint3D framePoint = new FramePoint3D(worldFrame); framePoint.setToZero(worldFrame); assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10)); }
for (int i = 0; i < maximumNumberOfFootstepsToConsider - 1; i++) YoFramePointInMultipleFrames earlyCornerPoint = new YoFramePointInMultipleFrames(yoNamePrefix + "EntryCornerPoints" + i, registry, framesToRegister); entryCornerPoints.add(earlyCornerPoint); YoFramePointInMultipleFrames lateCornerPoint = new YoFramePointInMultipleFrames(yoNamePrefix + "ExitCornerPoints" + i, registry, framesToRegister); exitCornerPoints.add(lateCornerPoint);
@Before public void setUp() { RigidBodyTransform transformToParent = new RigidBodyTransform(); // referenceFrame = ReferenceFrame.constructARootFrame("rootFrame"); referenceFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("referenceFrame", rootFrame1, transformToParent); positionYoFramePoint = new YoFramePoint3D("prefixTEST", referenceFrame, registry); orientationQuaternion = new YoFrameQuaternion("orientationPrefix", referenceFrame, registry); frame2 = ReferenceFrame.constructFrameWithUnchangingTransformToParent("frame2", rootFrame1, transformToParent); positionMultipleFrames = new YoFramePointInMultipleFrames("positionMultipleFrames", registry, rootFrame1, frame2); orientationMultipleFrames = new YoFrameQuaternionInMultipleFrames("orientationMultipleFrames", registry, rootFrame1, frame2); generator = new ConstantPoseTrajectoryGenerator(positionYoFramePoint, orientationQuaternion); }
YoFramePointInMultipleFrames currentPosition = new YoFramePointInMultipleFrames(currentPositionName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentVelocity = new YoFrameVectorInMultipleFrames(currentVelocityName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentAcceleration = new YoFrameVectorInMultipleFrames(currentAccelerationName, registry, referenceFrame);
YoFramePointInMultipleFrames initialPosition = new YoFramePointInMultipleFrames(initialPositionName, registry, referenceFrame); YoFrameVectorInMultipleFrames initialVelocity = new YoFrameVectorInMultipleFrames(initialVelocityName, registry, referenceFrame); YoFramePointInMultipleFrames finalPosition = new YoFramePointInMultipleFrames(finalPositionName, registry, referenceFrame); YoFrameVectorInMultipleFrames finalVelocity = new YoFrameVectorInMultipleFrames(finalVelocityName, registry, referenceFrame); YoFramePointInMultipleFrames currentPosition = new YoFramePointInMultipleFrames(currentPositionName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentVelocity = new YoFrameVectorInMultipleFrames(currentVelocityName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentAcceleration = new YoFrameVectorInMultipleFrames(currentAccelerationName, registry, referenceFrame);
YoFramePointInMultipleFrames initialPosition = new YoFramePointInMultipleFrames(initialPositionName, registry, referenceFrame); YoFrameVectorInMultipleFrames initialVelocity = new YoFrameVectorInMultipleFrames(initialVelocityName, registry, referenceFrame); YoFramePointInMultipleFrames finalPosition = new YoFramePointInMultipleFrames(finalPositionName, registry, referenceFrame); YoFrameVectorInMultipleFrames finalVelocity = new YoFrameVectorInMultipleFrames(finalVelocityName, registry, referenceFrame); YoFramePointInMultipleFrames currentPosition = new YoFramePointInMultipleFrames(currentPositionName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentVelocity = new YoFrameVectorInMultipleFrames(currentVelocityName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentAcceleration = new YoFrameVectorInMultipleFrames(currentAccelerationName, registry, referenceFrame);
singleSupportInitialICP = new YoFramePointInMultipleFrames(namePrefix + "SingleSupportInitialICP", registry, framesToRegister); singleSupportFinalICP = new YoFramePointInMultipleFrames(namePrefix + "SingleSupportFinalICP", registry, framesToRegister); YoFramePointInMultipleFrames earlyCornerPoint = new YoFramePointInMultipleFrames(namePrefix + "EntryCornerPoints" + i, registry, framesToRegister); entryCornerPoints.add(earlyCornerPoint); YoFramePointInMultipleFrames lateCornerPoint = new YoFramePointInMultipleFrames(namePrefix + "ExitCornerPoints" + i, registry, framesToRegister); exitCornerPoints.add(lateCornerPoint);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testSetIncludingFrame() { YoVariableRegistry registry = new YoVariableRegistry("youhou"); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, new ReferenceFrame[]{worldFrame, frameA}); yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame); FramePoint3D framePoint = EuclidFrameRandomTools.nextFramePoint3D(random, frameA, -100.0, 100.0, -100.0, 100.0, -100.0, 100.0); yoFramePointInMultipleFrames.setIncludingFrame(framePoint); assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10)); try { yoFramePointInMultipleFrames.setIncludingFrame(new FramePoint3D(frameC)); fail("Should have thrown an exception"); } catch (Exception e) { // Good } } }
initialPosition = new YoFramePointInMultipleFrames(namePrefix + "InitialPosition", registry, referenceFrame, tangentialPlane); finalPosition = new YoFramePointInMultipleFrames(namePrefix + "FinalPosition", registry, referenceFrame, tangentialPlane); initialDirection = new YoFrameVectorInMultipleFrames(namePrefix + "InitialDirection", registry, referenceFrame, tangentialPlane); currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame, tangentialPlane); currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame, tangentialPlane); currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame, tangentialPlane);
initialPosition = new YoFramePointInMultipleFrames(namePrefix + "InitialPosition", registry, referenceFrame, tangentialPlane); finalPosition = new YoFramePointInMultipleFrames(namePrefix + "FinalPosition", registry, referenceFrame, tangentialPlane); finalDirection = new YoFrameVectorInMultipleFrames(namePrefix + "FinalDirection", registry, referenceFrame, tangentialPlane); currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame, tangentialPlane); currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame, tangentialPlane); currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame, tangentialPlane);
YoFramePointInMultipleFrames entryConstantCMP = new YoFramePointInMultipleFrames(namePrefix + "EntryCMP" + i, parentRegistry, framesToRegister); entryConstantCMP.setToNaN(); entryCMPs.add(entryConstantCMP); entryCMPsInWorldFrameReadOnly.add(entryConstantCMP.buildUpdatedYoFramePointForVisualizationOnly()); YoFramePointInMultipleFrames exitConstantCMP = new YoFramePointInMultipleFrames(namePrefix + "ExitCMP" + i, parentRegistry, framesToRegister); exitConstantCMP.setToNaN(); exitCMPs.add(exitConstantCMP);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testChangeToRegisteredFrame() { YoVariableRegistry registry = new YoVariableRegistry("youhou"); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, new ReferenceFrame[]{worldFrame, frameA}); yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame); FramePoint3D framePoint = new FramePoint3D(worldFrame); Point3D point = RandomGeometry.nextPoint3D(random, 100.0, 100.0, 100.0); yoFramePointInMultipleFrames.set(point); framePoint.set(point); yoFramePointInMultipleFrames.changeFrame(frameA); framePoint.changeFrame(frameA); assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10)); try { yoFramePointInMultipleFrames.changeFrame(frameB); fail("Should have thrown an exception"); } catch (Exception e) { // Good } }
initialPosition = new YoFramePointInMultipleFrames(namePrefix + "InitialPosition", registry, referenceFrame); finalPosition = new YoFramePointInMultipleFrames(namePrefix + "FinalPosition", registry, referenceFrame); currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame); currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame); currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame);
initialPosition = new YoFramePointInMultipleFrames(namePrefix + "InitialPosition", registry, referenceFrame, distortedPlane); finalPosition = new YoFramePointInMultipleFrames(namePrefix + "FinalPosition", registry, referenceFrame, distortedPlane); initialDirection = new YoFrameVectorInMultipleFrames(namePrefix + "InitialDirection", registry, referenceFrame, distortedPlane); finalDirection = new YoFrameVectorInMultipleFrames(namePrefix + "FinalDirection", registry, referenceFrame, distortedPlane); currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame, distortedPlane); currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame, distortedPlane); currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame, distortedPlane);
initialPosition = new YoFramePointInMultipleFrames(namePrefix + "InitialPosition", registry, referenceFrame); initialVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "InitialVelocity", registry, referenceFrame); finalPosition = new YoFramePointInMultipleFrames(namePrefix + "FinalPosition", registry, referenceFrame); finalVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "FinalVelocity", registry, referenceFrame); finalPositionForViz = new YoFramePoint(namePrefix + "FinalPositionForViz", worldFrame, registry); finalOrientationForViz = new YoFrameOrientation(namePrefix + "FinalOrientationForViz", worldFrame, registry); currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame); currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame); currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame);