public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis, boolean isPartOfClosedKinematicLoop) { return addPassiveRevoluteJoint(jointName, parentBody, TransformTools.createTranslationTransform(jointOffset), jointAxis, isPartOfClosedKinematicLoop); }
public void rotate(double angle, Axis axis) { TransformTools.rotate(transform, angle, axis); }
/** This will get the magnitude of the transform between two transforms that are relative to the same frame. * @param rigidBodyTransform1 with respect to frame B * @param rigidBodyTransform2 with respect to frame B * @param radiusForScalingRotation * @return */ public static double getSizeOfTransformBetweenTwoWithRotationScaled(RigidBodyTransform rigidBodyTransform1, RigidBodyTransform rigidBodyTransform2, double radiusForScalingRotation) { RigidBodyTransform temp = getTransformFromA2toA1(rigidBodyTransform1, rigidBodyTransform2); return getMagnitudeOfTranslation(temp) + radiusForScalingRotation * getMagnitudeOfAngleOfRotation(temp); } }
public static double getSizeOfTransformWithRotationScaled(RigidBodyTransform rigidBodyTransform, double radiusForScalingRotation) { return getMagnitudeOfTranslation(rigidBodyTransform) + radiusForScalingRotation * getMagnitudeOfAngleOfRotation(rigidBodyTransform); }
@Override public EuclideanTrajectoryPointMessage transform(RigidBodyTransform transform) { EuclideanTrajectoryPointMessage transformedTrajectoryPointMessage = new EuclideanTrajectoryPointMessage(); transformedTrajectoryPointMessage.time = time; if (position != null) transformedTrajectoryPointMessage.position = TransformTools.getTransformedPoint(position, transform); else transformedTrajectoryPointMessage.position = null; if (linearVelocity != null) transformedTrajectoryPointMessage.linearVelocity = TransformTools.getTransformedVector(linearVelocity, transform); else transformedTrajectoryPointMessage.linearVelocity = null; return transformedTrajectoryPointMessage; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testgetTransformDifference() { RigidBodyTransform rigidBodyTransform = new RigidBodyTransform(); Random random = new Random(); int numberOfTests = 1000; for (int i = 0; i < numberOfTests; i++) { double angle = -Math.PI + 2 * Math.PI * random.nextDouble(); Vector3D vector3d = new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); vector3d.normalize(); AxisAngle axisAngle = new AxisAngle(vector3d, angle); double vectorScale = 10.0; Vector3D vector3dTranlation = new Vector3D(-vectorScale + 2.0 * vectorScale * random.nextDouble(), -vectorScale + 2.0 * vectorScale * random.nextDouble(), -vectorScale + 2.0 * vectorScale * random.nextDouble()); rigidBodyTransform.set(axisAngle, vector3dTranlation); double angleFromTransform = TransformTools.getMagnitudeOfAngleOfRotation(rigidBodyTransform); assertEquals(Math.abs(angle), angleFromTransform, 1e-9); double translation = TransformTools.getMagnitudeOfTranslation(rigidBodyTransform); assertEquals(vector3dTranlation.length(), translation, 1e-9); double radiusOfRotation = random.nextDouble(); double magnitudeOfTransform = TransformTools.getSizeOfTransformWithRotationScaled(rigidBodyTransform, radiusOfRotation); assertEquals(vector3dTranlation.length() + Math.abs(angle) * radiusOfRotation, magnitudeOfTransform, 1e-9); } }
public FootstepDataMessage transform(RigidBodyTransform transform) { FootstepDataMessage ret = this.clone(); // Point3d location; ret.location = TransformTools.getTransformedPoint(this.getLocation(), transform); // Quat4d orientation; ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); // Waypoints if they exist: if (trajectoryWaypoints != null) { for (int i = 0; i < trajectoryWaypoints.length; i++) ret.trajectoryWaypoints[i] = TransformTools.getTransformedPoint(trajectoryWaypoints[i], transform); } return ret; }
public double getOrientationDistance(FramePose2d framePose) { checkReferenceFrameMatch(framePose); RigidBodyTransform transformThis = new RigidBodyTransform(); this.getPose(transformThis); RigidBodyTransform transformThat = new RigidBodyTransform(); framePose.getPose(transformThat); return TransformTools.getMagnitudeOfAngleOfRotation(TransformTools.getTransformFromA2toA1(transformThis, transformThat)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testgetTransformDifferenceBetweenTwoTransforms() { Random random = new Random(); int numberOfTests = 1000; for (int i = 0; i < numberOfTests; i++) { DenseMatrix64F matrix = new DenseMatrix64F(4, 4); this.createRandomTransformationMatrix(matrix, random); RigidBodyTransform transformFromWorldToA1 = new RigidBodyTransform(matrix); this.createRandomTransformationMatrix(matrix, random); RigidBodyTransform transformFromWorldToA2 = new RigidBodyTransform(matrix); double radiusOfRotation = random.nextDouble(); double magnitudeOfTransform1 = TransformTools.getSizeOfTransformBetweenTwoWithRotationScaled(transformFromWorldToA1, transformFromWorldToA2, radiusOfRotation); RigidBodyTransform transformA2toA1 = TransformTools.getTransformFromA2toA1(transformFromWorldToA1, transformFromWorldToA2); double magnitudeOfTransform2 = TransformTools.getSizeOfTransformWithRotationScaled(transformA2toA1, radiusOfRotation); assertEquals(magnitudeOfTransform1, magnitudeOfTransform2, 1e-9); } }
public static RigidBodyTransform createTransformFromTranslationAndEulerAngles(double x, double y, double z, double roll, double pitch, double yaw) { return createTransformFromTranslationAndEulerAngles(new Vector3d(x, y, z), new Vector3d(roll, pitch, yaw)); }
public RigidBodyTransform getTransformFromThisToThat(FramePose thatPose) { checkReferenceFrameMatch(thatPose); RigidBodyTransform transformToThis = new RigidBodyTransform(); this.getPose(transformToThis); RigidBodyTransform transformToThat = new RigidBodyTransform(); thatPose.getPose(transformToThat); return TransformTools.getTransformFromA2toA1(transformToThat, transformToThis); }
public HandstepPacket transform(RigidBodyTransform transform) { HandstepPacket ret = new HandstepPacket(); // String rigidBodyName; ret.robotSide = this.getRobotSide(); // Point3d location; ret.location = TransformTools.getTransformedPoint(this.getLocation(), transform); // Quat4d orientation; ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); // Vector3d surface normal ret.surfaceNormal = TransformTools.getTransformedVector(this.getSurfaceNormal(), transform); ret.swingTrajectoryTime = this.getSwingTrajectoryTime(); return ret; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRotate() { RigidBodyTransform transform = new RigidBodyTransform(); TransformTools.appendRotation(transform, Math.PI / 4, Axis.X); RigidBodyTransform transform2 = new RigidBodyTransform(); transform2.setRotationRollAndZeroTranslation(Math.PI / 4); EuclidCoreTestTools.assertRigidBodyTransformEquals(transform, transform2, 1e-7); transform = new RigidBodyTransform(); TransformTools.appendRotation(transform, 3 * Math.PI / 4, Axis.Y); transform2 = new RigidBodyTransform(); transform2.setRotationPitchAndZeroTranslation(3 * Math.PI / 4); EuclidCoreTestTools.assertRigidBodyTransformEquals(transform, transform2, 1e-7); transform = new RigidBodyTransform(); TransformTools.appendRotation(transform, -Math.PI / 2, Axis.Z); transform2 = new RigidBodyTransform(); transform2.setRotationYawAndZeroTranslation(-Math.PI / 2); EuclidCoreTestTools.assertRigidBodyTransformEquals(transform, transform2, 1e-7); }
public static void main(String[] args) { RigidBodyTransform t = new RigidBodyTransform(); t.setTranslationAndIdentityRotation(new Vector3d(4.5, 6.6, 22)); System.out.println(getTransformedPoint(new Point3d(0.0, 1.0, 4.6), t)); }
public double getEffectiveDistanceToFramePose(FramePose framePose, double radiusOfRotation) { checkReferenceFrameMatch(framePose); RigidBodyTransform transformThis = new RigidBodyTransform(); this.getPose(transformThis); transformThis.invert(); RigidBodyTransform transformThat = new RigidBodyTransform(); framePose.getPose(transformThat); transformThat.invert(); return TransformTools.getSizeOfTransformBetweenTwoWithRotationScaled(transformThis, transformThat, radiusOfRotation); }
public static double getSizeOfTransformWithRotationScaled(RigidBodyTransform rigidBodyTransform, double radiusForScalingRotation) { return getMagnitudeOfTranslation(rigidBodyTransform) + radiusForScalingRotation * getMagnitudeOfAngleOfRotation(rigidBodyTransform); }
public TorusPosePacket transform(RigidBodyTransform transform) { TorusPosePacket ret = new TorusPosePacket(); ret.fingerHoleRadius = this.fingerHoleRadius; ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); ret.position = TransformTools.getTransformedPoint(this.getPosition(), transform); return ret; }
public static RigidBodyTransform createTransformFromTranslationAndEulerAngles(double x, double y, double z, double roll, double pitch, double yaw) { return createTransformFromTranslationAndEulerAngles(new Vector3D(x, y, z), new Vector3D(roll, pitch, yaw)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetTransformFromA1toA2Random() { DenseMatrix64F matrix = new DenseMatrix64F(4, 4); Random random = new Random(111L); this.createRandomTransformationMatrix(matrix, random); RigidBodyTransform transformFromWorldToA1 = new RigidBodyTransform(matrix); this.createRandomTransformationMatrix(matrix, random); RigidBodyTransform transformFromWorldToA2 = new RigidBodyTransform(matrix); ReferenceFrame a1 = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("a1", ReferenceFrame.getWorldFrame(), transformFromWorldToA1); ReferenceFrame a2 = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("a2", ReferenceFrame.getWorldFrame(), transformFromWorldToA2); RigidBodyTransform transformA2toA1 = TransformTools.getTransformFromA2toA1(transformFromWorldToA1, transformFromWorldToA2); Point3D a2Origin = new Point3D(); transformA2toA1.transform(a2Origin); // System.out.println("a2Origin after transform" + a2Origin); FramePoint3D a2OriginFramePoint = new FramePoint3D(a2); a2OriginFramePoint.changeFrame(a1); // System.out.println("a2OriginFramePoint = " + a2OriginFramePoint); a2Origin.epsilonEquals(a2OriginFramePoint, 1e-9); }
public void addVerticalDebrisLeaningAgainstAWall(double xRelativeToRobot, double yRelativeToRobot, double debrisYaw, double debrisPitch) { Point3D tempPosition = new Point3D(xRelativeToRobot, yRelativeToRobot, debrisLength / 2.0 * Math.cos(debrisPitch)); FramePose3D debrisPose = generateDebrisPose(tempPosition, debrisYaw, debrisPitch, 0.0); debrisRobots.add(createDebrisRobot(debrisPose)); double supportWidth = 0.1; double supportLength = 0.2; double supportHeight = 1.05*debrisLength; RigidBodyTransform debrisTransform = new RigidBodyTransform(); debrisPose.get(debrisTransform ); TransformTools.appendRotation(debrisTransform, -debrisPitch, Axis.Y); debrisPose.set(debrisTransform); debrisPose.setZ(0.0); PoseReferenceFrame debrisReferenceFrame = new PoseReferenceFrame("debrisReferenceFrame", debrisPose); FramePose3D supportPose = new FramePose3D(debrisReferenceFrame); double x = supportWidth / 2.0 + debrisLength/2.0 * Math.sin(debrisPitch); double y = 0.0; double z = supportHeight / 2.0; supportPose.setPosition(x, y, z); supportPose.changeFrame(constructionWorldFrame); RigidBodyTransform supportTransform = new RigidBodyTransform(); supportPose.get(supportTransform); combinedTerrainObject.addRotatableBox(supportTransform, supportWidth, supportLength, supportHeight, YoAppearance.AliceBlue()); }