/** * Creates a new reference frame such that it is centered at the given {@code point} and with its * z-axis aligned with the given {@code zAxis} vector. * <p> * Note that the parent frame is set to the reference frame the given {@code point} and * {@code zAxis} are expressed in. * </p> * * @param frameName the name of the new frame. * @param point location of the reference frame's origin. Not modified. * @param zAxis orientation the reference frame's z-axis. Not modified. * @return the new reference frame. * @throws ReferenceFrameMismatchException if {@code point} and {@code zAxis} are not expressed * in the same reference frame. */ public static ReferenceFrame constructReferenceFrameFromPointAndZAxis(String frameName, FramePoint3D point, FrameVector3D zAxis) { return constructReferenceFrameFromPointAndAxis(frameName, point, Axis.Z, zAxis); }
private void initializeFourBarWithRandomlyRotatedJointFrames(FramePoint3D jointAPosition, FramePoint3D jointBPosition, FramePoint3D jointCPosition, FramePoint3D jointDPosition, FrameVector3D jointAxisA, FrameVector3D jointAxisB, FrameVector3D jointAxisC, FrameVector3D jointAxisD) { ReferenceFrame jointAFrame = GeometryTools.constructReferenceFrameFromPointAndAxis("jointAFrame", jointAPosition, Axis.Z, new FrameVector3D(worldFrame, RandomGeometry.nextVector3D(random, 1.0))); ReferenceFrame jointBFrame = GeometryTools.constructReferenceFrameFromPointAndAxis("jointBFrame", jointBPosition, Axis.Z, new FrameVector3D(worldFrame, RandomGeometry.nextVector3D(random, 1.0))); ReferenceFrame jointCFrame = GeometryTools.constructReferenceFrameFromPointAndAxis("jointCFrame", jointCPosition, Axis.Z, new FrameVector3D(worldFrame, RandomGeometry.nextVector3D(random, 1.0))); ReferenceFrame jointDFrame = GeometryTools.constructReferenceFrameFromPointAndAxis("jointDFrame", jointDPosition, Axis.Z, new FrameVector3D(worldFrame, RandomGeometry.nextVector3D(random, 1.0))); Vector3D jointAxisAFrameA = new Vector3D(); Vector3D jointAxisBFrameB = new Vector3D(); Vector3D jointAxisCFrameC = new Vector3D(); Vector3D jointAxisDFrameD = new Vector3D(); jointAxisA.changeFrame(jointAFrame); jointAxisAFrameA.set(jointAxisA); jointAxisB.changeFrame(jointBFrame); jointAxisBFrameB.set(jointAxisB); jointAxisC.changeFrame(jointCFrame); jointAxisCFrameC.set(jointAxisC); jointAxisD.changeFrame(jointDFrame); jointAxisDFrameD.set(jointAxisD); RigidBodyTransform jointAtoElevator = jointAFrame.getTransformToDesiredFrame(worldFrame); RigidBodyTransform jointBtoA = jointBFrame.getTransformToDesiredFrame(jointAFrame); RigidBodyTransform jointCtoB = jointCFrame.getTransformToDesiredFrame(jointBFrame); RigidBodyTransform jointDtoC = jointDFrame.getTransformToDesiredFrame(jointCFrame); initializeFourBar(jointAtoElevator, jointBtoA, jointCtoB, jointDtoC, jointAxisAFrameA, jointAxisBFrameB, jointAxisCFrameC, jointAxisDFrameD); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout = 30000) public void testConstructFrameFromPointAndAxis() { Random random = new Random(1776L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); FramePoint3D randomPoint = new FramePoint3D(worldFrame); FrameVector3D randomVector = new FrameVector3D(worldFrame); int numberOfTests = 100000; for (int i = 0; i < numberOfTests; i++) { randomPoint.setIncludingFrame(EuclidFrameRandomTools.nextFramePoint3D(random, worldFrame, 10.0, 10.0, 10.0)); randomVector.setIncludingFrame(EuclidFrameRandomTools.nextFrameVector3D(random, worldFrame, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0)); ReferenceFrame frameA = GeometryTools.constructReferenceFrameFromPointAndZAxis("frameA", randomPoint, randomVector); ReferenceFrame frameB = GeometryTools.constructReferenceFrameFromPointAndAxis("frameB", randomPoint, Axis.Z, randomVector); EuclidCoreTestTools.assertRigidBodyTransformEquals(frameA.getTransformToRoot(), frameB.getTransformToRoot(), 1.0e-2); } }
masterJointAxis.changeFrame(masterJointA.getFrameBeforeJoint()); frameBeforeFourBarWithZAlongJointAxis = GeometryTools .constructReferenceFrameFromPointAndAxis(name + "FrameWithZAlongJointAxis", new FramePoint3D(masterJointA.getFrameBeforeJoint()), Axis.Z, masterJointAxis);