@Override public double getYdd() { return linearAcceleration.getY(); }
@Override public double getTheta_y() { return angularRate.getY(); }
@Override public void setAngularVelocity(Vector3D gyroscope, int imuIndex) { r_imu_gyro_x = gyroscope.getX(); r_imu_gyro_y = gyroscope.getY(); r_imu_gyro_z = gyroscope.getZ(); }
@Override public void setCompass(Vector3D compass, int imuIndex) { r_imu_compass_x = compass.getX(); r_imu_compass_y = compass.getY(); r_imu_compass_z = compass.getZ(); }
@Override public void setAcceleration(Vector3D acceleration, int imuIndex) { r_imu_accel_x = acceleration.getX(); r_imu_accel_y = acceleration.getY(); r_imu_accel_z = acceleration.getZ(); }
/** {@inheritDoc} */ @Override protected boolean isInsideEpsilonShapeFrame(double x, double y, double z, double epsilon) { double scaledX = x / (radii.getX() + epsilon); double scaledY = y / (radii.getY() + epsilon); double scaledZ = z / (radii.getZ() + epsilon); return EuclidCoreTools.normSquared(scaledX, scaledY, scaledZ) <= 1.0; }
@Override protected void doScaleInstruction(Graphics3DScaleInstruction graphics3DScale) { Vector3D scale = graphics3DScale.getScaleFactor(); Scale outputScale = new Scale(scale.getX(), scale.getY(), scale.getZ()); Group scaleGroup = new Group(); scaleGroup.getTransforms().add(outputScale); currentGroup.getChildren().add(scaleGroup); currentGroup = scaleGroup; }
@Override protected void doTranslateInstruction(Graphics3DTranslateInstruction graphics3DTranslate) { Vector3D t = graphics3DTranslate.getTranslation(); Translate outputTranslation = new Translate(t.getX(), t.getY(), t.getZ()); Group translationGroup = new Group(); translationGroup.getTransforms().add(outputTranslation); currentGroup.getChildren().add(translationGroup); currentGroup = translationGroup; }
@Test public void testAxisSetterWorks() { double newXValue = random.nextDouble(), newYValue = random.nextDouble(), newZValue = random.nextDouble(); Axis.set(randomVector, Axis.X, newXValue); Axis.set(randomVector, Axis.Y, newYValue); Axis.set(randomVector, Axis.Z, newZValue); assertEquals(newXValue, randomVector.getX(), allowedDelta); assertEquals(newYValue, randomVector.getY(), allowedDelta); assertEquals(newZValue, randomVector.getZ(), allowedDelta); }
private Axis createSDFJointAxis(OneDegreeOfFreedomJoint scsJoint) { Axis sdfJointAxis = new Axis(); Vector3D scsJointAxis = new Vector3D(); scsJoint.getJointAxis(scsJointAxis); String xyz = String.valueOf(scsJointAxis.getX()) + " " + String.valueOf(scsJointAxis.getY()) + " " + String.valueOf(scsJointAxis.getZ()); sdfJointAxis.setXyz(xyz); sdfJointAxis.setDynamics(createJointDynamics(scsJoint)); sdfJointAxis.setLimit(createJointLimit(scsJoint)); return sdfJointAxis; }
public void update(Vector3D vectorSource) { x.update(vectorSource.getX()); y.update(vectorSource.getY()); z.update(vectorSource.getZ()); }
private void testHeightAtRampForAnyRampWithTranslation(Point3D[] pointsOnRamp, RotatableRampTerrainObject ramp, Vector3D translation) { for (int i = 0; i < pointsOnRamp.length; i++) { String message = "Expected Height For point " + (pointsOnRamp[i].getX()+translation.getX()) + " " + (pointsOnRamp[i].getY()+translation.getY()) + " " + pointsOnRamp[i].getZ(); assertEquals(message, pointsOnRamp[i].getZ(), ramp.heightAt(pointsOnRamp[i].getX()+translation.getX(), pointsOnRamp[i].getY()+translation.getY(), pointsOnRamp[i].getZ()), epsilon); } }
@Override protected void doScaleInstruction(Graphics3DScaleInstruction graphics3dObjectScale) { Node scale = new Node(); Vector3D scaleFactor = graphics3dObjectScale.getScaleFactor(); scale.setLocalScale((float) scaleFactor.getX(), (float) scaleFactor.getY(), (float) scaleFactor.getZ()); currentNode.attachChild(scale); currentNode = scale; addChangedScaleListener(scale, graphics3dObjectScale); }
public void fixCameraOnNode(Graphics3DNode node) { checkCameraIsNotNull(); cameraController.setFixPosition(node.getTranslation().getX(), node.getTranslation().getY(), node.getTranslation().getZ()); }
public static void assertSurfaceNormalsMatchAndSnapPreservesXFromAbove(RigidBodyTransform snapTransform, RigidBodyTransform planarRegionTransform) { Vector3D expectedSurfaceNormal = new Vector3D(0.0, 0.0, 1.0); planarRegionTransform.transform(expectedSurfaceNormal); Vector3D actualSurfaceNormal = new Vector3D(0.0, 0.0, 1.0); snapTransform.transform(actualSurfaceNormal); EuclidCoreTestTools.assertTuple3DEquals(expectedSurfaceNormal, actualSurfaceNormal, 1e-7); Vector3D xAxis = new Vector3D(1.0, 0.0, 0.0); snapTransform.transform(xAxis); assertEquals(0.0, xAxis.getY(), 1e-7); }
protected void updatePerfectAngularVelocity() { twistInIMUFrame.setIncludingFrame(twist); twistInIMUFrame.changeFrame(imuFrame); angularVelocity.set(twistInIMUFrame.getAngularPart()); perfGyroX.set(angularVelocity.getX()); perfGyroY.set(angularVelocity.getY()); perfGyroZ.set(angularVelocity.getZ()); }
private void addBlock(double yaw, Vector3D position, Vector3D dimensions, AppearanceDefinition color) { AxisAngle orientation = new AxisAngle(new Vector3D(0.0, 0.0, 1.0), yaw); RigidBodyTransform blockPose = new RigidBodyTransform(orientation, position); Box3D block = new Box3D(blockPose, dimensions.getX(), dimensions.getY(), dimensions.getZ()); terrainObject.addTerrainObject(new RotatableBoxTerrainObject(block, color)); }
@Override protected void computeRotationTranslation(AffineTransform transform3D) { transform3D.setIdentity(); transform3D.setScale(scale * radii.getX(), scale * radii.getY(), scale * radii.getZ()); transform3D.setRotationYawPitchRoll(yawPitchRoll.getYaw().getValue(), yawPitchRoll.getPitch().getValue(), yawPitchRoll.getRoll().getValue()); transform3D.setTranslation(position); }
protected Footstep createFootstepPlacedAtBipedfootLocation(RobotSide side) { ReferenceFrame soleFrame = soleFrames.get(side); Vector3D translation = new Vector3D(); Quaternion rotation = new Quaternion(); soleFrame.getTransformToWorldFrame().getTranslation(translation); soleFrame.getTransformToWorldFrame().getRotation(rotation); FramePose2D solePose2d = new FramePose2D(soleFrame, new Point2D(translation.getX(), translation.getY()), rotation.getYaw()); Footstep foot = createFootstep(side, solePose2d); return foot; }