@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); } };
/** {@inheritDoc} */ @Override default void getJointConfiguration(RigidBodyTransform jointConfigurationToPack) { jointConfigurationToPack.setIdentity(); }
public void identity() { transform.setIdentity(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); transformToParent.setTranslation(originVector); }
protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); transformToParent.setTranslation(offset); } }
/** {@inheritDoc} */ @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { centerOfMassCalculator.reset(); transformToParent.setIdentity(); transformToParent.setTranslation(centerOfMassCalculator.getCenterOfMass()); } }
private static void computeRotationTransform(RigidBodyTransform transform3DToPack, double rotationAngle, Axis rotationAxis) { transform3DToPack.setIdentity(); switch(rotationAxis) { case X: { transform3DToPack.setRotationRollAndZeroTranslation(rotationAngle); break; } case Y: { transform3DToPack.setRotationPitchAndZeroTranslation(rotationAngle); break; } case Z: { transform3DToPack.setRotationYawAndZeroTranslation(rotationAngle); break; } default: { throw new RuntimeException("Shouldn't get here."); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetOrientationDistanceTrivial() { RigidBodyTransform transform1 = new RigidBodyTransform(); transform1.setIdentity(); FramePose3D framePose1 = new FramePose3D(ReferenceFrame.getWorldFrame(), transform1); RigidBodyTransform transform2 = new RigidBodyTransform(); transform2.setIdentity(); FramePose3D framePose2 = new FramePose3D(ReferenceFrame.getWorldFrame(), transform2); double distance = framePose1.getOrientationDistance(framePose2); assertEquals(0.0, distance, 1e-9); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeInterpolationOne() throws Exception { RigidBodyTransform t1 = new RigidBodyTransform(); t1.setIdentity(); RigidBodyTransform t2 = new RigidBodyTransform(); t2.setIdentity(); RigidBodyTransform t3 = new RigidBodyTransform(); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, 0.0); assertTrue(t1.equals(t3)); transformInterpolationCalculator.computeInterpolation(t1, t2, t3, 1.0); assertTrue(t2.equals(t3)); }
referenceFrame.transformToRoot.setIdentity();
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { poseOne.setToZero(frameOne); poseTwo.setToZero(frameTwo); poseOne.changeFrame(worldFrame); poseTwo.changeFrame(worldFrame); framePose.interpolate(poseOne, poseTwo, 0.5); transformToParent.setIdentity(); transformToParent.setRotationYawAndZeroTranslation(framePose.getYaw()); transformToParent.setTranslation(framePose.getPosition()); transformToParent.setTranslationZ(Math.min(poseOne.getZ(), poseTwo.getZ())); } }
public static void packExtrapolatedTransform(RigidBodyTransform from, RigidBodyTransform to, double ratio, RigidBodyTransform toPack) { Point3D pointToPack = new Point3D(); RotationMatrix orientationToPack = new RotationMatrix(); packExtrapolatedPoint(from.getTranslationVector(), to.getTranslationVector(), ratio, pointToPack); packExtrapolatedOrienation(from.getRotationMatrix(), to.getRotationMatrix(), ratio, orientationToPack); toPack.setIdentity(); toPack.setTranslation(pointToPack); toPack.setRotation(orientationToPack); }
private static void packRigidBodyTransformOnManifold(ReachingManifoldCommand reachingManifoldCommand, TDoubleArrayList configurations, RigidBodyTransform rigidBodyTransformToPack) { int dimensionOfManifold = reachingManifoldCommand.getDimensionOfManifold(); if (dimensionOfManifold != configurations.size()) throw new MismatchedSizeException("configuration space size and name size are not matched."); rigidBodyTransformToPack.setIdentity(); rigidBodyTransformToPack.setTranslation(reachingManifoldCommand.getManifoldOriginPosition()); rigidBodyTransformToPack.setRotation(reachingManifoldCommand.getManifoldOriginOrientation()); for (int i = 0; i < configurations.size(); i++) { RigidBodyTransform appendingTransform = ExploringRigidBodyTools.getLocalRigidBodyTransform(reachingManifoldCommand.getDegreeOfManifold(i), configurations.get(i)); rigidBodyTransformToPack.multiply(appendingTransform); } } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { //this fixes a threading issue. Generating garbage on purpose. RigidBodyTransform tempTransform = new RigidBodyTransform(); screenPosition.changeFrame(pixelsFrame); tempTransform.setIdentity(); tempTranslation.set(screenPosition.getX() + getPlotterWidthPixels() / 2.0, screenPosition.getY() - getPlotterHeightPixels() / 2.0, 0.0); tempTransform.appendTranslation(tempTranslation); tempTransform.appendYawRotation(screenRotation); tempTranslation.set(-getPlotterWidthPixels() / 2.0, getPlotterHeightPixels() / 2.0, 0.0); tempTransform.appendTranslation(tempTranslation); tempTransform.appendPitchRotation(Math.PI); tempTransform.appendYawRotation(Math.PI); transformToParent.set(tempTransform); } };
transform3D.setIdentity();
transform.setIdentity(); assertFalse(transform.hasRotation()); assertFalse(transform.hasTranslation());
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSingleRigidBodyRotation() { Random random = new Random(1766L); RigidBodyBasics elevator = new RigidBody("elevator", world); Vector3D jointAxis = RandomGeometry.nextVector3D(random); jointAxis.normalize(); RigidBodyTransform transformToParent = new RigidBodyTransform(); transformToParent.setIdentity(); RevoluteJoint joint = new RevoluteJoint("joint", elevator, transformToParent, jointAxis); RigidBodyBasics body = new RigidBody("body", joint, RandomGeometry.nextDiagonalMatrix3D(random), random.nextDouble(), new Vector3D()); joint.setQ(random.nextDouble()); joint.setQd(random.nextDouble()); Momentum momentum = computeMomentum(elevator, world); momentum.changeFrame(world); FrameVector3D linearMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getLinearPart())); FrameVector3D angularMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getAngularPart())); FrameVector3D linearMomentumCheck = new FrameVector3D(world); Matrix3D inertia = new Matrix3D(body.getInertia().getMomentOfInertia()); Vector3D angularMomentumCheckVector = new Vector3D(jointAxis); angularMomentumCheckVector.scale(joint.getQd()); inertia.transform(angularMomentumCheckVector); FrameVector3D angularMomentumCheck = new FrameVector3D(body.getInertia().getReferenceFrame(), angularMomentumCheckVector); angularMomentumCheck.changeFrame(world); double epsilon = 1e-9; EuclidCoreTestTools.assertTuple3DEquals(linearMomentumCheck, linearMomentum, epsilon); EuclidCoreTestTools.assertTuple3DEquals(angularMomentumCheck, angularMomentum, epsilon); assertTrue(angularMomentum.length() > epsilon); }
TDoubleArrayList optimalSolution = solver.getOptimalInput(); rigidBodyTransformToPack.setIdentity(); rigidBodyTransformToPack.appendTranslation(reachingManifoldCommand.getManifoldOriginPosition()); rigidBodyTransformToPack.setRotation(reachingManifoldCommand.getManifoldOriginOrientation());
RigidBodyTransform pelvisTransformWithOffset = new RigidBodyTransform(); pelvisTransformWithOffset.setIdentity(); pelvisTransformWithOffset.multiply(pelvisTransformInPast_Translation); pelvisTransformWithOffset.multiply(offsetTranslationTransform);